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 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 initRobot () {
cerr << "running init\n";
Dlf->EnableControl(0);
Dlb->EnableControl(0);
Drf->EnableControl(0);
Drb->EnableControl(0);
arm1->EnableControl();
arm1_sec->EnableControl();
arm2->EnableControl();
Dlf->ConfigEncoderCodesPerRev(250);
Dlf->SetPID(1,0,0);
Dlb->ConfigEncoderCodesPerRev(250);
Dlb->SetPID(1,0,0);
Drf->ConfigEncoderCodesPerRev(250);
Drf->SetPID(1,0,0);
Drb->ConfigEncoderCodesPerRev(250);
Drb->SetPID(1,0,0);
Wait(.1);
if(robotInted==false) {
int count=220;
arm2->Set(-.3);
while(count-->0 && LimitClaw.Get() == 1) Wait(.005);
arm2->Set(.15);
while(count-->0 && LimitClaw.Get() == 0) Wait(.005);
arm2->Set(0);
if(count>0)
EncClaw.Reset();
arm1->Set(-.3);
arm1_sec->Set(-.3);
while(count-->0 && LimitArm.Get() == 1) Wait(.005);
arm1->Set(.5);
arm1_sec->Set(.5);
while(count-->0 && LimitArm.Get() == 0) Wait(.005);
if(count>0)
EncArm.Reset();
arm1->Set(0);
arm1_sec->Set(0);
robotInted = true;
}
}
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 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();
}
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);
}
//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");
}
