main.cpp

00001 // Firmware to demonstrate typical "Tank-Drive" robot motor control.
00002 // one left motor, and one right motor.
00003 //
00004 // Can do simple commanding from a bluetooth terminal (like Android's BlueTerm),
00005 // or from the Android app used in http://www.instructables.com/id/Simple-RC-car-for-beginners-Android-control-over-/
00006 //
00007 // The FreeScale FRDM-K64F is a much more powerful board than needed for this task,
00008 // but it is a good template for a typical "tank-drive" robot based on the FRDM-K64F.
00009 //
00010 // The DBH-1x motor driver has very similar inputs to the very common L298N
00011 // dual-H-bridge driver chip.  One main difference is that it warns that
00012 // the drive is to be used at no more than 98% PWM.  In order
00013 // to meet this extra requirement over common L298 motor driver logic,
00014 // the direction indicator inputs are PWM at 98% instead of logic "1"
00015 //
00016 // Aaron Birenboim, http://boim.com    31jul2015
00017 // Apache license
00018 
00019 #include "mbed.h"
00020 
00021 //DigitalOut gpo(D0);
00022 //DigitalOut led(LED_RED);
00023 //PwmOut ENA( PTD1);   // D13 on Arduino Shield
00024 //PwmOut IN1A(PTD3);  // D12 on Arduino Shield
00025 //PwmOut IN2A(PTD2);  // D11 on Arduino Shield
00026 //PwmOut ENB( PTD0);   // D13 on Arduino Shield
00027 //PwmOut IN1B(PTC4);  // D12 on Arduino Shield
00028 //PwmOut IN2B(PTA0);  // D11 on Arduino Shield
00029 
00030 Timer Time;
00031 inline int millis() {return(Time.read_ms());} // mimic Arduino millis() function
00032 
00033 // Tried to inherit/polymorph serial capabilities...  but could
00034 // not get to compile... or get access to mbed::stream capabilities.
00035 // I know this is sloppy...  but I'm just going to make a global
00036 // Serial, and let otherclasses have a reference to it.
00037 #include "Serial.h"
00038 //Serial CmdSerial(PTC17,PTC16);  // Command/Diagnostic serial port on UART3, sicne I don't know how to use USB ports (yet)
00039 Serial CmdSerial(PTC15,PTC14);  // Command/Diagnostic serial port on "bluetooth add-on" header
00040 Serial DiagSerial(USBTX, USBRX);
00041 
00042 // emulation of some Arduino serial methods.
00043 //    this class has a singleton interrupt callback, so it
00044 //    creates a singleton global
00045 #include "ASerial.h"  // emulation of some common Arduino Serial methods
00046 ASerial cSerial(CmdSerial); 
00047 //ASerial cSerial(DiagSerial); 
00048 
00049 // Set up motor drive for left and right motors
00050 #define DBH1   // use DBH-1x modifications to typicsl L298 drive logic
00051 #include "MotorDrive298.h"
00052 // en, in1, in2, ct
00053 MotorDrive MotL(PTD2,PTD3,PTD1,PTB2);
00054 MotorDrive MotR(PTC3,PTC4,PTD0,PTB3);
00055 
00056 #include "Command.h"
00057 
00058 // ------------------------------------------------------------------------------
00059 
00060 void initMotorDrive(MotorDrive &md)
00061 {
00062     md.setCommandTimeout(15000);  // ms between commands before automatic emergency stop
00063     //ms.setPWMfreqHz(8000);
00064     
00065     // these should be the defaults
00066     //md.setStartupTime(5);  // full power pulse this long when starting from full STOP
00067     //md.setStopDeadTime(3000); // wait this many ms after emergency STOP before starting up again
00068     //md.setMinPWM(0.004f);  // any PWM command below this istreated as 0
00069     //md.setMaxPWM(0.98f);  // these drives can fail if attempt to run full-100%
00070     md.setDecelRate(500);  // deceleration rate on STOP.  This frac/ms
00071     
00072 }
00073 
00074 // Since this board has fancy tri-color LED, let's sequence it
00075 // instead of a boring old flash for a heartbeat
00076 DigitalOut ledR(LED_RED);
00077 DigitalOut ledG(LED_GREEN);
00078 DigitalOut ledB(LED_BLUE);
00079 void toggleFlash()
00080 {
00081     static int k=0;
00082     k++;
00083     if ((k<0) || (k>7)) k=0;
00084     // Gray code counter...
00085     switch(k)
00086     {
00087         case 1:
00088         case 5:  ledG = !ledG; break;
00089         case 3:
00090         case 7:  ledB = !ledB; break;
00091         default: ledR = !ledR; break;
00092     }
00093 }
00094 
00095 void reportCurrent()
00096 {
00097   float cr, cl;
00098   cr = MotR.getCurrent();
00099   cl = MotL.getCurrent();
00100   DiagSerial.printf("\tCurrent:  left=%.3f  right=%.3f\r\n",cl,cr);
00101 }
00102 
00103 //void dumpSerialChar()
00104 //{
00105 //  int i = cSerial.getc();
00106 //  CmdSerial.printf("%d %c\n",i,i);    
00107 //}
00108     
00109 // ================================================== main
00110 
00111 int prevCommandTime=0;
00112 
00113 #define FLASH_DT 800
00114 int tFlash = 0;
00115 
00116 // for diagnostics, just print a few messages, then be quiet to improve
00117 // performance when in actual use.
00118 int nMsg = 9;
00119 
00120 int main()
00121 {
00122     DiagSerial.baud(115200);   DiagSerial.puts("TankDrive Diagnostics\r");
00123 
00124     // have been getting lock-ups when running app.  could 57600 be too fast for BT UART on K64F?
00125     CmdSerial.baud(57600);
00126     CmdSerial.puts("\r\nTankDrive for K64F with Bluetooth\r\n\n");
00127     CmdSerial.attach(&gotChar);  // singleton serial character buffer
00128     
00129     // Set motor drive parameters
00130     initMotorDrive(MotL);
00131     initMotorDrive(MotR);
00132     
00133     Time.reset();
00134     Time.start();
00135 
00136     CommandReader cmd;
00137     
00138     //int detailMsg=9;    
00139     while (true) {
00140         int t = Time.read_ms();
00141 //if(--detailMsg>0)CmdSerial.printf("%d\r\n",t);
00142         char code;
00143         int val;
00144         int stat = cmd.get(code,val);
00145 
00146         if (stat)
00147         {
00148             prevCommandTime = t;
00149             if (nMsg>0){nMsg--;DiagSerial.printf("\r\n\t\tcmd>%c%d\r\n",code,val);}
00150 
00151             switch(code)
00152             {
00153             case 'L': MotL.setSpeed(val/255.0,t); break;
00154             case 'R': MotR.setSpeed(val/255.0,t); break;
00155             default : 
00156                 DiagSerial.printf("Unidentified command \"%c%d\" (stop)",code,val);
00157                 MotL.stop();
00158                 MotR.stop();
00159             }
00160 
00161 //CmdSerial.puts("\nrcd\r");
00162 //CmdSerial.puts("\r\n");
00163 //detailMsg=2;
00164 //CmdSerial.putc('\n');
00165         }
00166         else
00167         { // no command, do housekeeping (misc state update stuff)
00168             MotL.update(t);
00169             MotR.update(t);
00170 
00171             if ((prevCommandTime > 0x0fffff00) && (t < 999))
00172             {  // time counter is close to wrapping around.  make sure this does not happen.
00173                // I think we can tolerate a minor glitch once every 24.8 days of continuous use
00174                prevCommandTime = tFlash = 0;
00175                Time.reset();
00176                Time.start();
00177                CmdSerial.puts("\r\nClock wrap-around\r\n");
00178                while(Time.read() < 1);
00179                t = 1;
00180             }
00181         
00182             if (t - tFlash > FLASH_DT)
00183             { // Flash standard LED to show things are running
00184                 tFlash = t;
00185                 DiagSerial.printf("dt=%d\r",t);
00186                 toggleFlash();
00187                 //reportCurrent();
00188                 //wait(0.8f);
00189             }
00190         }
00191     }
00192 }