Permalink
Find file
Fetching contributors…
Cannot retrieve contributors at this time
193 lines (171 sloc) 5.05 KB
#include "WPILib.h"
#include "Spark.h"
class Robot: public IterativeRobot
{
//Construct for objects here
Joystick *driveStick;
Joystick *manipulatorStick;
CANTalon *lDrive1;
CANTalon *lDrive2;
CANTalon *rDrive1;
CANTalon *rDrive2;
DoubleSolenoid *Shifter;
Spark *spark1;
public:
Robot() {
Wait(1);
driveStick = new Joystick (0);
manipulatorStick = new Joystick (1);
lDrive1 = new CANTalon(1);
lDrive2 = new CANTalon(2);
rDrive1 = new CANTalon(3);
rDrive2 = new CANTalon(4);
Shifter = new DoubleSolenoid (0,1);
spark1 = new Spark (0);
//CameraServer::GetInstance()->SetQuality(50);
//table = NetworkTable::GetTable("GRIP/myContoursReport");
}
private:
LiveWindow *lw = LiveWindow::GetInstance();
const std::string autoNameDefault = "Default";
const std::string autoNameCustom = "My Auto";
std::string autoSelected;
double leftX = 0;
double leftY = 0;
double rightX = 0;
double rightY = 0;
double mLeftY = 0;
double mRightY = 0;
double leftTrigger = 0;
double rightTrigger = 0;
double mRightTrigger = 0;
double mLeftTrigger = 0;
bool dUp = false;
bool dRight = false;
bool dDown = false;
bool dLeft = false;
bool dUpR = false;
bool dUpL = false;
bool dDownR = false;
bool dDownL = false;
bool autoShot = false;
bool buttonVal0 = false;
bool buttonVal1 = false;
bool buttonVal2 = false;
bool buttonVal3 = false;
double threshold = 0.09;
void AutonomousInit() // code for auto mode !!!this will run and stop only when disabled!!!
{
// code for auto goes here !!!It will run when auto is enabled on the ds!!!
}
void TeleopInit() // holds code for pre teleop enable
{
}
void TeleopPeriodic() // code for the robot to move is placed here (runs when
// the robot is enabled in teleop mode)
{
//CameraServer::GetInstance()->StartAutomaticCapture("cam0");
//SmartDashboard::PutNumber("DB/Slider 0", accel->GetX());
//SmartDashboard::PutNumber("DB/Slider 1", accel->GetY());
// SmartDashboard::PutNumber("DB/Slider 2", accel->GetZ());
//SmartDashboard::PutNumber("DB/Slider 3", gyro->GetAngle());
//frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
//distance = (sonicIn->GetRaw() * inches);
//sonicOut->Pulse(10);
leftX = driveStick->GetRawAxis(0);
if(fabs(leftX) < threshold)
leftX = 0;
leftY = driveStick->GetRawAxis(-1);
if(fabs(leftY) < threshold)
leftY = 0;
mLeftY = manipulatorStick->GetRawAxis(1);
if(fabs(mLeftY) < threshold)
mLeftY = 0;
rightX = driveStick->GetRawAxis(4);
if(fabs(rightX) < threshold)
rightX = 0;
rightY = driveStick->GetRawAxis(5);
if(fabs(rightY) < threshold)
rightY = 0;
mRightY = manipulatorStick->GetRawAxis(5);
if(fabs(mRightY) < threshold)
mRightY = 0;
leftTrigger = (driveStick->GetRawAxis(2));
if(fabs(leftTrigger) < (threshold))
leftTrigger = 0;
rightTrigger = (driveStick->GetRawAxis(3));
if(fabs(rightTrigger) < (threshold))
rightTrigger = 0;
mRightTrigger = (manipulatorStick->GetRawAxis(3));
if(fabs(mRightTrigger) < (threshold))
mRightTrigger = 0;
mLeftTrigger = (manipulatorStick->GetRawAxis(2));
if(fabs(mLeftTrigger) < (threshold))
mLeftTrigger = 0;
leftX = pow(fabs(leftX), 1.4);
if(driveStick->GetRawAxis(0) > 0) {
leftX = -leftX;
}
/*if(driveStick->GetRawAxis(1) < 0) {
leftY = -leftY;
}*/
leftY = pow(fabs(leftY), 1.4);
if(driveStick->GetRawAxis(1) < 0) {
leftY = -leftY;
}
rightX = pow(fabs(rightX), 1.4);
if(driveStick->GetRawAxis(4) > 0) {
rightX = -rightX;
}
rightY = pow(fabs(rightY), 1.4);
if(driveStick->GetRawAxis(5) > 0) {
rightY = -rightY;
}
mRightY = pow(fabs(mRightY), 1.4);
if(manipulatorStick->GetRawAxis(5) > 0) {
mRightY = -mRightY;
}
mLeftY = pow(fabs(mLeftY), 1.4);
if(manipulatorStick->GetRawAxis(1) > 0) {
mLeftY = -mLeftY;
}
rDrive1->Set(-(leftY - leftX));
rDrive2->Set(-(leftY - leftX));
lDrive1->Set(leftY + leftX);
lDrive2->Set(leftY + leftX);
if(driveStick->GetRawButton(3)) {
Shifter->Set(DoubleSolenoid::Value::kReverse);
driveStick->SetRumble(driveStick->kLeftRumble, 1);
driveStick->SetRumble(driveStick->kRightRumble, 1);
Wait(0.01);
driveStick->SetRumble(driveStick->kLeftRumble, 0);
driveStick->SetRumble(driveStick->kRightRumble, 0);
//rShifter->Set(DoubleSolenoid::Value::kForward);
} else if(driveStick->GetRawButton(2)) {
Shifter->Set(DoubleSolenoid::Value::kForward);
driveStick->SetRumble(driveStick->kLeftRumble, 1);
driveStick->SetRumble(driveStick->kRightRumble, 1);
Wait(0.01);
driveStick->SetRumble(driveStick->kLeftRumble, 0);
driveStick->SetRumble(driveStick->kRightRumble, 0);
//rShifter->Set(DoubleSolenoid::Value::kReverse);
}
// I am untested be careful!!!!!!!
if(driveStick->GetRawButton(4))
{
spark1->Set(1);
}
else
{
spark1->Set(0);
}
Wait(0.005);
}// end TeleopPeriodic
void TestInit()
{
}
void TestPeriodic()
{
}
};
START_ROBOT_CLASS(Robot)