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();
}
//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");
}