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@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?

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 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 }