void RobotInit(void) {
// Actions which would be performed once (and only once) upon initialization of the
// robot would be put here.
// Test to see if Dashboard is connected---------------------------------------------------------
// printf("HUE MIN: %d\n", SmartDashboard::GetNumber("HUE MIN"));
// Initialize exponent value from SmartDashboard
exp = 3.0;
buttonWasDown = false;
// Initialize settings for encoder drive PIDControllers
// Drive_PID_Controller->SetOutputRange(-0.2, 0.2);
// Drive_PID_Controller->SetTolerance(0.1);
// Initialize settings for RobotTurn
// Goal_Align_PID->SetOutputRange(-0.2, 0.2);
// Goal_Align_PID->SetTolerance(0.1);
// Set encoders
Front_R->ConfigEncoderCodesPerRev(360);
Front_L->ConfigEncoderCodesPerRev(360);
// Set each drive motor to have an encoder to be its friend
Front_R->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
Front_L->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
printf("RobotInit() completed.\n");
}
void Disabled (void) {
debug("disable");
Dlf->StopMotor();
Dlb->StopMotor();
Drf->StopMotor();
Drb->StopMotor();
}
void Drive (float speed, float turn, float strafe) {
Dlf->Set(range(speed + turn + strafe)*250, 2);
Dlb->Set(range(speed + turn - strafe)*250, 2);
Drf->Set(range(-speed + turn + strafe)*250, 2);
Drb->Set(range(-speed + turn - strafe)*250, 2);
//CANJaguar::UpdateSyncGroup(2);
}
void DoctaEight::leftyrighty(double left, double right)//set drive motors on either side
{
REDRUM;
righty.Set(negate*right*.9);
rightyB.Set(negate*right*.9);
lefty.Set(negate*left*.9);
leftyB.Set(negate*left*.9);
}
void setMotors(float left,float right)
{
left=-left;
right=-right;
left*=300;
right*=300;
frontLeft->Set(-left);
frontRight->Set(right);
backRight->Set(right);
backLeft->Set(-left);
//CANJaguar::UpdateSyncGroup(2);
}
CANRobotDemo()
: speedJag(2, CANJaguar::kSpeed)
, stick(1)
, axis(Joystick::kXAxis)
, commandTask("DashboardCommandServer", (FUNCPTR)DashboardCommandServerStub)
{
GetWatchdog().SetExpiration(100);
speedJag.ConfigEncoderCodesPerRev(360);
speedJag.ConfigMaxOutputVoltage(6.0);
speedJag.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
wpi_stackTraceEnable(true);
commandTask.Start((INT32)this);
}
void UpdateDashboardStatus()
{
Dashboard &dashHigh = m_ds->GetHighPriorityDashboardPacker();
dashHigh.AddCluster(); // PID (not used for now)
dashHigh.AddDouble(0.0); // P
dashHigh.AddDouble(0.0); // I
dashHigh.AddDouble(0.0); // D
dashHigh.FinalizeCluster();
dashHigh.AddDouble(speedJag.GetSpeed()); // Current position
dashHigh.AddDouble(speedJag.Get()); // Setpoint
dashHigh.AddDouble(speedJag.GetOutputVoltage()); // Output Voltage
dashHigh.Finalize();
}
void OperatorControl(void) {
while(!IsDisabled()) {
GetWatchdog().Feed();
float speed = stick.GetRawAxis(2);
float strafe = -1*stick.GetRawAxis(1);
float turn = -1*stick.GetRawAxis(3);
Dlf->Set(speed + turn + strafe);
Dlb->Set(speed + turn - strafe);
Drf->Set(-speed + turn + strafe);
Drb->Set(-speed + turn - strafe);
Wait(.05);
}
}
void TeleopInit(void) {
m_telePeriodicLoops = 0; // Reset the loop counter for teleop mode
m_dsPacketsReceivedInCurrentSecond = 0; // Reset the number of dsPackets in current second
// Default autoPilot to off
autoPilot = false;
Front_R->EnableControl();
Front_L->EnableControl();
// Enable Goal Align PID
// Goal_Align_PID->Disable(); // Stop previous enables
// Goal_Align_PID->Enable();
// Goal_Align_PID->SetSetpoint(0.0);
}
void RawControl::armAngle(float angle) {
//angle/=300.0f;
arm->Set(angle);
//cout<<"angle= "<<angle<<"\n";
}
void AutonomousInit() {
init();
currentDistance = 0;
goalDistance = 6.0*12.0; // 6 feet
autonomousShooterDelay = 0;
shooterMotor->Set(0);
loaderMotor->Set(Relay::kOff);
}
void TeleopPeriodic(void) {
// increment the number of teleop periodic loops completed
m_telePeriodicLoops++;
GetWatchdog().Feed();
// if(autoPilot == true)
// {
// Auto Align Disable Button
// if(operatorGamepad->GetButton(Joystick::kTopButton) == 2)
// {
// Goal_Align_PID->Disable(); // Stop outputs
// Goal_Align_PID->Enable(); // Start PIDContoller up again
// Goal_Align_PID->SetSetpoint(0.0);
// autoPilot = false;
// }
// }
// else
// {
// Calculate jaguar output based on exponent we pass from SmartDashboard
double leftOutput, rightOutput;
leftOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(2));
rightOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(5));
m_robotDrive->SetLeftRightMotorOutputs(leftOutput, rightOutput);
if(operatorGamepad->GetRawButton(1) && !buttonWasDown)
{
printf("LEFT ENCODER: %f\n", Front_L->GetPosition());
printf("RIGHT ENCODER: %f\n", Front_R->GetPosition());
}
buttonWasDown = operatorGamepad->GetRawButton(1);
// Auto Align Button
// if(operatorGamepad->GetButton(Joystick::kTopButton) == 1)
// {
// // Turn Auto Align on if we see a goal and we know the azimuth
// if(SmartDashboard::GetBoolean(FOUND_KEY) == true)
// {
// Goal_Align_PID->SetSetpoint(SmartDashboard::GetNumber(AZIMUTH_KEY));
// autoPilot = true;
// }
// }
// }
}
void StartWheels()
{
if (!spinFastNow) {
printf(">>> StartWheels\n");
Log(LOG_START, 0, 0);
spinFastNow = true;
// start shooter wheels in %vbus mode, max output
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
jagVbus(topWheel1, maxOutput);
Log(LOG_MODE, 1, 1);
#endif
#ifdef HAVE_TOP_PWM1
topWheel1->Set(maxOutput);
Log(LOG_MODE, 1, 1);
#endif
#ifdef HAVE_TOP_CAN2
jagVbus(topWheel2, maxOutput);
Log(LOG_MODE, 2, 1);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
jagVbus(bottomWheel1, maxOutput);
Log(LOG_MODE, 3, 1);
#endif
#ifdef HAVE_BOTTOM_PWM1
bottomWheel1->Set(maxOutput);
Log(LOG_MODE, 3, 1);
#endif
#ifdef HAVE_BOTTOM_CAN2
jagVbus(bottomWheel2, maxOutput);
Log(LOG_MODE, 4, 1);
#endif
#endif
topPID = bottomPID = false;
// reset reporting counter
report = 0;
printf("<<< StartWheels\n");
}
}
/**
* Initialization code for test mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters test mode.
*/
void TestInit()
{
printf(">>> TestInit\n");
#ifdef HAVE_COMPRESSOR
compressor->Start();
#endif
#ifdef HAVE_ARM
arm->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_INJECTOR
injectorL->Set(DoubleSolenoid::kOff);
injectorR->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_EJECTOR
ejector->Set(false);
#endif
#ifdef HAVE_LEGS
legs->Set(false);
#endif
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
jagVbus(topWheel1, 0.0);
#endif
#ifdef HAVE_TOP_PWM1
topWheel1->Set(0.0);
#endif
#ifdef HAVE_TOP_CAN2
jagVbus(topWheel2, 0.0);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
jagVbus(bottomWheel1, 0.0);
#endif
#ifdef HAVE_BOTTOM_PWM1
bottomWheel1->Set(0.0);
#endif
#ifdef HAVE_BOTTOM_CAN2
jagVbus(bottomWheel2, 0.0);
#endif
#endif
printf("<<< TestInit\n");
}
void TeleopPeriodic()
{
SmartDashboard::PutNumber("joystickX",stick.GetX());
SmartDashboard::PutNumber("joystickY",stick.GetY());
//SmartDashboard::PutBoolean("f*****g buttons", stick.GetRawButton(1));
//SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage());
SmartDashboard::PutBoolean("infra",infra.Get());
SmartDashboard::PutNumber("accelX",accel.GetX());
SmartDashboard::PutNumber("accelY",accel.GetY());
SmartDashboard::PutNumber("accelZ",accel.GetZ());
servo.Set(
trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick
);
SmartDashboard::PutNumber("servo", servo.Get());
jag1.Set(stick.GetY());
jag2.Set(stick.GetY());
//tal1.Set(stick.GetY());
SmartDashboard::PutNumber("jag1", jag1.Get());
SmartDashboard::PutNumber("jag2", jag2.Get());
/*SmartDashboard::PutNumber("encpos", enc.Get());
SmartDashboard::PutNumber("encspd", enc.GetRate());*/
if (stick.GetRawButton(1) && !actuatePressed) {
pistonVal=!pistonVal;
piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse);
actuatePressed = true;
}
else if (!stick.GetRawButton(1))
actuatePressed = false;
SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward);
}
void Claw(double joy) {
if(joy < 10) joy = 10;
if(joy > 230) joy = 230;
int location = EncClaw.Get();
double speed = PIDClaw(joy, location);
//if(location < 15) speed *= .2;
if(speed > .32) speed = .32;
if(speed < -.32) speed = -.32;
if(speed < .1 && speed > -.1) speed = 0;
arm2->Set(speed,2);
}
void StopWheels()
{
if (spinFastNow) {
printf(">>> StopWheels\n");
Log(LOG_STOP, 0, 0);
spinFastNow = false;
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
jagStop(topWheel1);
Log(LOG_MODE, 1, 0);
#endif
#ifdef HAVE_TOP_PWM1
topWheel1->Disable();
Log(LOG_MODE, 1, 0);
#endif
#ifdef HAVE_TOP_CAN2
jagStop(topWheel2);
Log(LOG_MODE, 2, 0);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
jagStop(bottomWheel1);
Log(LOG_MODE, 3, 0);
#endif
#ifdef HAVE_BOTTOM_PWM1
bottomWheel1->Disable();
Log(LOG_MODE, 3, 0);
#endif
#ifdef HAVE_BOTTOM_CAN2
jagStop(bottomWheel2);
Log(LOG_MODE, 4, 0);
#endif
#endif
topPID = bottomPID = false;
printf("<<< StopWheels\n");
}
}
/**
* Run the closed loop position controller on the Jag.
*/
void OperatorControl()
{
printf("In OperatorControl\n");
GetWatchdog().SetEnabled(true);
while (IsOperatorControl() && !IsDisabled())
{
GetWatchdog().Feed();
// Set the desired setpoint
speedJag.Set(stick.GetAxis(axis) * 150.0);
UpdateDashboardStatus();
Wait(0.05);
}
}
void MecanumDrive::InitMotor( CANJaguar& motor )
{
motor.ChangeControlMode( m_currControlMode );
if ( m_currControlMode == CANJaguar::kSpeed )
{
motor.ConfigEncoderCodesPerRev(360);
motor.ConfigMaxOutputVoltage(12.0);
motor.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
motor.SetPID(.7,.004,0);
motor.SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
}
motor.EnableControl();
}
void MecanumDrive::CheckForRestartedMotor( CANJaguar& motor, const char * strDescription )
{
if ( m_currControlMode != CANJaguar::kSpeed ) // kSpeed is the default
{
if ( motor.GetPowerCycled() )
{
Wait(0.10); // Wait 100 ms
InitMotor( motor );
char error[256];
sprintf(error, "\n\n>>>>%s %s", strDescription, "Jaguar Power Cycled - re-initializing");
printf(error);
setErrorData(error, strlen(error), 100);
}
}
}
void Arm(double joy) {
int location = EncArm.Get();
/*
if(location < 10 && joy < 0) joy = 0;
if(location > 110 && joy > 0) joy = 0;
arm1->Set(joy);
arm1_sec->Set(joy);
return;
*/
if(joy < -10) joy = -10;
if(joy > 110) joy = 110;
double speed = PIDArm(joy, location);
if(speed > .5) speed = .5;
if(speed < -.3) speed = -.3;
if(speed < 0 && location < 10) speed = 0;
if(speed > 0 && location > 110) speed = 0;
speed = LowArm(speed);
if(speed < .01 && speed > -.01) speed = 0;
arm1->Set(speed,3);
arm1_sec->Set(speed,3);
}
void RawControl::resetArm()
{
arm->SetPID(-5, 0, 0);
arm->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
arm->SetSafetyEnabled(false);
arm->SetPositionReference(CANJaguar::kPosRef_Potentiometer);
arm->ConfigEncoderCodesPerRev(1);
arm->EnableControl(0);
}
void AutonomousPeriodic() {
currentDistance = leftDriveEncoder->GetDistance();
shooterMotor->Set(-shooterMotorVolts);
if (currentDistance >= goalDistance){
drive->setLeft(0);
drive->setRight(0);
loaderMotor->Set(Relay::kForward);
} else {
drive->setLeft(.49);
drive->setRight(.5);
}
//log->info("LRD %f %f",
// leftDriveEncoder->GetDistance(),
// rightDriveEncoder->GetDistance());
//log->print();
}
void output (void)
{
REDRUM;
if (IsAutonomous())
driverOut->PrintfLine(DriverStationLCD::kUser_Line1, "blaarag");
else if (IsOperatorControl())
{
REDRUM;
}
outputCounter++;
if (outputCounter % 30 == 0){
REDRUM;
driverOut->PrintfLine(DriverStationLCD::kUser_Line2, "Top Shooter RPM: %f",(float)LTop.GetSpeed());
driverOut->PrintfLine(DriverStationLCD::kUser_Line3, "Bot Shooter RPM: %f",(float)LBot.GetSpeed());
// driverOut->PrintfLine(DriverStationLCD::kUser_Line6, "Pilot Z-Axis: %f",pilot.GetZ());
}
if (outputCounter % 60 == 0){
REDRUM;
driverOut->PrintfLine(DriverStationLCD::kUser_Line4, "Top CANJag Temp: %f Celcius",LTop.GetTemperature()*(9/5) + 32);
driverOut->PrintfLine(DriverStationLCD::kUser_Line5, "Bot CANJag Temp: %f Celcius",LBot.GetTemperature()*(9/5) + 32);
outputCounter = 1;
}
driverOut->UpdateLCD();
}//nom nom nom
void RawControl::resetJags()
{
{
frontRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
frontRight->SetSafetyEnabled(false);
frontRight->ConfigEncoderCodesPerRev(360);
frontRight->EnableControl(0);
}
{
frontLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
frontLeft->SetSafetyEnabled(false);
frontLeft->ConfigEncoderCodesPerRev(360);
frontLeft->EnableControl(0);
}
{
backLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
backLeft->SetSafetyEnabled(false);
backLeft->ConfigEncoderCodesPerRev(360);
backLeft->EnableControl(0);
}
{
backRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
backRight->SetSafetyEnabled(false);
backRight->ConfigEncoderCodesPerRev(360);
backRight->EnableControl(0);
}
frontLeft->ChangeControlMode(CANJaguar::kPercentVbus);
frontRight->ChangeControlMode(CANJaguar::kPercentVbus);
backLeft->ChangeControlMode(CANJaguar::kPercentVbus);
backRight->ChangeControlMode(CANJaguar::kPercentVbus);
frontLeft->ConfigFaultTime(0);
backLeft->ConfigFaultTime(0);
backRight->ConfigFaultTime(0);
frontRight->ConfigFaultTime(0);
frontLeft->EnableControl(0);
frontRight->EnableControl(0);
backLeft->EnableControl(0);
backRight->EnableControl(0);
}
void DoctaEight::OperatorControl(void)
{
REDRUM;
LTop.SetSafetyEnabled(0);
LTop.EnableControl();
LBot.SetSafetyEnabled(0);
LBot.EnableControl();
while (IsOperatorControl())
{
REDRUM;
output();
if (pilot.GetRawButton(6))
{
arm.Set (.3);
}
else if (pilot.GetRawButton(5))
{
arm.Set (-.3);
}
else
arm.Set (0);
//Set Shooter state and reset RPMoffset if necessary
if (copilot.GetRawButton(1)) //BUTTON A
{
REDRUM;
if(ShooterState != 1)
{
REDRUM;
ShooterState = 1;
RPMoffset = 0;
}
}
else if (copilot.GetRawButton(2)) //BUTTON B
{
REDRUM;
if(ShooterState != 2)
{
REDRUM;
ShooterState = 2;
RPMoffset = 0;
}
}
else if (copilot.GetRawButton(3)) //BUTTON X
{
REDRUM;
if(ShooterState != 3)
{
REDRUM;
ShooterState = 3;
RPMoffset = 0;
}
}
else if (copilot.GetRawButton(4)) //BUTTON Y
{
REDRUM;
if(ShooterState != 4)
{
REDRUM;
ShooterState = 4;
RPMoffset = 0;
}
}
//increment or decrement RPMoffset
if(copilot.GetRawButton(5)) //BUTTON LB
{
REDRUM;
FlagB5 = true;
}
else if(copilot.GetRawButton(6)) //BUTTON RB
{
REDRUM;
FlagB6 = true;
}
if(FlagB5 == true && copilot.GetRawButton(5) == false)
{
REDRUM;
RPMoffset -= 50;
FlagB5 = false;
}
else if(FlagB6 && !copilot.GetRawButton(6))
{
REDRUM;
RPMoffset += 50;
FlagB6 = false;
}
if (pilot.GetRawButton(1) && !cycle)
{
cycle = 1;
negate*=-1;
}
else
{
cycle=0;
}
//prepare shooter/launcher ouput speed
if(ShooterState == 1) {
REDRUM; //BUTTON A
LTopSpeed = MAX/12;
LBotSpeed = RPMMid + RPMoffset;
}
else if(ShooterState == 2) { //BUTTON B
REDRUM;
LTopSpeed = MAX/12;
LBotSpeed = RPMLow + RPMoffset;
}
else if(ShooterState == 3) { //BUTTON X
REDRUM;
LTopSpeed = MAX/12;
LBotSpeed = RPMHigh + RPMoffset;
}
else if(ShooterState == 4) { //BUTTON Y
REDRUM;
LTopSpeed = 0;
LBotSpeed = 0;
}
//set shooter launcher speed to CANJags
LTop.Set(LTopSpeed);
LBot.Set(-LBotSpeed);
//move simple platform arm
leftyrighty(-pilot.GetY(), -pilot.GetRawAxis(4));
intake.Set(-copilot.GetY());
}
//stops encoders
}
float setSpeed(float newMagnitude)
{
tarTheta = atan2(yVector, xVector);
curTheta = -(posEncoder->GetVoltage() - FLOFFSET ) / 5 * 2 * PI;
// Code Snippet
diffTheta = tarTheta - curTheta;
if (diffTheta > PI)
{
diffTheta -= 2*PI;
}
else if (diffTheta < -PI)
{
diffTheta += 2*PI;
}
if (diffTheta > PI/2)
{
diffTheta -= PI;
mag = mag * -1;
}
else if (diffTheta < -PI/2)
{
diffTheta += PI;
mag = mag * -1;
}
turnVel = diffTheta / (PI/2);
if (0 < turnVel && turnVel < .25)
{
turnVel = .25;
}
if (0 > turnVel && turnVel > -.25)
{
turnVel = -.25;
}
if (fabs(diffTheta) < PI/45 )
{
turnVel = 0;
}
if (((turnVel > 0 && prevTurnVel < 0)
|| (turnVel < 0&& prevTurnVel> 0))
&& !changeSign)
{
changeSign = true;
// moveTime = baneTimer.Get() + .1; **FIX BANETIMER
}
if (changeSign)
{
turnVel = 0;
// if (moveTime < baneTimer.Get())
{
changeSign = false;
}
}
// /Code Snippet
if (!(xVector == 0 && yVector == 0))
{
turnWheel->Set(turnVel);
moveWheel->Set(mag);
}
else
{
turnWheel->Set(0);
moveWheel->Set(0);
}
}
//CHECK THIS OUT!!
void RawControl::configJags() {
//will need to be tuned on the new robot
frontLeft->SetPID(3, .07, 0);//tested values are 1,.02,0
frontRight->SetPID(3, .09, 0);
backLeft->SetPID(1, .013, 0);
backRight->SetPID(1.2, .013, 0);
arm->SetPID(ARM_P, -.02, 0);
backLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
frontLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
frontRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
backRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
arm->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
//shoot everything remotely safety related
backLeft->SetSafetyEnabled(false);
backRight->SetSafetyEnabled(false);
frontLeft->SetSafetyEnabled(false);
frontRight->SetSafetyEnabled(false);
arm->SetSafetyEnabled(false);
frontLeft->ConfigMaxOutputVoltage(13);
frontRight->ConfigMaxOutputVoltage(13);
backLeft->ConfigMaxOutputVoltage(13);
backRight->ConfigMaxOutputVoltage(13);
arm->ConfigMaxOutputVoltage(13);
frontLeft->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
frontRight->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
backRight->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
backLeft->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
arm->SetPositionReference(CANJaguar::kPosRef_Potentiometer);
//not sure on these values either
frontLeft->ConfigEncoderCodesPerRev(360);
frontRight->ConfigEncoderCodesPerRev(360);
backLeft->ConfigEncoderCodesPerRev(360);
backRight->ConfigEncoderCodesPerRev(360);
arm->ConfigPotentiometerTurns(1);
frontLeft->ChangeControlMode(CANJaguar::kPercentVbus);
frontRight->ChangeControlMode(CANJaguar::kPercentVbus);
backLeft->ChangeControlMode(CANJaguar::kPercentVbus);
backRight->ChangeControlMode(CANJaguar::kPercentVbus);
frontLeft->EnableControl(0);
frontRight->EnableControl(0);
backLeft->EnableControl(0);
backRight->EnableControl(0);
frontLeft->ConfigFaultTime(0);
backLeft->ConfigFaultTime(0);
backRight->ConfigFaultTime(0);
frontRight->ConfigFaultTime(0);
arm->ConfigFaultTime(0);
arm->EnableControl(0);
arm->EnableControl(0);
/*
fl=new CANJaguar(2,CANJaguar::kSpeed);
fr=new CANJaguar(3,CANJaguar::kSpeed);
bl=new CANJaguar(4,CANJaguar::kSpeed);
br=new CANJaguar(1,CANJaguar::kSpeed);
fl->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
fr->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
br->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
bl->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
fl->ConfigEncoderCodesPerRev(1440);
fr->ConfigEncoderCodesPerRev(1440);
bl->ConfigEncoderCodesPerRev(1440);
br->ConfigEncoderCodesPerRev(1440);
*/
}
int RawControl::zero() {
arm->EnableControl(0);
return 0;
}
float RawControl::getArmPos()
{
return arm->GetPosition();
}