Aaron Birenboim / Mbed 2 deprecated TankDrive Featured

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

Dependencies:   mbed

main.cpp@2:54d27fdcbe5c, 2015-08-02 (annotated)

Committer:
Mr_What
Date:
Sun Aug 02 18:34:12 2015 +0000
Revision:
2:54d27fdcbe5c
Parent:
1:23d0a615756a
Child:
3:502f90649834
moved to new re-port of modified L298N driver.; still having serial communications issues

Who changed what in which revision?

UserRevisionLine numberNew 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 0:41ca27337c2b 40
Mr_What 2:54d27fdcbe5c 41 // emulation of some Arduino serial methods.
Mr_What 2:54d27fdcbe5c 42 // this class has a singleton interrupt callback, so it
Mr_What 2:54d27fdcbe5c 43 // creates a singleton global
Mr_What 0:41ca27337c2b 44 #include "ASerial.h" // emulation of some common Arduino Serial methods
Mr_What 0:41ca27337c2b 45 ASerial cSerial(CmdSerial);
Mr_What 0:41ca27337c2b 46
Mr_What 0:41ca27337c2b 47 // Set up motor drive for left and right motors
Mr_What 1:23d0a615756a 48 #define DBH1 // use DBH-1x modifications to typicsl L298 drive logic
Mr_What 1:23d0a615756a 49 #include "MotorDrive298.h"
Mr_What 0:41ca27337c2b 50 MotorDrive MotL(PTD1,PTD3,PTD2,PTB2);
Mr_What 0:41ca27337c2b 51 MotorDrive MotR(PTD0,PTC4,PTA0,PTB3);
Mr_What 0:41ca27337c2b 52
Mr_What 0:41ca27337c2b 53 #include "Command.h"
Mr_What 0:41ca27337c2b 54
Mr_What 1:23d0a615756a 55 // ------------------------------------------------------------------------------
Mr_What 1:23d0a615756a 56
Mr_What 0:41ca27337c2b 57 void initMotorDrive(MotorDrive &md)
Mr_What 0:41ca27337c2b 58 {
Mr_What 1:23d0a615756a 59 md.setCommandTimeout(15000); // ms between commands before automatic emergency stop
Mr_What 0:41ca27337c2b 60 //ms.setPWMfreqHz(8000);
Mr_What 0:41ca27337c2b 61
Mr_What 0:41ca27337c2b 62 // these should be the defaults
Mr_What 0:41ca27337c2b 63 //md.setStartupTime(5); // full power pulse this long when starting from full STOP
Mr_What 0:41ca27337c2b 64 //md.setStopDeadTime(3000); // wait this many ms after emergency STOP before starting up again
Mr_What 0:41ca27337c2b 65 //md.setMinPWM(0.004f); // any PWM command below this istreated as 0
Mr_What 0:41ca27337c2b 66 //md.setMaxPWM(0.98f); // these drives can fail if attempt to run full-100%
Mr_What 1:23d0a615756a 67 md.setDecelRate(500); // deceleration rate on STOP. This frac/ms
Mr_What 0:41ca27337c2b 68
Mr_What 0:41ca27337c2b 69 }
Mr_What 0:41ca27337c2b 70
Mr_What 0:41ca27337c2b 71 // Since this board has fancy tri-color LED, let's sequence it
Mr_What 0:41ca27337c2b 72 // instead of a boring old flash for a heartbeat
Mr_What 0:41ca27337c2b 73 DigitalOut ledR(LED_RED);
Mr_What 0:41ca27337c2b 74 DigitalOut ledG(LED_GREEN);
Mr_What 0:41ca27337c2b 75 DigitalOut ledB(LED_BLUE);
Mr_What 0:41ca27337c2b 76 void toggleFlash()
Mr_What 0:41ca27337c2b 77 {
Mr_What 0:41ca27337c2b 78 static int k=0;
Mr_What 0:41ca27337c2b 79 k++;
Mr_What 0:41ca27337c2b 80 if ((k<0) || (k>7)) k=0;
Mr_What 0:41ca27337c2b 81 // Gray code counter...
Mr_What 0:41ca27337c2b 82 switch(k)
Mr_What 0:41ca27337c2b 83 {
Mr_What 0:41ca27337c2b 84 case 1:
Mr_What 0:41ca27337c2b 85 case 5: ledG = !ledG; break;
Mr_What 0:41ca27337c2b 86 case 3:
Mr_What 0:41ca27337c2b 87 case 7: ledB = !ledB; break;
Mr_What 0:41ca27337c2b 88 default: ledR = !ledR; break;
Mr_What 0:41ca27337c2b 89 }
Mr_What 0:41ca27337c2b 90 }
Mr_What 0:41ca27337c2b 91
Mr_What 0:41ca27337c2b 92 void reportCurrent()
Mr_What 0:41ca27337c2b 93 {
Mr_What 0:41ca27337c2b 94 float cr, cl;
Mr_What 0:41ca27337c2b 95 cr = MotR.getCurrent();
Mr_What 0:41ca27337c2b 96 cl = MotL.getCurrent();
Mr_What 2:54d27fdcbe5c 97 CmdSerial.printf("\tCurrent: left=%.3f right=%.3f\r\n",cl,cr);
Mr_What 0:41ca27337c2b 98 }
Mr_What 0:41ca27337c2b 99
Mr_What 0:41ca27337c2b 100 //void dumpSerialChar()
Mr_What 0:41ca27337c2b 101 //{
Mr_What 0:41ca27337c2b 102 // int i = cSerial.getc();
Mr_What 0:41ca27337c2b 103 // CmdSerial.printf("%d %c\n",i,i);
Mr_What 0:41ca27337c2b 104 //}
Mr_What 0:41ca27337c2b 105
Mr_What 0:41ca27337c2b 106 // ================================================== main
Mr_What 0:41ca27337c2b 107
Mr_What 0:41ca27337c2b 108 int prevCommandTime=0;
Mr_What 0:41ca27337c2b 109
Mr_What 0:41ca27337c2b 110 #define FLASH_DT 800
Mr_What 0:41ca27337c2b 111 int tFlash = 0;
Mr_What 0:41ca27337c2b 112
Mr_What 0:41ca27337c2b 113 // for diagnostics, just print a few messages, then be quiet to improve
Mr_What 0:41ca27337c2b 114 // performance when in actual use.
Mr_What 1:23d0a615756a 115 int nMsg = 9;
Mr_What 0:41ca27337c2b 116
Mr_What 0:41ca27337c2b 117 int main()
Mr_What 0:41ca27337c2b 118 {
Mr_What 1:23d0a615756a 119 CmdSerial.baud(57600);
Mr_What 1:23d0a615756a 120 CmdSerial.puts("\r\nTankDrive for K64F with Bluetooth\r\n\n");
Mr_What 2:54d27fdcbe5c 121 CmdSerial.attach(&gotChar); // singleton serial character buffer
Mr_What 2:54d27fdcbe5c 122
Mr_What 0:41ca27337c2b 123 // Set motor drive parameters
Mr_What 0:41ca27337c2b 124 initMotorDrive(MotL);
Mr_What 0:41ca27337c2b 125 initMotorDrive(MotR);
Mr_What 0:41ca27337c2b 126
Mr_What 0:41ca27337c2b 127 Time.reset();
Mr_What 0:41ca27337c2b 128 Time.start();
Mr_What 0:41ca27337c2b 129
Mr_What 2:54d27fdcbe5c 130 CommandReader cmd;
Mr_What 1:23d0a615756a 131
Mr_What 1:23d0a615756a 132 //int detailMsg=9;
Mr_What 0:41ca27337c2b 133 while (true) {
Mr_What 0:41ca27337c2b 134 int t = Time.read_ms();
Mr_What 1:23d0a615756a 135 //if(--detailMsg>0)CmdSerial.printf("%d\r\n",t);
Mr_What 0:41ca27337c2b 136 char code;
Mr_What 0:41ca27337c2b 137 int val;
Mr_What 0:41ca27337c2b 138 int stat = cmd.get(code,val);
Mr_What 1:23d0a615756a 139
Mr_What 0:41ca27337c2b 140 if (stat)
Mr_What 0:41ca27337c2b 141 {
Mr_What 0:41ca27337c2b 142 prevCommandTime = t;
Mr_What 0:41ca27337c2b 143 if (nMsg>0){nMsg--;CmdSerial.printf(">%c%d\r\n",code,val);}
Mr_What 1:23d0a615756a 144
Mr_What 0:41ca27337c2b 145 switch(code)
Mr_What 0:41ca27337c2b 146 {
Mr_What 1:23d0a615756a 147 case 'L': MotL.setSpeed(val/255.0,t); break;
Mr_What 1:23d0a615756a 148 case 'R': MotR.setSpeed(val/255.0,t); break;
Mr_What 0:41ca27337c2b 149 default :
Mr_What 1:23d0a615756a 150 CmdSerial.printf("Unidentified command \"%c%d\" (stop)",code,val);
Mr_What 1:23d0a615756a 151 MotL.stop();
Mr_What 1:23d0a615756a 152 MotR.stop();
Mr_What 0:41ca27337c2b 153 }
Mr_What 1:23d0a615756a 154 //CmdSerial.puts("\nrcd\r");
Mr_What 1:23d0a615756a 155 //CmdSerial.puts("\r\n");
Mr_What 1:23d0a615756a 156 //detailMsg=2;
Mr_What 1:23d0a615756a 157 //CmdSerial.putc('\n');
Mr_What 0:41ca27337c2b 158 }
Mr_What 0:41ca27337c2b 159 else
Mr_What 0:41ca27337c2b 160 { // no command, do housekeeping (misc state update stuff)
Mr_What 2:54d27fdcbe5c 161 //MotL.update(t);
Mr_What 2:54d27fdcbe5c 162 //MotR.update(t);
Mr_What 0:41ca27337c2b 163
Mr_What 0:41ca27337c2b 164 if ((prevCommandTime > 0x0fffff00) && (t < 999))
Mr_What 0:41ca27337c2b 165 { // time counter is close to wrapping around. make sure this does not happen.
Mr_What 0:41ca27337c2b 166 // I think we can tolerate a minor glitch once every 24.8 days of continuous use
Mr_What 0:41ca27337c2b 167 prevCommandTime = tFlash = 0;
Mr_What 0:41ca27337c2b 168 Time.reset();
Mr_What 0:41ca27337c2b 169 Time.start();
Mr_What 1:23d0a615756a 170 CmdSerial.puts("\r\nClock wrap-around\r\n");
Mr_What 0:41ca27337c2b 171 while(Time.read() < 1);
Mr_What 0:41ca27337c2b 172 t = 1;
Mr_What 0:41ca27337c2b 173 }
Mr_What 0:41ca27337c2b 174
Mr_What 0:41ca27337c2b 175 if (t - tFlash > FLASH_DT)
Mr_What 0:41ca27337c2b 176 { // Flash standard LED to show things are running
Mr_What 0:41ca27337c2b 177 tFlash = t;
Mr_What 0:41ca27337c2b 178 CmdSerial.printf("dt=%d\r\n",t);
Mr_What 0:41ca27337c2b 179 toggleFlash();
Mr_What 0:41ca27337c2b 180 //reportCurrent();
Mr_What 0:41ca27337c2b 181 //wait(0.8f);
Mr_What 0:41ca27337c2b 182 }
Mr_What 0:41ca27337c2b 183 }
Mr_What 0:41ca27337c2b 184 }
Mr_What 0:41ca27337c2b 185 }