void set_motor_speed1( int speed ) { if (speed>0) mot1.ForwardM1( 0x80, speed ); else mot1.BackwardM1( 0x80, -speed ); }
void set_motor_speed4( int speed ) { if (speed>0) mot2.ForwardM2( 0x80, speed ); else mot2.BackwardM2( 0x80, -speed ); }
//============================================================================== // MotorsDriver::RDrive //============================================================================== void MotorsDriver::LDrive(short sVal) { if (sVal != LDrivePrev) { LDrivePrev = sVal; if (sVal >= 0) RClaw.ForwardM2(PACKETS_ADDRESS, sVal); else RClaw.BackwardM2(PACKETS_ADDRESS, -sVal); } }
void setup_encoders() { uint8_t M1mode = 0x81; uint8_t M2mode = 0x81; uint8_t address= 0x80; bool result = mot.SetM1EncoderMode( address, M1mode ); result = mot.SetM2EncoderMode( address, M2mode ); result = mot.ReadEncoderModes( address, M1mode, M2mode ); printf("Read EncoderModes m1,m2 = %x, %x\n", M1mode, M2mode ); }
/* void* serial_interface(void* mParam) // serial port used for arduino connections & GPS. { LoadCell_SerialInterface* mFoot = (LoadCell_SerialInterface*) mParam; static char app_name[12]; static char read_r[4]; static char no_write_w[4]; static char device_p[4]; static char read_[20]; static char no_write_[20]; static char device_[20]; sprintf(app_name, "./avisual"); sprintf(read_r, "-R"); sprintf(no_write_w, "-w"); sprintf(device_p, "-p"); sprintf(read_, "ascii"); sprintf(no_write_, "0"); //printf("mFoot=%p; left_foot=%p\n", mFoot, &left_foot); if (mFoot == &left_foot) { sprintf(device_, "/dev/ttyACM0"); mFoot->_cl_rx_dump = 1; mFoot->_cl_rx_dump_ascii = 1; mFoot->_cl_port = strdup("/dev/ttyACM0"); mFoot->_cl_tx_bytes = 0; mFoot->left_foot = false; } else { mFoot->_cl_port = strdup("/dev/ttyACM1"); mFoot->_cl_rx_dump = 1; mFoot->_cl_rx_dump_ascii = 1; mFoot->_cl_tx_bytes = 0; mFoot->left_foot = true; //sprintf(device_, "/dev/ttyACM1"); } char * argv[] = { app_name, read_r, read_, no_write_w, no_write_, device_p, device_ }; mFoot->serial_loadcell_main( 7, argv ); return NULL; } */ void read_main_battery() { // mot.SetMainVoltages ( address, 100, 250 ); // mot.SetLogicVoltages( address, 100, 250 ); uint16_t volts1 = mot.ReadMainBatteryVoltage( 0x80 ); float volt = ((float)volts1)/10.; printf("\nMain Battery board #1 = %6.1f volts\n", volt ); volts1 = mot.ReadLogicBatteryVoltage( 0x80 ); volt = ((float)volts1)/10.; printf("\nLogic Battery board #1 = %6.1f volts\n", volts1 ); //mot.SetM1MaxCurrent(0x80, 20.0); //mot.SetM2MaxCurrent(0x80, 20.0); }
void read_encoders() { uint32_t enc1; uint32_t enc2; uint8_t status; bool valid= false; enc1 = mot.ReadEncM1(address, &status, &valid); enc2 = mot.ReadEncM2(address, &status, &valid); printf("Encoders m1,m2= %6.2f, %6.2f \n", 2.0*enc1/2047., 2.0*enc2/2047. ); bool result = mot.ReadEncoders( address, enc1, enc2); printf("Encoders m1,m2= %6.2f, %6.2f \n", 2.0*enc1/2047., 2.0*enc2/2047. ); //result = mot.ReadISpeeds( address, uint32_t &ispeed1,uint32_t &ispeed2); }
int main( int argc, char *argv[] ) { print_args(argc,argv); bool stop=false; init_claws(); time_t t; time(&t); time_stamp = *(localtime(&t)); printf("====================== CLAW =========================\n"); if (argc>1) { if (strcmp(argv[1], "stop")==0) { stop = true; printf("Stop!"); int count = 10; while (count--) { mot.ForwardM1 (address, 0); mot.ForwardM2 (address, 0); mot2.ForwardM1(address, 0); mot2.ForwardM2(address, 0); /* mot3.ForwardM1(address, 0); mot3.ForwardM2(address, 0); mot4.ForwardM1(address, 0); mot4.ForwardM2(address, 0); */ usleep(1000000); // 0.1 sec usleep(1000000); // 0.1 sec } exit(1); } } while (1) { printf("\nForward:\n"); mot.ForwardM1(address, 64); mot.ForwardM2(address, 64); /* mot2.ForwardM1(address, 64); mot2.ForwardM2(address, 64); */ usleep(1000000); // 0.1 sec //usleep(1000000); // 0.1 sec //read_main_battery(); printf("\nBackward:\n"); mot.BackwardM1(address, 64); mot.BackwardM2(address, 64); /* mot2.BackwardM1(address, 64); mot2.BackwardM2(address, 64); */ usleep(1000000); // 0.1 sec //usleep(1000000); // 0.1 sec } }
void init_claws() { if (mot.connected == true) printf("Roboclaw #1 - Available\n" ); else printf("Roboclaw #1 - not available\n"); if (mot2.connected == true) printf("Roboclaw #2 - Available\n" ); else printf("Roboclaw #2 - not available\n"); if (mot3.connected == true) printf("Roboclaw #3 - Available\n" ); else printf("Roboclaw #3 - not available\n"); if (mot4.connected == true) printf("Roboclaw #4 - Available\n" ); else printf("Roboclaw #4 - not available\n"); int maxCurrent = 20; read_main_battery( ); //uint16_t config = 0x0067; //mot.SetConfig ( address, config ); usleep(1000000); // 0.1 sec //config = 0; //bool result = mot.GetConfig( address, config ); //printf("Config=%4x\n", config); bool valid=false; uint16_t error = mot.ReadError(address, &valid); printf("hip ErrorStatus = %4x %d\n\n", error, valid); char text[80]; bool result = mot.ReadVersion( address, text ); printf("\ntext=%s\n", text); //setup_encoders(); //read_encoders (); /* mot.SetM1MaxCurrent(0x80, maxCurrent*100); mot2.SetMainVoltages( address, 100, 250 ); mot3.SetMainVoltages( address, 100, 250 ); mot4.SetMainVoltages( address, 100, 250 ); */ }
void CheckVoltages(void) { #ifdef AVS_PIN uint16_t wVS = analogRead(AVS_PIN); #ifdef DEBUG if (g_fDebugOutput && (wVS != g_wVSPrev)) { Serial.print("VS: "); Serial.println(wVS, DEC); g_wVSPrev = wVS; } #endif if ((wVS < (uint16_t)AVAL_MIN) && g_fServosInit) { // detach any servos we may have... MSound( 3, 100, 2500, 100, 2500, 100, 2500); } #elif defined(PACKET_MODE) // Using Roboclaw in packet mode, so we can ask it for the // motor voltages... Note: This requires us to send and receive // stuff serially, so probably only do this every so often. if ((millis()-g_ulCVTimeLast) > 1000) { // more than a second since we last tested uint16_t wVS = RClaw.ReadMainBatteryVoltage(PACKETS_ADDRESS); #ifdef DEBUG if (g_fDebugOutput && (wVS != g_wVSPrev)) { Serial.print("VS: "); Serial.println(wVS, DEC); g_wVSPrev = wVS; } #endif if ((wVS < (uint16_t)VOLTAGE_MIN1) && g_fServosInit) { // detach any servos we may have... MSound( 3, 100, 2500, 100, 2500, 100, 2500); } g_ulCVTimeLast = millis(); // remember last we did it. } #endif }
//-------------------------------------------------------------------------- // Main: the main function. //-------------------------------------------------------------------------- int main(int argc, char *argv[]) { // Install signal handler to allow us to do some cleanup... struct sigaction sigIntHandler; sigIntHandler.sa_handler = SignalHandler; sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; sigaction(SIGINT, &sigIntHandler, NULL); char abT[40]; // give a nice large buffer. uint8_t cbRead; printf("Start\n"); if (argc > 1) { for (int i=1; i < argc; i++) { printf("%d - %s\n", i, argv[i]); } } char *pszDevice; if (!RClaw.begin(pszDevice = (argc > 1? argv[1] : szRClawDevice), B38400)) { printf("RClaw (%s) Begin failed\n", pszDevice); return 0; } if (!command.begin(pszDevice = (argc > 2? argv[2] : szCommanderDevice), B38400)) { printf("Commander (%s) Begin failed\n", pszDevice); return 0; } int error; delay(250); Serial.begin(/*57600*/); // Try to load the Rover Configuration Data rcd.Load(); g_MotorsDriver.Init(); Serial.println("Kurt's Rover Program Startup\n"); g_fDebugOutput = false; // start with it off! g_fShowDebugPrompt = true; g_fRoverActive = false; g_fRoverActivePrev = false; g_fServosInit = false; g_bGear = 3; // We init in 3rd gear. g_bSteeringMode = ONE_STICK_MODE; // Initialize our pan and tilt servos InitializeServos(); // Make sure the servos are active for(;;) { //-------------------------------------------------------------------------- // Loop: the main arduino main Loop function //-------------------------------------------------------------------------- // We also have a simple debug monitor that allows us to // check things. call it here.. if (TerminalMonitor()) continue; CheckVoltages(); // check voltages - if we get too low shut down the servos... // Lets get the PS2 data... ControlInput(); // Drive the rover if (g_fRoverActive) { if (g_bSteeringMode == TANK_MODE) { sRDrivePWM = LStickY; //RStickY; // BUGBUG - appears like wrong ones doing each... sLDrivePWM = RStickY; // LStickY; } else { // One stick driving if ((RStickY >=0) && (RStickX >= 0)) { // Quadrant 1 sRDrivePWM = RStickY - RStickX; sLDrivePWM = max(RStickX, RStickY); } else if ((RStickY<0) && (RStickX>=0)) { //Quadrant 2 sRDrivePWM = (RStickY + RStickX); sLDrivePWM = min (-RStickX, RStickY); } else if ((RStickY<0) && (RStickX<0)) { //Quadrant 3 sRDrivePWM = min (RStickX, RStickY); sLDrivePWM = (RStickY - RStickX); } else if ((RStickY>=0) && (RStickX<0)) { // Quadrant 4 sRDrivePWM = max(-RStickX, RStickY); sLDrivePWM = (RStickY + RStickX); } else { sRDrivePWM = 0; sLDrivePWM = 0; } } // Lets output the appropriate stuff to the motor controller // ok lets figure out our speeds to output to the two motors. two different commands // depending on going forward or backward. // Scale the two values for the motors. sRDrivePWM = max(min((sRDrivePWM * g_bGear) / 4, 127), -127); // This should keep up in the -127 to +127 range and scale it depending on what gear we are in. sLDrivePWM = max(min((sLDrivePWM * g_bGear) / 4, 127), -127); #ifdef DEBUG if (g_fDebugOutput) { if ((RStickY != RStickYPrev) || (RStickX != RStickXPrev) || (LStickY != LStickYPrev) || (LStickX != LStickXPrev) || (sRDrivePWM != sRDrivePWMPrev) || (sLDrivePWM != sLDrivePWMPrev)) { Serial.print(LStickY, DEC); Serial.print(","); Serial.print(LStickX, DEC); Serial.print(" "); Serial.print(RStickY, DEC); Serial.print(","); Serial.print(RStickX, DEC); Serial.print(" - "); Serial.print(sLDrivePWM, DEC); Serial.print(","); Serial.println(sRDrivePWM, DEC); LStickYPrev = LStickY; LStickXPrev = LStickX; RStickYPrev = RStickY; RStickXPrev = RStickX; sRDrivePWMPrev = sRDrivePWM; sLDrivePWMPrev = sLDrivePWM; } } #endif // Call our motors driver code which may change depending on how we talk to the motors... g_MotorsDriver.RDrive(sRDrivePWM); g_MotorsDriver.LDrive(sLDrivePWM); // Also if we have a pan/tilt lets update that as well.. #ifdef BBB_SERVO_SUPPORT if (g_bSteeringMode != TANK_MODE) { if (LStickX ) { if (command.buttons & BUT_L6) { //modify which thing we are controlling depending on if L6 is down or not. w = max(min(g_wRot + LStickX/8, rcd.aServos[RoverConfigData::ROTATE].wMax), rcd.aServos[RoverConfigData::ROTATE].wMin); if (w != g_wRot) { pinRot.SetDutyUS(w); g_wRot = w; } } else { w = max(min(g_wPan + LStickX/8, rcd.aServos[RoverConfigData::PAN].wMax), rcd.aServos[RoverConfigData::PAN].wMin); if (w != g_wPan) { pinPan.SetDutyUS(w); g_wPan = w; } } } if (LStickY) { w = max(min(g_wTilt + LStickY/8, rcd.aServos[RoverConfigData::TILT].wMax), rcd.aServos[RoverConfigData::TILT].wMin); if (w != g_wTilt) { pinTilt.SetDutyUS(w); g_wTilt = w; } } } #endif delay (10); } else { if (g_fRoverActivePrev) { MSound( 3, 100, 2500, 80, 2250, 60, 2000); g_MotorsDriver.RDrive(0); g_MotorsDriver.LDrive(0); } delay (10); } g_fRoverActivePrev = g_fRoverActive; } }