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@0:41ca27337c2b, 2015-07-28 (annotated)

Committer:
Mr_What
Date:
Tue Jul 28 14:59:19 2015 +0000
Revision:
0:41ca27337c2b
Child:
1:23d0a615756a
sloppy Arduino port, but (mostly) running.  Ready to test motor drive.

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 0:41ca27337c2b 9
Mr_What 0:41ca27337c2b 10 #include "mbed.h"
Mr_What 0:41ca27337c2b 11
Mr_What 0:41ca27337c2b 12 //DigitalOut gpo(D0);
Mr_What 0:41ca27337c2b 13 //DigitalOut led(LED_RED);
Mr_What 0:41ca27337c2b 14 //PwmOut ENA( PTD1); // D13 on Arduino Shield
Mr_What 0:41ca27337c2b 15 //PwmOut IN1A(PTD3); // D12 on Arduino Shield
Mr_What 0:41ca27337c2b 16 //PwmOut IN2A(PTD2); // D11 on Arduino Shield
Mr_What 0:41ca27337c2b 17 //PwmOut ENB( PTD0); // D13 on Arduino Shield
Mr_What 0:41ca27337c2b 18 //PwmOut IN1B(PTC4); // D12 on Arduino Shield
Mr_What 0:41ca27337c2b 19 //PwmOut IN2B(PTA0); // D11 on Arduino Shield
Mr_What 0:41ca27337c2b 20
Mr_What 0:41ca27337c2b 21 Timer Time;
Mr_What 0:41ca27337c2b 22 inline int millis() {return(Time.read_ms());} // mimic Arduino millis() function
Mr_What 0:41ca27337c2b 23
Mr_What 0:41ca27337c2b 24 // Tried to inherit/polymorph serial capabilities... but could
Mr_What 0:41ca27337c2b 25 // not get to compile... or get access to mbed::stream capabilities.
Mr_What 0:41ca27337c2b 26 // I know this is sloppy... but I'm just going to make a global
Mr_What 0:41ca27337c2b 27 // Serial, and let otherclasses have a reference to it.
Mr_What 0:41ca27337c2b 28 #include "Serial.h"
Mr_What 0:41ca27337c2b 29 //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 30 Serial CmdSerial(PTC15,PTC14); // Command/Diagnostic serial port on "bluetooth add-on" header
Mr_What 0:41ca27337c2b 31
Mr_What 0:41ca27337c2b 32 #include "ASerial.h" // emulation of some common Arduino Serial methods
Mr_What 0:41ca27337c2b 33 ASerial cSerial(CmdSerial);
Mr_What 0:41ca27337c2b 34
Mr_What 0:41ca27337c2b 35 // Set up motor drive for left and right motors
Mr_What 0:41ca27337c2b 36 #include "MotorDriveDBH1.h"
Mr_What 0:41ca27337c2b 37 MotorDrive MotL(PTD1,PTD3,PTD2,PTB2);
Mr_What 0:41ca27337c2b 38 MotorDrive MotR(PTD0,PTC4,PTA0,PTB3);
Mr_What 0:41ca27337c2b 39
Mr_What 0:41ca27337c2b 40 #include "Command.h"
Mr_What 0:41ca27337c2b 41
Mr_What 0:41ca27337c2b 42 void initMotorDrive(MotorDrive &md)
Mr_What 0:41ca27337c2b 43 {
Mr_What 0:41ca27337c2b 44 md.setCommandTimeout(5000); // ms between commands before automatic emergency stop
Mr_What 0:41ca27337c2b 45 //ms.setPWMfreqHz(8000);
Mr_What 0:41ca27337c2b 46
Mr_What 0:41ca27337c2b 47 // these should be the defaults
Mr_What 0:41ca27337c2b 48 //md.setStartupTime(5); // full power pulse this long when starting from full STOP
Mr_What 0:41ca27337c2b 49 //md.setStopDeadTime(3000); // wait this many ms after emergency STOP before starting up again
Mr_What 0:41ca27337c2b 50 //md.setMinPWM(0.004f); // any PWM command below this istreated as 0
Mr_What 0:41ca27337c2b 51 //md.setMaxPWM(0.98f); // these drives can fail if attempt to run full-100%
Mr_What 0:41ca27337c2b 52 //md.setDecel(0.01f); // deceleration rate on STOP. This frac/ms
Mr_What 0:41ca27337c2b 53
Mr_What 0:41ca27337c2b 54 }
Mr_What 0:41ca27337c2b 55 // make sure deadman release stops immediately
Mr_What 0:41ca27337c2b 56 void deadmanReleased()
Mr_What 0:41ca27337c2b 57 {
Mr_What 0:41ca27337c2b 58 MotR.emergencyStop();
Mr_What 0:41ca27337c2b 59 MotL.emergencyStop();
Mr_What 0:41ca27337c2b 60 cSerial.println("STOP");
Mr_What 0:41ca27337c2b 61 }
Mr_What 0:41ca27337c2b 62
Mr_What 0:41ca27337c2b 63 // Since this board has fancy tri-color LED, let's sequence it
Mr_What 0:41ca27337c2b 64 // instead of a boring old flash for a heartbeat
Mr_What 0:41ca27337c2b 65 DigitalOut ledR(LED_RED);
Mr_What 0:41ca27337c2b 66 DigitalOut ledG(LED_GREEN);
Mr_What 0:41ca27337c2b 67 DigitalOut ledB(LED_BLUE);
Mr_What 0:41ca27337c2b 68 void toggleFlash()
Mr_What 0:41ca27337c2b 69 {
Mr_What 0:41ca27337c2b 70 static int k=0;
Mr_What 0:41ca27337c2b 71 k++;
Mr_What 0:41ca27337c2b 72 if ((k<0) || (k>7)) k=0;
Mr_What 0:41ca27337c2b 73 // Gray code counter...
Mr_What 0:41ca27337c2b 74 switch(k)
Mr_What 0:41ca27337c2b 75 {
Mr_What 0:41ca27337c2b 76 case 1:
Mr_What 0:41ca27337c2b 77 case 5: ledG = !ledG; break;
Mr_What 0:41ca27337c2b 78 case 3:
Mr_What 0:41ca27337c2b 79 case 7: ledB = !ledB; break;
Mr_What 0:41ca27337c2b 80 default: ledR = !ledR; break;
Mr_What 0:41ca27337c2b 81 }
Mr_What 0:41ca27337c2b 82 }
Mr_What 0:41ca27337c2b 83
Mr_What 0:41ca27337c2b 84 void reportCurrent()
Mr_What 0:41ca27337c2b 85 {
Mr_What 0:41ca27337c2b 86 float cr, cl;
Mr_What 0:41ca27337c2b 87 cr = MotR.getCurrent();
Mr_What 0:41ca27337c2b 88 cl = MotL.getCurrent();
Mr_What 0:41ca27337c2b 89 cSerial.print("Current: left=");cSerial.print(cl);
Mr_What 0:41ca27337c2b 90 cSerial.print(" right=");cSerial.println(cr);
Mr_What 0:41ca27337c2b 91 }
Mr_What 0:41ca27337c2b 92
Mr_What 0:41ca27337c2b 93 //void dumpSerialChar()
Mr_What 0:41ca27337c2b 94 //{
Mr_What 0:41ca27337c2b 95 // int i = cSerial.getc();
Mr_What 0:41ca27337c2b 96 // CmdSerial.printf("%d %c\n",i,i);
Mr_What 0:41ca27337c2b 97 //}
Mr_What 0:41ca27337c2b 98
Mr_What 0:41ca27337c2b 99 // ================================================== main
Mr_What 0:41ca27337c2b 100
Mr_What 0:41ca27337c2b 101 int prevCommandTime=0;
Mr_What 0:41ca27337c2b 102
Mr_What 0:41ca27337c2b 103 #define FLASH_DT 800
Mr_What 0:41ca27337c2b 104 int tFlash = 0;
Mr_What 0:41ca27337c2b 105
Mr_What 0:41ca27337c2b 106 // for diagnostics, just print a few messages, then be quiet to improve
Mr_What 0:41ca27337c2b 107 // performance when in actual use.
Mr_What 0:41ca27337c2b 108 int nMsg = 99;
Mr_What 0:41ca27337c2b 109
Mr_What 0:41ca27337c2b 110 int main()
Mr_What 0:41ca27337c2b 111 {
Mr_What 0:41ca27337c2b 112
Mr_What 0:41ca27337c2b 113 // Set motor drive parameters
Mr_What 0:41ca27337c2b 114 initMotorDrive(MotL);
Mr_What 0:41ca27337c2b 115 initMotorDrive(MotR);
Mr_What 0:41ca27337c2b 116
Mr_What 0:41ca27337c2b 117 CmdSerial.baud(57600);
Mr_What 0:41ca27337c2b 118 CmdSerial.puts("\r\nTankDrive for K64F with Bluetooth\r\n\n");
Mr_What 0:41ca27337c2b 119
Mr_What 0:41ca27337c2b 120 Time.reset();
Mr_What 0:41ca27337c2b 121 Time.start();
Mr_What 0:41ca27337c2b 122
Mr_What 0:41ca27337c2b 123 CommandReader cmd(cSerial);
Mr_What 0:41ca27337c2b 124
Mr_What 0:41ca27337c2b 125 while (true) {
Mr_What 0:41ca27337c2b 126 int t = Time.read_ms();
Mr_What 0:41ca27337c2b 127
Mr_What 0:41ca27337c2b 128 char code;
Mr_What 0:41ca27337c2b 129 int val;
Mr_What 0:41ca27337c2b 130 int stat = cmd.get(code,val);
Mr_What 0:41ca27337c2b 131 if (stat)
Mr_What 0:41ca27337c2b 132 {
Mr_What 0:41ca27337c2b 133 prevCommandTime = t;
Mr_What 0:41ca27337c2b 134 if (nMsg>0){nMsg--;CmdSerial.printf(">%c%d\r\n",code,val);}
Mr_What 0:41ca27337c2b 135 /*
Mr_What 0:41ca27337c2b 136 switch(code)
Mr_What 0:41ca27337c2b 137 {
Mr_What 0:41ca27337c2b 138 case 'L': MotL.setSpeed(val,t); break;
Mr_What 0:41ca27337c2b 139 case 'R': MotR.setSpeed(val,t); break;
Mr_What 0:41ca27337c2b 140 default :
Mr_What 0:41ca27337c2b 141 MotL.setSpeed(0,t); // odd command. just stop
Mr_What 0:41ca27337c2b 142 MotR.setSpeed(0,t);
Mr_What 0:41ca27337c2b 143 CmdSerial.puts("<stop\r\n");
Mr_What 0:41ca27337c2b 144 }
Mr_What 0:41ca27337c2b 145 */
Mr_What 0:41ca27337c2b 146 }
Mr_What 0:41ca27337c2b 147 else
Mr_What 0:41ca27337c2b 148 { // no command, do housekeeping (misc state update stuff)
Mr_What 0:41ca27337c2b 149 //MotL.update(t);
Mr_What 0:41ca27337c2b 150 //MotR.update(t);
Mr_What 0:41ca27337c2b 151
Mr_What 0:41ca27337c2b 152 if ((prevCommandTime > 0x0fffff00) && (t < 999))
Mr_What 0:41ca27337c2b 153 { // time counter is close to wrapping around. make sure this does not happen.
Mr_What 0:41ca27337c2b 154 // I think we can tolerate a minor glitch once every 24.8 days of continuous use
Mr_What 0:41ca27337c2b 155 prevCommandTime = tFlash = 0;
Mr_What 0:41ca27337c2b 156 Time.reset();
Mr_What 0:41ca27337c2b 157 Time.start();
Mr_What 0:41ca27337c2b 158 while(Time.read() < 1);
Mr_What 0:41ca27337c2b 159 t = 1;
Mr_What 0:41ca27337c2b 160 }
Mr_What 0:41ca27337c2b 161
Mr_What 0:41ca27337c2b 162 if (t - tFlash > FLASH_DT)
Mr_What 0:41ca27337c2b 163 { // Flash standard LED to show things are running
Mr_What 0:41ca27337c2b 164 tFlash = t;
Mr_What 0:41ca27337c2b 165 CmdSerial.printf("dt=%d\r\n",t);
Mr_What 0:41ca27337c2b 166 toggleFlash();
Mr_What 0:41ca27337c2b 167 //reportCurrent();
Mr_What 0:41ca27337c2b 168 //wait(0.8f);
Mr_What 0:41ca27337c2b 169 }
Mr_What 0:41ca27337c2b 170 }
Mr_What 0:41ca27337c2b 171 }
Mr_What 0:41ca27337c2b 172 }