Basic tank-style drive robot control firmware for Freescale FRDM-K64F. Controls motors on a Dual-Full-H-Bridge with EN, like DBH-1x series, from Bluetooth serial commands
main.cpp
- Committer:
- Mr_What
- Date:
- 2015-07-31
- Revision:
- 1:23d0a615756a
- Parent:
- 0:41ca27337c2b
- Child:
- 2:54d27fdcbe5c
File content as of revision 1:23d0a615756a:
// Firmware to demonstrate typical "Tank-Drive" robot motor control. // one left motor, and one right motor. // // Can do simple commanding from a bluetooth terminal (like Android's BlueTerm), // or from the Android app used in http://www.instructables.com/id/Simple-RC-car-for-beginners-Android-control-over-/ // // The FreeScale FRDM-K64F is a much more powerful board than needed for this task, // but it is a good template for a typical "tank-drive" robot based on the FRDM-K64F. // // The DBH-1x motor driver has very similar inputs to the very common L298N // dual-H-bridge driver chip. One main difference is that it warns that // the drive is to be used at no more than 98% PWM. In order // to meet this extra requirement over common L298 motor driver logic, // the direction indicator inputs are PWM at 98% instead of logic "1" // // Aaron Birenboim, http://boim.com 31jul2015 // Apache license #include "mbed.h" //DigitalOut gpo(D0); //DigitalOut led(LED_RED); //PwmOut ENA( PTD1); // D13 on Arduino Shield //PwmOut IN1A(PTD3); // D12 on Arduino Shield //PwmOut IN2A(PTD2); // D11 on Arduino Shield //PwmOut ENB( PTD0); // D13 on Arduino Shield //PwmOut IN1B(PTC4); // D12 on Arduino Shield //PwmOut IN2B(PTA0); // D11 on Arduino Shield Timer Time; inline int millis() {return(Time.read_ms());} // mimic Arduino millis() function // Tried to inherit/polymorph serial capabilities... but could // not get to compile... or get access to mbed::stream capabilities. // I know this is sloppy... but I'm just going to make a global // Serial, and let otherclasses have a reference to it. #include "Serial.h" //Serial CmdSerial(PTC17,PTC16); // Command/Diagnostic serial port on UART3, sicne I don't know how to use USB ports (yet) Serial CmdSerial(PTC15,PTC14); // Command/Diagnostic serial port on "bluetooth add-on" header #include "ASerial.h" // emulation of some common Arduino Serial methods ASerial cSerial(CmdSerial); // Set up motor drive for left and right motors #define DBH1 // use DBH-1x modifications to typicsl L298 drive logic #include "MotorDrive298.h" MotorDrive MotL(PTD1,PTD3,PTD2,PTB2); MotorDrive MotR(PTD0,PTC4,PTA0,PTB3); #include "Command.h" // ------------------------------------------------------------------------------ void initMotorDrive(MotorDrive &md) { md.setCommandTimeout(15000); // ms between commands before automatic emergency stop //ms.setPWMfreqHz(8000); // these should be the defaults //md.setStartupTime(5); // full power pulse this long when starting from full STOP //md.setStopDeadTime(3000); // wait this many ms after emergency STOP before starting up again //md.setMinPWM(0.004f); // any PWM command below this istreated as 0 //md.setMaxPWM(0.98f); // these drives can fail if attempt to run full-100% md.setDecelRate(500); // deceleration rate on STOP. This frac/ms } // Since this board has fancy tri-color LED, let's sequence it // instead of a boring old flash for a heartbeat DigitalOut ledR(LED_RED); DigitalOut ledG(LED_GREEN); DigitalOut ledB(LED_BLUE); void toggleFlash() { static int k=0; k++; if ((k<0) || (k>7)) k=0; // Gray code counter... switch(k) { case 1: case 5: ledG = !ledG; break; case 3: case 7: ledB = !ledB; break; default: ledR = !ledR; break; } } void reportCurrent() { float cr, cl; cr = MotR.getCurrent(); cl = MotL.getCurrent(); CmdSerial.printf("\r\nCurrent: left=%.3f right=%.3f\r\n",cl,cr); } //void dumpSerialChar() //{ // int i = cSerial.getc(); // CmdSerial.printf("%d %c\n",i,i); //} // ================================================== main int prevCommandTime=0; #define FLASH_DT 800 int tFlash = 0; // for diagnostics, just print a few messages, then be quiet to improve // performance when in actual use. int nMsg = 9; int main() { CmdSerial.baud(57600); CmdSerial.puts("\r\nTankDrive for K64F with Bluetooth\r\n\n"); // Set motor drive parameters initMotorDrive(MotL); initMotorDrive(MotR); Time.reset(); Time.start(); CommandReader cmd(cSerial); //int detailMsg=9; while (true) { int t = Time.read_ms(); //if(--detailMsg>0)CmdSerial.printf("%d\r\n",t); char code; int val; int stat = cmd.get(code,val); if (stat) { prevCommandTime = t; if (nMsg>0){nMsg--;CmdSerial.printf(">%c%d\r\n",code,val);} switch(code) { case 'L': MotL.setSpeed(val/255.0,t); break; case 'R': MotR.setSpeed(val/255.0,t); break; default : CmdSerial.printf("Unidentified command \"%c%d\" (stop)",code,val); MotL.stop(); MotR.stop(); } //CmdSerial.puts("\nrcd\r"); //CmdSerial.puts("\r\n"); //detailMsg=2; //CmdSerial.putc('\n'); } else { // no command, do housekeeping (misc state update stuff) MotL.update(t); MotR.update(t); if ((prevCommandTime > 0x0fffff00) && (t < 999)) { // time counter is close to wrapping around. make sure this does not happen. // I think we can tolerate a minor glitch once every 24.8 days of continuous use prevCommandTime = tFlash = 0; Time.reset(); Time.start(); CmdSerial.puts("\r\nClock wrap-around\r\n"); while(Time.read() < 1); t = 1; } if (t - tFlash > FLASH_DT) { // Flash standard LED to show things are running tFlash = t; CmdSerial.printf("dt=%d\r\n",t); toggleFlash(); //reportCurrent(); //wait(0.8f); } } } }