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(); }
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 RunWheels() { // uint32_t t0, t1, t2, t3; // schedule updates to avoid overloading CAN bus or CPU switch (report++) { case 12: // 240 milliseconds report = 0; // reset counter case 0: // Update PID parameters double newP = SmartDashboard::GetNumber("Shooter P"); double newI = SmartDashboard::GetNumber("Shooter I"); double newD = SmartDashboard::GetNumber("Shooter D"); if (newP != kP || newI != kI || newD != kD) { kP = newP; kI = newI; kD = newD; #ifdef HAVE_TOP_WHEEL if (topPID) { #ifdef HAVE_TOP_CAN1 ; // topWheel1->SetPID( kP, kI, kD ); #endif #ifdef HAVE_TOP_CAN2 topWheel2->SetPID( kP, kI, kD ); #endif } #endif #ifdef HAVE_BOTTOM_WHEEL if (bottomPID) { #ifdef HAVE_BOTTOM_CAN1 ; // bottomWheel1->SetPID( kP, kI, kD ); #endif #ifdef HAVE_BOTTOM_CAN2 bottomWheel2->SetPID( kP, kI, kD ); #endif } #endif } break; case 4: // 80 milliseconds #ifdef HAVE_TOP_WHEEL //t0 = GetFPGATime(); // Get top output voltage, current and measured speed #ifdef HAVE_TOP_CAN1 double topI1 = topWheel1->GetOutputCurrent(); #endif #ifdef HAVE_TOP_CAN2 double topI2 = topWheel2->GetOutputCurrent(); topJagSpeed = topWheel2->GetSpeed(); #endif //t1 = GetFPGATime(); topTachSpeed = topTach->PIDGet(); #ifdef HAVE_TOP_CAN1 // stupid floating point! Log(LOG_CURRENT, 1, (uint32_t)(topI1 * 1000 + 0.5)); #endif #ifdef HAVE_TOP_CAN2 Log(LOG_CURRENT, 2, (uint32_t)(topI2 * 1000 + 0.5)); Log(LOG_SPEED, 2, (uint32_t)(topJagSpeed + 0.5)); #endif // Send values to SmartDashboard #ifdef HAVE_TOP_CAN1 SmartDashboard::PutNumber("Top Current 1", topI1); #endif #ifdef HAVE_TOP_CAN2 SmartDashboard::PutNumber("Top Current 2", topI2); SmartDashboard::PutNumber("Top Jag ", topJagSpeed); #endif SmartDashboard::PutNumber("Top Tach ", topTachSpeed); // Get setpoint topSpeed = SmartDashboard::GetNumber("Top Set "); //t2 = GetFPGATime(); if (spinFastNow) { if (topPID) { if (topJagSpeed < topSpeed * vbusThreshold) { topPID = false; // below threshold: switch both motors to full output #ifdef HAVE_TOP_CAN1 jagVbus(topWheel1, maxOutput); Log(LOG_MODE, 1, 1); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(maxOutput); Log(LOG_MODE, 1, 1); #endif #ifdef HAVE_TOP_CAN2 jagVbus(topWheel2, maxOutput); Log(LOG_MODE, 2, 1); #endif } else { ; // above threshold: run motor 1 off, PID on motor 2 #ifdef HAVE_TOP_CAN1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_CAN2 topWheel2->Set(topSpeed); #endif } } else { if (topJagSpeed >= topSpeed * pidThreshold) { ; // above threshold: switch motor 1 off, motor 2 PID topPID = true; #ifdef HAVE_TOP_CAN1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_CAN2 jagPID(topWheel2, topSpeed); Log(LOG_MODE, 2, 2); #endif } else { ; // below threshold: run both motors at full output #ifdef HAVE_TOP_CAN1 topWheel1->Set(maxOutput); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(maxOutput); #endif #ifdef HAVE_TOP_CAN2 topWheel2->Set(maxOutput); #endif } } //t3 = GetFPGATime(); //printf("%10u %10u %10u\n", (uint32_t)(t1 - t0), (uint32_t)(t2 - t1), (uint32_t)(t3 - t2)); } #endif break; case 8: // 160 milliseconds #ifdef HAVE_BOTTOM_WHEEL // Get bottom output voltage, current and measured speed //t0 = GetFPGATime(); #ifdef HAVE_BOTTOM_CAN1 double bottomI1 = bottomWheel1->GetOutputCurrent(); #endif #ifdef HAVE_BOTTOM_CAN2 double bottomI2 = bottomWheel2->GetOutputCurrent(); bottomJagSpeed = bottomWheel2->GetSpeed(); #endif //t1 = GetFPGATime(); bottomTachSpeed = bottomTach->PIDGet(); #ifdef HAVE_BOTTOM_CAN1 Log(LOG_CURRENT, 3, (uint32_t)(bottomI1 * 1000 + 0.5)); #endif #ifdef HAVE_BOTTOM_CAN2 Log(LOG_CURRENT, 4, (uint32_t)(bottomI2 * 1000 + 0.5)); Log(LOG_SPEED, 4, (uint32_t)(bottomJagSpeed + 0.5)); #endif // Send values to SmartDashboard #ifdef HAVE_BOTTOM_CAN1 SmartDashboard::PutNumber("Bottom Current 1", bottomI1); #endif #ifdef HAVE_BOTTOM_CAN2 SmartDashboard::PutNumber("Bottom Current 2", bottomI2); SmartDashboard::PutNumber("Bottom Jag ", bottomJagSpeed); #endif SmartDashboard::PutNumber("Bottom Tach ", bottomTachSpeed); // Get setpoint bottomSpeed = SmartDashboard::GetNumber("Bottom Set "); //t2 = GetFPGATime(); if (spinFastNow) { if (bottomPID) { if (bottomJagSpeed < bottomSpeed * vbusThreshold) { bottomPID = false; // below threshold: switch both motors to full output #ifdef HAVE_BOTTOM_CAN1 jagVbus(bottomWheel1, maxOutput); Log(LOG_MODE, 3, 1); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(maxOutput); Log(LOG_MODE, 3, 1); #endif #ifdef HAVE_BOTTOM_CAN2 jagVbus(bottomWheel2, maxOutput); Log(LOG_MODE, 4, 1); #endif } else { ; // above threshold: run motor 1 off, PID on motor 2 #ifdef HAVE_BOTTOM_CAN1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_CAN2 bottomWheel2->Set(bottomSpeed); #endif } } else { if (bottomJagSpeed >= bottomSpeed * pidThreshold) { // above threshold: switch motor 1 off, motor 2 PID bottomPID = true; #ifdef HAVE_BOTTOM_CAN1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_CAN2 jagPID(bottomWheel2, bottomSpeed); Log(LOG_MODE, 4, 2); #endif } else { ; // below threshold: run both motors at full output #ifdef HAVE_BOTTOM_CAN1 bottomWheel1->Set(maxOutput); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(maxOutput); #endif #ifdef HAVE_BOTTOM_CAN2 bottomWheel2->Set(maxOutput); #endif } } //t3 = GetFPGATime(); //printf("%10u %10u %10u\n", (uint32_t)(t1 - t0), (uint32_t)(t2 - t1), (uint32_t)(t3 - t2)); } #endif 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); }