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::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 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 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 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 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
}
int RawControl::zero() {
arm->EnableControl(0);
return 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;
}
};
// Shamelessly stolen from PCVideoServer
int DashboardCommandServer()
{
/* Setup to PC sockets */
struct sockaddr_in serverAddr;
int sockAddrSize = sizeof(serverAddr);
int pcSock = ERROR;
bzero ((char *) &serverAddr, sockAddrSize);
serverAddr.sin_len = (u_char) sockAddrSize;
serverAddr.sin_family = AF_INET;
serverAddr.sin_port = htons (kDashboardCommandPort);
serverAddr.sin_addr.s_addr = htonl (INADDR_ANY);
while (true)
{
taskSafe();
// Create the socket.
if ((pcSock = socket (AF_INET, SOCK_STREAM, 0)) == ERROR)
{
perror ("socket");
continue;
}
// Set the TCP socket so that it can be reused if it is in the wait state.
int reuseAddr = 1;
setsockopt(pcSock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast<char*>(&reuseAddr), sizeof(reuseAddr));
// Bind socket to local address.
if (bind (pcSock, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR)
{
perror ("bind");
close (pcSock);
continue;
}
// Create queue for client connection requests.
if (listen (pcSock, 1) == ERROR)
{
perror ("listen");
close (pcSock);
continue;
}
struct sockaddr_in clientAddr;
int clientAddrSize;
int newPCSock = accept (pcSock, reinterpret_cast<sockaddr*>(&clientAddr), &clientAddrSize);
if (newPCSock == ERROR)
{
close(pcSock);
continue;
}
char cmdBuffer[32];
char *pBuffer;
while(1)
{
int numBytes = 0;
pBuffer = cmdBuffer;
while (numBytes < 2 || (*(pBuffer-2) != '\r' && *(pBuffer-1) != '\n'))
{
numBytes += read(newPCSock, pBuffer++, 1);
}
char command = cmdBuffer[0];
switch (command)
{
case 'E':
speedJag.EnableControl();
//printf("Enable\n");
break;
case 'D':
speedJag.DisableControl();
//printf("Disable\n");
break;
case 'G':
{
double P, I, D;
memcpy((char*)&P, cmdBuffer+1, sizeof(double));
memcpy((char*)&I, cmdBuffer+9, sizeof(double));
memcpy((char*)&D, cmdBuffer+17, sizeof(double));
speedJag.SetPID(P, I, D);
//printf("Set- P: %f I: %f D: %f\n", P, I, D);
//P = speedJag.GetP();
//I = speedJag.GetI();
//D = speedJag.GetD();
//printf("Get- P: %f I: %f D: %f\n", P, I, D);
}
break;
}
//no point in running too fast -
Wait(0.01);
}
// Clean up
close (newPCSock);
newPCSock = ERROR;
close (pcSock);
pcSock = ERROR;
taskUnsafe();
Wait(0.1);
}
return (OK);
}
