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); }