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
Diff: main.cpp
- Revision:
- 1:23d0a615756a
- Parent:
- 0:41ca27337c2b
- Child:
- 2:54d27fdcbe5c
--- a/main.cpp Tue Jul 28 14:59:19 2015 +0000 +++ b/main.cpp Fri Jul 31 17:37:52 2015 +0000 @@ -6,6 +6,15 @@ // // 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" @@ -33,15 +42,18 @@ ASerial cSerial(CmdSerial); // Set up motor drive for left and right motors -#include "MotorDriveDBH1.h" +#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(5000); // ms between commands before automatic emergency stop + md.setCommandTimeout(15000); // ms between commands before automatic emergency stop //ms.setPWMfreqHz(8000); // these should be the defaults @@ -49,16 +61,9 @@ //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.setDecel(0.01f); // deceleration rate on STOP. This frac/ms + md.setDecelRate(500); // deceleration rate on STOP. This frac/ms } -// make sure deadman release stops immediately -void deadmanReleased() -{ - MotR.emergencyStop(); - MotL.emergencyStop(); - cSerial.println("STOP"); -} // Since this board has fancy tri-color LED, let's sequence it // instead of a boring old flash for a heartbeat @@ -86,8 +91,7 @@ float cr, cl; cr = MotR.getCurrent(); cl = MotL.getCurrent(); - cSerial.print("Current: left=");cSerial.print(cl); - cSerial.print(" right=");cSerial.println(cr); + CmdSerial.printf("\r\nCurrent: left=%.3f right=%.3f\r\n",cl,cr); } //void dumpSerialChar() @@ -105,49 +109,53 @@ // for diagnostics, just print a few messages, then be quiet to improve // performance when in actual use. -int nMsg = 99; +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); - - CmdSerial.baud(57600); - CmdSerial.puts("\r\nTankDrive for K64F with Bluetooth\r\n\n"); 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,t); break; - case 'R': MotR.setSpeed(val,t); break; + case 'L': MotL.setSpeed(val/255.0,t); break; + case 'R': MotR.setSpeed(val/255.0,t); break; default : - MotL.setSpeed(0,t); // odd command. just stop - MotR.setSpeed(0,t); - CmdSerial.puts("<stop\r\n"); + 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); + MotL.update(t); + MotR.update(t); if ((prevCommandTime > 0x0fffff00) && (t < 999)) { // time counter is close to wrapping around. make sure this does not happen. @@ -155,6 +163,7 @@ prevCommandTime = tFlash = 0; Time.reset(); Time.start(); + CmdSerial.puts("\r\nClock wrap-around\r\n"); while(Time.read() < 1); t = 1; }