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:
- 0:41ca27337c2b
- Child:
- 1:23d0a615756a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Jul 28 14:59:19 2015 +0000 @@ -0,0 +1,172 @@ +// 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. + +#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 +#include "MotorDriveDBH1.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 + //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.setDecel(0.01f); // 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 +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(); + cSerial.print("Current: left=");cSerial.print(cl); + cSerial.print(" right=");cSerial.println(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 = 99; + +int main() +{ + + // 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); + + while (true) { + int t = Time.read_ms(); + + 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; + default : + MotL.setSpeed(0,t); // odd command. just stop + MotR.setSpeed(0,t); + CmdSerial.puts("<stop\r\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(); + 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); + } + } + } +}