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);
}
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);
}
}
//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);
*/
}
void CANJaguarConfiguration :: Configure ( CANJaguar & Jag )
{
Jag.DisableControl ();
Jag.ConfigMaxOutputVoltage ( MaxOutputVoltage );
Jag.SetVoltageRampRate ( VoltageRampRate );
Jag.ConfigFaultTime ( FaultTime );
Jag.ConfigNeutralMode ( NeutralMode );
Jag.ConfigLimitMode ( LimitMode );
Jag.ConfigForwardLimit ( ForwardPositionLimit );
Jag.ConfigForwardLimit ( ReversePositionLimit );
switch ( Feedback )
{
case kFeedbackType_None:
switch ( Mode )
{
case CANSpeedController :: kPercentVbus:
Jag.SetPercentMode ();
break;
case CANSpeedController :: kCurrent:
Jag.SetCurrentMode ( P, I, D );
Jag.Set ( 0 );
Jag.EnableControl ();
break;
case CANSpeedController :: kVoltage:
Jag.SetVoltageMode ();
break;
default:
Jag.SetVoltageMode ( CANJaguar :: Encoder, EncoderCounts );
Jag.ConfigMaxOutputVoltage ( 0.0 );
break;
}
break;
case kFeedbackType_Encoder:
switch ( Mode )
{
case CANSpeedController :: kPercentVbus:
Jag.SetPercentMode ( CANJaguar :: Encoder, EncoderCounts );
break;
case CANSpeedController :: kCurrent:
Jag.SetCurrentMode ( CANJaguar :: Encoder, EncoderCounts, P, I, D );
Jag.Set ( 0 );
Jag.EnableControl ();
break;
case CANSpeedController :: kSpeed:
Jag.SetSpeedMode ( CANJaguar :: Encoder, EncoderCounts, P, I, D );
Jag.Set ( 0 );
Jag.EnableControl ();
break;
case CANSpeedController :: kVoltage:
Jag.SetVoltageMode ( CANJaguar :: Encoder, EncoderCounts );
break;
default:
Jag.SetVoltageMode ( CANJaguar :: Encoder, EncoderCounts );
Jag.ConfigMaxOutputVoltage ( 0.0 );
break;
}
break;
case kFeedbackType_QuadEncoder:
switch ( Mode )
{
case CANSpeedController :: kPercentVbus:
Jag.SetPercentMode ( CANJaguar :: QuadEncoder, EncoderCounts );
break;
case CANSpeedController :: kCurrent:
Jag.SetCurrentMode ( CANJaguar :: QuadEncoder, EncoderCounts, P, I, D );
Jag.Set ( 0 );
Jag.EnableControl ();
break;
case CANSpeedController :: kSpeed:
Jag.SetSpeedMode ( CANJaguar :: QuadEncoder, EncoderCounts, P, I, D );
Jag.Set ( 0 );
Jag.EnableControl ();
break;
case CANSpeedController :: kPosition:
Jag.SetPositionMode ( CANJaguar :: QuadEncoder, EncoderCounts, P, I, D );
Jag.Set ( 0 );
Jag.EnableControl ();
break;
case CANSpeedController :: kVoltage:
Jag.SetVoltageMode ( CANJaguar :: QuadEncoder, EncoderCounts );
Jag.Set ( 0 );
Jag.EnableControl ();
break;
default:
Jag.SetVoltageMode ( CANJaguar :: Encoder, EncoderCounts );
Jag.ConfigMaxOutputVoltage ( 0.0 );
break;
}
break;
case kFeedbackType_Potentiometer:
switch ( Mode )
{
case CANSpeedController :: kPercentVbus:
Jag.SetPercentMode ( CANJaguar :: Potentiometer );
break;
case CANSpeedController :: kCurrent:
Jag.SetCurrentMode ( CANJaguar :: Potentiometer, P, I, D );
Jag.Set ( 0 );
Jag.EnableControl ();
break;
case CANSpeedController :: kPosition:
Jag.SetPositionMode ( CANJaguar :: Potentiometer, P, I, D );
Jag.Set ( 0 );
Jag.EnableControl ();
break;
case CANSpeedController :: kVoltage:
Jag.SetVoltageMode ( CANJaguar :: Potentiometer );
break;
default:
Jag.SetVoltageMode ( CANJaguar :: Encoder, EncoderCounts );
Jag.ConfigMaxOutputVoltage ( 0.0 );
break;
}
break;
}
};