Diff: main.cpp

Revision:

0:41ca27337c2b

Child:

1:23d0a615756a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jul 28 14:59:19 2015 +0000
@@ -0,0 +1,172 @@
+// Firmware to demonstrate typical "Tank-Drive" robot motor control.
+// one left motor, and one right motor.
+//
+// Can do simple commanding from a bluetooth terminal (like Android's BlueTerm),
+// or from the Android app used in http://www.instructables.com/id/Simple-RC-car-for-beginners-Android-control-over-/
+//
+// The FreeScale FRDM-K64F is a much more powerful board than needed for this task,
+// but it is a good template for a typical "tank-drive" robot based on the FRDM-K64F.
+
+#include "mbed.h"
+
+//DigitalOut gpo(D0);
+//DigitalOut led(LED_RED);
+//PwmOut ENA( PTD1);   // D13 on Arduino Shield
+//PwmOut IN1A(PTD3);  // D12 on Arduino Shield
+//PwmOut IN2A(PTD2);  // D11 on Arduino Shield
+//PwmOut ENB( PTD0);   // D13 on Arduino Shield
+//PwmOut IN1B(PTC4);  // D12 on Arduino Shield
+//PwmOut IN2B(PTA0);  // D11 on Arduino Shield
+
+Timer Time;
+inline int millis() {return(Time.read_ms());} // mimic Arduino millis() function
+
+// Tried to inherit/polymorph serial capabilities...  but could
+// not get to compile... or get access to mbed::stream capabilities.
+// I know this is sloppy...  but I'm just going to make a global
+// Serial, and let otherclasses have a reference to it.
+#include "Serial.h"
+//Serial CmdSerial(PTC17,PTC16);  // Command/Diagnostic serial port on UART3, sicne I don't know how to use USB ports (yet)
+Serial CmdSerial(PTC15,PTC14);  // Command/Diagnostic serial port on "bluetooth add-on" header
+
+#include "ASerial.h"  // emulation of some common Arduino Serial methods
+ASerial cSerial(CmdSerial); 
+
+// Set up motor drive for left and right motors
+#include "MotorDriveDBH1.h"
+MotorDrive MotL(PTD1,PTD3,PTD2,PTB2);
+MotorDrive MotR(PTD0,PTC4,PTA0,PTB3);
+
+#include "Command.h"
+
+void initMotorDrive(MotorDrive &md)
+{
+    md.setCommandTimeout(5000);  // ms between commands before automatic emergency stop
+    //ms.setPWMfreqHz(8000);
+    
+    // these should be the defaults
+    //md.setStartupTime(5);  // full power pulse this long when starting from full STOP
+    //md.setStopDeadTime(3000); // wait this many ms after emergency STOP before starting up again
+    //md.setMinPWM(0.004f);  // any PWM command below this istreated as 0
+    //md.setMaxPWM(0.98f);  // these drives can fail if attempt to run full-100%
+    //md.setDecel(0.01f);  // deceleration rate on STOP.  This frac/ms
+    
+}
+// make sure deadman release stops immediately
+void deadmanReleased()
+{
+  MotR.emergencyStop();
+  MotL.emergencyStop();
+  cSerial.println("STOP");
+}
+
+// Since this board has fancy tri-color LED, let's sequence it
+// instead of a boring old flash for a heartbeat
+DigitalOut ledR(LED_RED);
+DigitalOut ledG(LED_GREEN);
+DigitalOut ledB(LED_BLUE);
+void toggleFlash()
+{
+    static int k=0;
+    k++;
+    if ((k<0) || (k>7)) k=0;
+    // Gray code counter...
+    switch(k)
+    {
+        case 1:
+        case 5:  ledG = !ledG; break;
+        case 3:
+        case 7:  ledB = !ledB; break;
+        default: ledR = !ledR; break;
+    }
+}
+
+void reportCurrent()
+{
+  float cr, cl;
+  cr = MotR.getCurrent();
+  cl = MotL.getCurrent();
+  cSerial.print("Current:  left=");cSerial.print(cl);
+  cSerial.print("   right=");cSerial.println(cr);
+}
+
+//void dumpSerialChar()
+//{
+//  int i = cSerial.getc();
+//  CmdSerial.printf("%d %c\n",i,i);    
+//}
+    
+// ================================================== main
+
+int prevCommandTime=0;
+
+#define FLASH_DT 800
+int tFlash = 0;
+
+// for diagnostics, just print a few messages, then be quiet to improve
+// performance when in actual use.
+int nMsg = 99;
+
+int main()
+{
+
+    // Set motor drive parameters
+    initMotorDrive(MotL);
+    initMotorDrive(MotR);
+
+    CmdSerial.baud(57600);
+    CmdSerial.puts("\r\nTankDrive for K64F with Bluetooth\r\n\n");
+    
+    Time.reset();
+    Time.start();
+
+    CommandReader cmd(cSerial);
+        
+    while (true) {
+        int t = Time.read_ms();
+
+        char code;
+        int val;
+        int stat = cmd.get(code,val);
+        if (stat)
+        {
+            prevCommandTime = t;
+            if (nMsg>0){nMsg--;CmdSerial.printf(">%c%d\r\n",code,val);}
+/*
+            switch(code)
+            {
+            case 'L': MotL.setSpeed(val,t); break;
+            case 'R': MotR.setSpeed(val,t); break;
+            default : 
+                MotL.setSpeed(0,t);  // odd command.  just stop
+                MotR.setSpeed(0,t);
+                CmdSerial.puts("<stop\r\n");
+            }
+*/
+        }
+        else
+        { // no command, do housekeeping (misc state update stuff)
+            //MotL.update(t);
+            //MotR.update(t);
+
+            if ((prevCommandTime > 0x0fffff00) && (t < 999))
+            {  // time counter is close to wrapping around.  make sure this does not happen.
+               // I think we can tolerate a minor glitch once every 24.8 days of continuous use
+               prevCommandTime = tFlash = 0;
+               Time.reset();
+               Time.start();
+               while(Time.read() < 1);
+               t = 1;
+            }
+        
+            if (t - tFlash > FLASH_DT)
+            { // Flash standard LED to show things are running
+                tFlash = t;
+                CmdSerial.printf("dt=%d\r\n",t);
+                toggleFlash();
+                //reportCurrent();
+                //wait(0.8f);
+            }
+        }
+    }
+}