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 RawControl::checkJag()
{
if(frontRight->GetFaults()!=0)
{
frontRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
frontRight->SetSafetyEnabled(false);
frontRight->ConfigEncoderCodesPerRev(360);
frontRight->EnableControl(0);
}
if(frontLeft->GetFaults()!=0)
{
frontLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
frontLeft->SetSafetyEnabled(false);
frontLeft->ConfigEncoderCodesPerRev(360);
frontLeft->EnableControl(0);
}
if(backLeft->GetFaults()!=0)
{
backLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
backLeft->SetSafetyEnabled(false);
backLeft->ConfigEncoderCodesPerRev(360);
backLeft->EnableControl(0);
}
if(backRight->GetFaults()!=0)
{
backRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
backRight->SetSafetyEnabled(false);
backRight->ConfigEncoderCodesPerRev(360);
backRight->EnableControl(0);
}
if(arm->GetFaults()!=0)
{
arm->SetPID(ARM_P, -.02, 0);
arm->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
arm->SetSafetyEnabled(false);
arm->ConfigMaxOutputVoltage(13);
arm->SetPositionReference(CANJaguar::kPosRef_Potentiometer);
arm->ConfigPotentiometerTurns(1);
arm->EnableControl(0);
}
}
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 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
}
//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);
*/
}
/**
* Robot-wide initialization code should go here.
*
* Use this method for default Robot-wide initialization which will
* be called when the robot is first powered on. It will be called exactly 1 time.
*/
void RobotInit()
{
printf(">>> RobotInit\n");
LogInit();
#ifdef HAVE_COMPRESSOR
compressor = new Compressor(1, 1);
#endif
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
topWheel1 = new CANJaguar(1);
topWheel1->SetSafetyEnabled(false); // motor safety off while configuring
topWheel1->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
topWheel1->ConfigEncoderCodesPerRev( 1 );
#endif
#ifdef HAVE_TOP_PWM1
topWheel1 = new Victor(1);
topWheel1->SetSafetyEnabled(false); // motor safety off while configuring
#endif
#ifdef HAVE_TOP_CAN2
topWheel2 = new CANJaguar(2);
topWheel2->SetSafetyEnabled(false); // motor safety off while configuring
topWheel2->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
topWheel2->ConfigEncoderCodesPerRev( 1 );
#endif
topTach = new Tachometer(2);
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
bottomWheel1 = new CANJaguar(3);
bottomWheel1->SetSafetyEnabled(false); // motor safety off while configuring
bottomWheel1->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
bottomWheel1->ConfigEncoderCodesPerRev( 1 );
#endif
#ifdef HAVE_BOTTOM_PWM1
bottomWheel1 = new Victor(2);
bottomWheel1->SetSafetyEnabled(false); // motor safety off while configuring
#endif
#ifdef HAVE_BOTTOM_CAN2
bottomWheel2 = new CANJaguar(4);
bottomWheel2->SetSafetyEnabled(false); // motor safety off while configuring
bottomWheel2->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
bottomWheel2->ConfigEncoderCodesPerRev( 1 );
#endif
bottomTach = new Tachometer(3);
#endif
#ifdef HAVE_ARM
arm = new DoubleSolenoid(2, 1);
#endif
#ifdef HAVE_INJECTOR
injectorL = new DoubleSolenoid(5, 3);
injectorR = new DoubleSolenoid(6, 4);
#endif
#ifdef HAVE_EJECTOR
ejector = new Solenoid(7);
#endif
#ifdef HAVE_LEGS
legs = new Solenoid(8);
#endif
ds = DriverStation::GetInstance();
eio = &ds->GetEnhancedIO();
gamepad = new Joystick(1);
LiveWindow *lw = LiveWindow::GetInstance();
#ifdef HAVE_COMPRESSOR
lw->AddActuator("K9", "Compressor", compressor);
#endif
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
lw->AddActuator("K9", "Top1", topWheel1);
#endif
#ifdef HAVE_TOP_PWM1
lw->AddActuator("K9", "Top1", topWheel1);
#endif
#ifdef HAVE_TOP_CAN2
lw->AddActuator("K9", "Top2", topWheel2);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
lw->AddActuator("K9", "Bottom1", bottomWheel1);
#endif
#ifdef HAVE_BOTTOM_PWM1
lw->AddActuator("K9", "Bottom1", bottomWheel1);
#endif
#ifdef HAVE_BOTTOM_CAN2
lw->AddActuator("K9", "Bottom2", bottomWheel2);
#endif
#endif
#ifdef HAVE_ARM
lw->AddActuator("K9", "Arm", arm);
#endif
#ifdef HAVE_INJECTOR
lw->AddActuator("K9", "InjectorL", injectorL);
lw->AddActuator("K9", "InjectorR", injectorR);
#endif
#ifdef HAVE_EJECTOR
lw->AddActuator("K9", "Ejector", ejector);
#endif
#ifdef HAVE_LEGS
lw->AddActuator("K9", "Legs", legs);
#endif
SmartDashboard::PutNumber("Shooter P", kP);
SmartDashboard::PutNumber("Shooter I", kI);
SmartDashboard::PutNumber("Shooter D", kD);
spinFastNow = false;
#ifdef HAVE_TOP_WHEEL
SmartDashboard::PutNumber("Top Set ", topSpeed);
#ifdef HAVE_TOP_CAN1
SmartDashboard::PutNumber("Top Current 1", 0.0);
#endif
#ifdef HAVE_TOP_CAN2
SmartDashboard::PutNumber("Top Current 2", 0.0);
SmartDashboard::PutNumber("Top Jag ", 0.0);
#endif
SmartDashboard::PutNumber("Top Tach ", 0.0);
#endif
#ifdef HAVE_BOTTOM_WHEEL
SmartDashboard::PutNumber("Bottom Set ", bottomSpeed);
#ifdef HAVE_BOTTOM_CAN1
SmartDashboard::PutNumber("Bottom Current 1", 0.0);
#endif
#ifdef HAVE_BOTTOM_CAN2
SmartDashboard::PutNumber("Bottom Current 2", 0.0);
SmartDashboard::PutNumber("Bottom Jag ", 0.0);
#endif
SmartDashboard::PutNumber("Bottom Tach ", 0.0);
#endif
SetPeriod(0); //Set update period to sync with robot control packets (20ms nominal)
printf("<<< RobotInit\n");
}
