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@4:7620d21baef3, 2015-08-16 (annotated)
- Committer:
- Mr_What
- Date:
- Sun Aug 16 22:46:25 2015 +0000
- Revision:
- 4:7620d21baef3
- Parent:
- 3:502f90649834
seems to be running from app with reduced diagnostics
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Mr_What | 0:41ca27337c2b | 1 | // Firmware to demonstrate typical "Tank-Drive" robot motor control. |
Mr_What | 0:41ca27337c2b | 2 | // one left motor, and one right motor. |
Mr_What | 0:41ca27337c2b | 3 | // |
Mr_What | 0:41ca27337c2b | 4 | // Can do simple commanding from a bluetooth terminal (like Android's BlueTerm), |
Mr_What | 0:41ca27337c2b | 5 | // or from the Android app used in http://www.instructables.com/id/Simple-RC-car-for-beginners-Android-control-over-/ |
Mr_What | 0:41ca27337c2b | 6 | // |
Mr_What | 0:41ca27337c2b | 7 | // The FreeScale FRDM-K64F is a much more powerful board than needed for this task, |
Mr_What | 0:41ca27337c2b | 8 | // but it is a good template for a typical "tank-drive" robot based on the FRDM-K64F. |
Mr_What | 1:23d0a615756a | 9 | // |
Mr_What | 1:23d0a615756a | 10 | // The DBH-1x motor driver has very similar inputs to the very common L298N |
Mr_What | 1:23d0a615756a | 11 | // dual-H-bridge driver chip. One main difference is that it warns that |
Mr_What | 1:23d0a615756a | 12 | // the drive is to be used at no more than 98% PWM. In order |
Mr_What | 1:23d0a615756a | 13 | // to meet this extra requirement over common L298 motor driver logic, |
Mr_What | 1:23d0a615756a | 14 | // the direction indicator inputs are PWM at 98% instead of logic "1" |
Mr_What | 1:23d0a615756a | 15 | // |
Mr_What | 1:23d0a615756a | 16 | // Aaron Birenboim, http://boim.com 31jul2015 |
Mr_What | 1:23d0a615756a | 17 | // Apache license |
Mr_What | 0:41ca27337c2b | 18 | |
Mr_What | 0:41ca27337c2b | 19 | #include "mbed.h" |
Mr_What | 0:41ca27337c2b | 20 | |
Mr_What | 0:41ca27337c2b | 21 | //DigitalOut gpo(D0); |
Mr_What | 0:41ca27337c2b | 22 | //DigitalOut led(LED_RED); |
Mr_What | 0:41ca27337c2b | 23 | //PwmOut ENA( PTD1); // D13 on Arduino Shield |
Mr_What | 0:41ca27337c2b | 24 | //PwmOut IN1A(PTD3); // D12 on Arduino Shield |
Mr_What | 0:41ca27337c2b | 25 | //PwmOut IN2A(PTD2); // D11 on Arduino Shield |
Mr_What | 0:41ca27337c2b | 26 | //PwmOut ENB( PTD0); // D13 on Arduino Shield |
Mr_What | 0:41ca27337c2b | 27 | //PwmOut IN1B(PTC4); // D12 on Arduino Shield |
Mr_What | 0:41ca27337c2b | 28 | //PwmOut IN2B(PTA0); // D11 on Arduino Shield |
Mr_What | 0:41ca27337c2b | 29 | |
Mr_What | 0:41ca27337c2b | 30 | Timer Time; |
Mr_What | 0:41ca27337c2b | 31 | inline int millis() {return(Time.read_ms());} // mimic Arduino millis() function |
Mr_What | 0:41ca27337c2b | 32 | |
Mr_What | 0:41ca27337c2b | 33 | // Tried to inherit/polymorph serial capabilities... but could |
Mr_What | 0:41ca27337c2b | 34 | // not get to compile... or get access to mbed::stream capabilities. |
Mr_What | 0:41ca27337c2b | 35 | // I know this is sloppy... but I'm just going to make a global |
Mr_What | 0:41ca27337c2b | 36 | // Serial, and let otherclasses have a reference to it. |
Mr_What | 0:41ca27337c2b | 37 | #include "Serial.h" |
Mr_What | 0:41ca27337c2b | 38 | //Serial CmdSerial(PTC17,PTC16); // Command/Diagnostic serial port on UART3, sicne I don't know how to use USB ports (yet) |
Mr_What | 0:41ca27337c2b | 39 | Serial CmdSerial(PTC15,PTC14); // Command/Diagnostic serial port on "bluetooth add-on" header |
Mr_What | 3:502f90649834 | 40 | Serial DiagSerial(USBTX, USBRX); |
Mr_What | 0:41ca27337c2b | 41 | |
Mr_What | 2:54d27fdcbe5c | 42 | // emulation of some Arduino serial methods. |
Mr_What | 2:54d27fdcbe5c | 43 | // this class has a singleton interrupt callback, so it |
Mr_What | 2:54d27fdcbe5c | 44 | // creates a singleton global |
Mr_What | 0:41ca27337c2b | 45 | #include "ASerial.h" // emulation of some common Arduino Serial methods |
Mr_What | 0:41ca27337c2b | 46 | ASerial cSerial(CmdSerial); |
Mr_What | 3:502f90649834 | 47 | //ASerial cSerial(DiagSerial); |
Mr_What | 0:41ca27337c2b | 48 | |
Mr_What | 0:41ca27337c2b | 49 | // Set up motor drive for left and right motors |
Mr_What | 1:23d0a615756a | 50 | #define DBH1 // use DBH-1x modifications to typicsl L298 drive logic |
Mr_What | 1:23d0a615756a | 51 | #include "MotorDrive298.h" |
Mr_What | 3:502f90649834 | 52 | // en, in1, in2, ct |
Mr_What | 3:502f90649834 | 53 | MotorDrive MotL(PTD2,PTD3,PTD1,PTB2); |
Mr_What | 3:502f90649834 | 54 | MotorDrive MotR(PTC3,PTC4,PTD0,PTB3); |
Mr_What | 0:41ca27337c2b | 55 | |
Mr_What | 0:41ca27337c2b | 56 | #include "Command.h" |
Mr_What | 0:41ca27337c2b | 57 | |
Mr_What | 1:23d0a615756a | 58 | // ------------------------------------------------------------------------------ |
Mr_What | 1:23d0a615756a | 59 | |
Mr_What | 0:41ca27337c2b | 60 | void initMotorDrive(MotorDrive &md) |
Mr_What | 0:41ca27337c2b | 61 | { |
Mr_What | 1:23d0a615756a | 62 | md.setCommandTimeout(15000); // ms between commands before automatic emergency stop |
Mr_What | 0:41ca27337c2b | 63 | //ms.setPWMfreqHz(8000); |
Mr_What | 0:41ca27337c2b | 64 | |
Mr_What | 0:41ca27337c2b | 65 | // these should be the defaults |
Mr_What | 0:41ca27337c2b | 66 | //md.setStartupTime(5); // full power pulse this long when starting from full STOP |
Mr_What | 0:41ca27337c2b | 67 | //md.setStopDeadTime(3000); // wait this many ms after emergency STOP before starting up again |
Mr_What | 0:41ca27337c2b | 68 | //md.setMinPWM(0.004f); // any PWM command below this istreated as 0 |
Mr_What | 0:41ca27337c2b | 69 | //md.setMaxPWM(0.98f); // these drives can fail if attempt to run full-100% |
Mr_What | 1:23d0a615756a | 70 | md.setDecelRate(500); // deceleration rate on STOP. This frac/ms |
Mr_What | 0:41ca27337c2b | 71 | |
Mr_What | 0:41ca27337c2b | 72 | } |
Mr_What | 0:41ca27337c2b | 73 | |
Mr_What | 0:41ca27337c2b | 74 | // Since this board has fancy tri-color LED, let's sequence it |
Mr_What | 0:41ca27337c2b | 75 | // instead of a boring old flash for a heartbeat |
Mr_What | 0:41ca27337c2b | 76 | DigitalOut ledR(LED_RED); |
Mr_What | 0:41ca27337c2b | 77 | DigitalOut ledG(LED_GREEN); |
Mr_What | 0:41ca27337c2b | 78 | DigitalOut ledB(LED_BLUE); |
Mr_What | 0:41ca27337c2b | 79 | void toggleFlash() |
Mr_What | 0:41ca27337c2b | 80 | { |
Mr_What | 0:41ca27337c2b | 81 | static int k=0; |
Mr_What | 0:41ca27337c2b | 82 | k++; |
Mr_What | 0:41ca27337c2b | 83 | if ((k<0) || (k>7)) k=0; |
Mr_What | 0:41ca27337c2b | 84 | // Gray code counter... |
Mr_What | 0:41ca27337c2b | 85 | switch(k) |
Mr_What | 0:41ca27337c2b | 86 | { |
Mr_What | 0:41ca27337c2b | 87 | case 1: |
Mr_What | 0:41ca27337c2b | 88 | case 5: ledG = !ledG; break; |
Mr_What | 0:41ca27337c2b | 89 | case 3: |
Mr_What | 0:41ca27337c2b | 90 | case 7: ledB = !ledB; break; |
Mr_What | 0:41ca27337c2b | 91 | default: ledR = !ledR; break; |
Mr_What | 0:41ca27337c2b | 92 | } |
Mr_What | 0:41ca27337c2b | 93 | } |
Mr_What | 0:41ca27337c2b | 94 | |
Mr_What | 0:41ca27337c2b | 95 | void reportCurrent() |
Mr_What | 0:41ca27337c2b | 96 | { |
Mr_What | 0:41ca27337c2b | 97 | float cr, cl; |
Mr_What | 0:41ca27337c2b | 98 | cr = MotR.getCurrent(); |
Mr_What | 0:41ca27337c2b | 99 | cl = MotL.getCurrent(); |
Mr_What | 4:7620d21baef3 | 100 | DiagSerial.printf("\tCurrent: left=%.3f right=%.3f\r\n",cl,cr); |
Mr_What | 0:41ca27337c2b | 101 | } |
Mr_What | 0:41ca27337c2b | 102 | |
Mr_What | 0:41ca27337c2b | 103 | //void dumpSerialChar() |
Mr_What | 0:41ca27337c2b | 104 | //{ |
Mr_What | 0:41ca27337c2b | 105 | // int i = cSerial.getc(); |
Mr_What | 0:41ca27337c2b | 106 | // CmdSerial.printf("%d %c\n",i,i); |
Mr_What | 0:41ca27337c2b | 107 | //} |
Mr_What | 0:41ca27337c2b | 108 | |
Mr_What | 0:41ca27337c2b | 109 | // ================================================== main |
Mr_What | 0:41ca27337c2b | 110 | |
Mr_What | 0:41ca27337c2b | 111 | int prevCommandTime=0; |
Mr_What | 0:41ca27337c2b | 112 | |
Mr_What | 0:41ca27337c2b | 113 | #define FLASH_DT 800 |
Mr_What | 0:41ca27337c2b | 114 | int tFlash = 0; |
Mr_What | 0:41ca27337c2b | 115 | |
Mr_What | 0:41ca27337c2b | 116 | // for diagnostics, just print a few messages, then be quiet to improve |
Mr_What | 0:41ca27337c2b | 117 | // performance when in actual use. |
Mr_What | 1:23d0a615756a | 118 | int nMsg = 9; |
Mr_What | 0:41ca27337c2b | 119 | |
Mr_What | 0:41ca27337c2b | 120 | int main() |
Mr_What | 0:41ca27337c2b | 121 | { |
Mr_What | 4:7620d21baef3 | 122 | DiagSerial.baud(115200); DiagSerial.puts("TankDrive Diagnostics\r"); |
Mr_What | 4:7620d21baef3 | 123 | |
Mr_What | 4:7620d21baef3 | 124 | // have been getting lock-ups when running app. could 57600 be too fast for BT UART on K64F? |
Mr_What | 1:23d0a615756a | 125 | CmdSerial.baud(57600); |
Mr_What | 1:23d0a615756a | 126 | CmdSerial.puts("\r\nTankDrive for K64F with Bluetooth\r\n\n"); |
Mr_What | 2:54d27fdcbe5c | 127 | CmdSerial.attach(&gotChar); // singleton serial character buffer |
Mr_What | 3:502f90649834 | 128 | |
Mr_What | 0:41ca27337c2b | 129 | // Set motor drive parameters |
Mr_What | 0:41ca27337c2b | 130 | initMotorDrive(MotL); |
Mr_What | 0:41ca27337c2b | 131 | initMotorDrive(MotR); |
Mr_What | 0:41ca27337c2b | 132 | |
Mr_What | 0:41ca27337c2b | 133 | Time.reset(); |
Mr_What | 0:41ca27337c2b | 134 | Time.start(); |
Mr_What | 0:41ca27337c2b | 135 | |
Mr_What | 2:54d27fdcbe5c | 136 | CommandReader cmd; |
Mr_What | 1:23d0a615756a | 137 | |
Mr_What | 1:23d0a615756a | 138 | //int detailMsg=9; |
Mr_What | 0:41ca27337c2b | 139 | while (true) { |
Mr_What | 0:41ca27337c2b | 140 | int t = Time.read_ms(); |
Mr_What | 1:23d0a615756a | 141 | //if(--detailMsg>0)CmdSerial.printf("%d\r\n",t); |
Mr_What | 0:41ca27337c2b | 142 | char code; |
Mr_What | 0:41ca27337c2b | 143 | int val; |
Mr_What | 0:41ca27337c2b | 144 | int stat = cmd.get(code,val); |
Mr_What | 1:23d0a615756a | 145 | |
Mr_What | 0:41ca27337c2b | 146 | if (stat) |
Mr_What | 0:41ca27337c2b | 147 | { |
Mr_What | 0:41ca27337c2b | 148 | prevCommandTime = t; |
Mr_What | 3:502f90649834 | 149 | if (nMsg>0){nMsg--;DiagSerial.printf("\r\n\t\tcmd>%c%d\r\n",code,val);} |
Mr_What | 1:23d0a615756a | 150 | |
Mr_What | 0:41ca27337c2b | 151 | switch(code) |
Mr_What | 0:41ca27337c2b | 152 | { |
Mr_What | 1:23d0a615756a | 153 | case 'L': MotL.setSpeed(val/255.0,t); break; |
Mr_What | 1:23d0a615756a | 154 | case 'R': MotR.setSpeed(val/255.0,t); break; |
Mr_What | 0:41ca27337c2b | 155 | default : |
Mr_What | 3:502f90649834 | 156 | DiagSerial.printf("Unidentified command \"%c%d\" (stop)",code,val); |
Mr_What | 1:23d0a615756a | 157 | MotL.stop(); |
Mr_What | 1:23d0a615756a | 158 | MotR.stop(); |
Mr_What | 0:41ca27337c2b | 159 | } |
Mr_What | 3:502f90649834 | 160 | |
Mr_What | 1:23d0a615756a | 161 | //CmdSerial.puts("\nrcd\r"); |
Mr_What | 1:23d0a615756a | 162 | //CmdSerial.puts("\r\n"); |
Mr_What | 1:23d0a615756a | 163 | //detailMsg=2; |
Mr_What | 1:23d0a615756a | 164 | //CmdSerial.putc('\n'); |
Mr_What | 0:41ca27337c2b | 165 | } |
Mr_What | 0:41ca27337c2b | 166 | else |
Mr_What | 0:41ca27337c2b | 167 | { // no command, do housekeeping (misc state update stuff) |
Mr_What | 3:502f90649834 | 168 | MotL.update(t); |
Mr_What | 3:502f90649834 | 169 | MotR.update(t); |
Mr_What | 0:41ca27337c2b | 170 | |
Mr_What | 0:41ca27337c2b | 171 | if ((prevCommandTime > 0x0fffff00) && (t < 999)) |
Mr_What | 0:41ca27337c2b | 172 | { // time counter is close to wrapping around. make sure this does not happen. |
Mr_What | 0:41ca27337c2b | 173 | // I think we can tolerate a minor glitch once every 24.8 days of continuous use |
Mr_What | 0:41ca27337c2b | 174 | prevCommandTime = tFlash = 0; |
Mr_What | 0:41ca27337c2b | 175 | Time.reset(); |
Mr_What | 0:41ca27337c2b | 176 | Time.start(); |
Mr_What | 1:23d0a615756a | 177 | CmdSerial.puts("\r\nClock wrap-around\r\n"); |
Mr_What | 0:41ca27337c2b | 178 | while(Time.read() < 1); |
Mr_What | 0:41ca27337c2b | 179 | t = 1; |
Mr_What | 0:41ca27337c2b | 180 | } |
Mr_What | 0:41ca27337c2b | 181 | |
Mr_What | 0:41ca27337c2b | 182 | if (t - tFlash > FLASH_DT) |
Mr_What | 0:41ca27337c2b | 183 | { // Flash standard LED to show things are running |
Mr_What | 0:41ca27337c2b | 184 | tFlash = t; |
Mr_What | 3:502f90649834 | 185 | DiagSerial.printf("dt=%d\r",t); |
Mr_What | 0:41ca27337c2b | 186 | toggleFlash(); |
Mr_What | 0:41ca27337c2b | 187 | //reportCurrent(); |
Mr_What | 0:41ca27337c2b | 188 | //wait(0.8f); |
Mr_What | 0:41ca27337c2b | 189 | } |
Mr_What | 0:41ca27337c2b | 190 | } |
Mr_What | 0:41ca27337c2b | 191 | } |
Mr_What | 0:41ca27337c2b | 192 | } |