Diff: main.cpp

Revision:

0:95384d72794f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 07 05:20:37 2018 +0000
@@ -0,0 +1,310 @@
+#include "mbed.h"
+#include "BufferedSerial.h"
+#include "PidController.h"
+#include "DRV8833.h"
+#include "RGB_LED.h"
+
+#define WHEELCIRC 1.0f              //-- Circumference of drive wheel for speed calc
+#define PULSES_PER_REV 1806.96f     //-- Pulses per revolution for encoder
+
+struct messageToSend {
+    bool Status;
+    bool Position;
+    bool LeftMotorPID;
+    bool RightMotorPID;
+    bool TurningPID;
+    bool WatchDog;
+} ;
+
+
+//#############################################################################
+// -- IO Definitions
+//#############################################################################
+
+DigitalIn mybutton(USER_BUTTON);
+DigitalOut led(LED1);
+
+BufferedSerial gui(PC_10, PC_11);
+//Serial gui(USBTX, USBRX);
+
+
+//#############################################################################
+// -- Class Objects
+//#############################################################################
+
+PidController leftMtrPID;
+PidController rightMtrPID;
+RGB_LED Lights1(PA_5, PA_6, PA_7);
+RGB_LED Lights2(PB_6, PC_7, PA_9);
+DRV8833 LeftMotor(PC_6, PC_8, PC_4, PA_12, PULSES_PER_REV);
+DRV8833 RightMotor(PC_9, PB_8, PC_5, PA_11, PULSES_PER_REV);
+
+Ticker driveTicker;
+
+//#############################################################################
+// -- Global Variables
+//#############################################################################
+char txmsg[80];
+char rxMSG[80];
+char statusMSG[28]; //-- 
+
+
+int msgcounter;     //Used for trigger various message send requests
+messageToSend sendRequests;
+bool EnableDrive;
+bool pidtuneL, pidtuneR;
+float ManualSpeedCmd;
+bool DriveAutomaticMode;
+bool BrakeMotorReq;
+int LeftLastEncCount, RightLastEncCount;    //-- Last recorded encoder count for speed calc
+
+
+//#############################################################################
+// -- Function Prototypes
+//#############################################################################
+void driveSpeedISR(void);
+float CalcDriveSpeed(int currentCount, int lastCount);
+void messageManager(void);
+uint16_t ByteArrayToUInt(char *Buffer, uint8_t startAdd);
+int16_t ByteArrayToInt(char *Buffer, uint8_t startAdd);
+float ByteArrayToFloat(char *Buffer, uint8_t startAdd);
+
+
+//#############################################################################
+// -- Main Program
+//#############################################################################
+int main()
+{
+
+    gui.baud(115200);           //--ESP Operates at Higher Baud Rate
+    driveTicker.attach(callback(&driveSpeedISR), 0.1);  //-- Call every 100ms
+    
+    led = 1;                    //-- Simple check to see that program downloaded and started correctly
+    
+    //-- Initialise global vars
+    EnableDrive = false;
+    ManualSpeedCmd = 0.0;
+    DriveAutomaticMode = false;
+    BrakeMotorReq = false;
+    pidtuneL = pidtuneR = false;
+    LeftLastEncCount =  RightLastEncCount = 0;
+    
+    gui.printf("Start\n");
+    while(1) {
+        //led = feedback;
+        if (!mybutton) {
+            gui.printf("Bttn\n");
+        }
+        
+        // -- Message Manager to ensure that messages being sent don't overwrite each other
+        messageManager();        
+        
+        // -- enough information for new command
+        if (gui.canReadLine()) {
+            gui.readLine(rxMSG);
+            switch (rxMSG[0]) {
+                case 'W':
+                    sendRequests.WatchDog = true;
+                    break;
+                    
+                case 'C':
+                    EnableDrive = (rxMSG[2] > 0) ? true : false;
+                    ManualSpeedCmd = ByteArrayToFloat(rxMSG, 3);
+                    switch(rxMSG[1])
+                    {
+                        case 'A': DriveAutomaticMode = true;
+                                leftMtrPID.EndDiag();
+                                rightMtrPID.EndDiag();
+                                break;
+                                
+                        case 'M': DriveAutomaticMode = false;
+                                leftMtrPID.EndDiag();
+                                rightMtrPID.EndDiag();
+                                break;
+                                
+                        case 'L': if (!pidtuneL)
+                                {
+                                    leftMtrPID.StartDiag();
+                                    pidtuneL = true;
+                                    }
+                                break;
+                                
+                        case 'R': if (!pidtuneR)
+                                {
+                                    rightMtrPID.StartDiag();
+                                    pidtuneR = true;
+                                    }
+                                break;
+                        }
+                    break;
+
+                case 'L':
+                    leftMtrPID.UpdateSettings(0.0, ByteArrayToFloat(rxMSG, 13), ByteArrayToFloat(rxMSG, 17), ByteArrayToFloat(rxMSG, 21), ByteArrayToFloat(rxMSG, 5), ByteArrayToFloat(rxMSG, 9));
+                    break;
+                    
+                
+                case 'R':
+                    rightMtrPID.UpdateSettings(0.0, ByteArrayToFloat(rxMSG, 13), ByteArrayToFloat(rxMSG, 17), ByteArrayToFloat(rxMSG, 21), ByteArrayToFloat(rxMSG, 5), ByteArrayToFloat(rxMSG, 9));
+                    break;   
+                    
+//                case 'T':
+//                    bearingPID.UpdateSettings(0.0, ByteArrayToFloat(rxMSG, 13), ByteArrayToFloat(rxMSG, 17), ByteArrayToFloat(rxMSG, 21), ByteArrayToFloat(rxMSG, 5), ByteArrayToFloat(rxMSG, 9));
+//                    break;                
+            }
+        }
+    }// -- End of While
+}  //-- End of Main
+
+
+//#############################################################################
+// Attatch this to ticker interupt. Deals with all aspects of the Drive Wheels
+//#############################################################################
+
+void driveSpeedISR(void)
+{
+    float leftSpeed, rightSpeed;
+    int leftPwm, rightPwm;
+    
+    //-- Get Current Speeds 
+    leftSpeed = CalcDriveSpeed(LeftMotor.getCount(), LeftLastEncCount);
+    rightSpeed = CalcDriveSpeed(RightMotor.getCount(), RightLastEncCount);
+    
+    //-- Store Last Count to GLOBAL variabels
+    LeftLastEncCount = LeftMotor.getCount();
+    RightLastEncCount = RightMotor.getCount();
+    
+    
+    
+    //-- Shiloh You will need todo some code here about the AUTO speed
+    //float speedSetpoint;
+   // if (DriveAutomaticMode) speedSetpoint = AutoSpeed;
+    //else speedSetpoint = ManualSpeedCmd;
+    
+    
+    //-- PID 
+    leftPwm = (int)(leftMtrPID.Calculate(ManualSpeedCmd, leftSpeed));
+    rightPwm = (int)(rightMtrPID.Calculate(ManualSpeedCmd, rightSpeed));
+    
+    
+    //-- Outputs to motors
+    if (BrakeMotorReq)
+    {
+        LeftMotor.brake();
+        RightMotor.brake();
+        }
+    else
+    {
+        if (leftPwm > 0) {LeftMotor.forward(leftPwm);}
+        else if (leftPwm < 0) {LeftMotor.reverse(leftPwm);}
+        else LeftMotor.stop();
+    
+        if (rightPwm > 0) {RightMotor.forward(rightPwm);}
+        else if (rightPwm < 0) {RightMotor.reverse(rightPwm);}
+        else RightMotor.stop();
+        }
+    return;
+}
+
+//#############################################################################
+// -- Calculates the Current Wheel Speed (in mm/s)
+//#############################################################################
+float CalcDriveSpeed(int currentCount, int lastCount)
+{
+    float deltaCount = currentCount - lastCount;
+
+    //-- In the event that your just getting flicker send back 0
+    if (abs(deltaCount) < 10) {
+        return 0.0f;
+    } else {
+        //_actualDistance += 109.956f*(deltaCount/PULSES_PER_REV);
+        return WHEELCIRC*10*(deltaCount/PULSES_PER_REV);                            // = Wheel Circ in mm multiply by 10 (as this is called every 100ms)
+    }
+}
+
+//#############################################################################
+// To manage the sending of messages
+//#############################################################################
+void messageManager(void)
+{
+    // **The order that theses IF statements are layed out defines the priority of message
+    if (sendRequests.WatchDog) {
+        gui.printf("W\n");
+        sendRequests.WatchDog = false;
+        return;
+    }
+    
+    if (sendRequests.Status) {
+        gui.printf(statusMSG);
+        sendRequests.Status = false;
+        return;
+        }
+        
+    if (sendRequests.Position) {
+ //       gui.printf("W\n");
+        sendRequests.Position = false;
+        return;
+    }
+    
+    if (sendRequests.LeftMotorPID) {
+        char temp[30];
+        temp[0] = 'L';
+        leftMtrPID.GetDiagnosticsMessage(temp+1);
+        temp[27]='\r';
+        temp[28]='\n';
+        gui.printf(temp);
+        sendRequests.LeftMotorPID = false;
+        return;
+    }
+    
+    if (sendRequests.RightMotorPID) {
+        char temp[30];
+        temp[0] = 'R';
+        rightMtrPID.GetDiagnosticsMessage(temp+1);
+        temp[27]='\r';
+        temp[28]='\n';
+        gui.printf(temp);
+        sendRequests.RightMotorPID = false;
+        return;
+        }
+        
+    if (sendRequests.TurningPID) {
+//        char[30] temp;
+//        temp[0] = "T";
+//        leftMtrPID.GetDiagnosticsMessage(temp+1)
+//        temp[27]='\r';
+//        temp[28]='\n';
+//        gui.printf(temp);
+        sendRequests.TurningPID = false;
+        return;
+    }
+}
+
+//#############################################################################
+// helper functions
+//#############################################################################
+//-- Convert byte array (From C# app) to Unsigned Integer
+uint16_t ByteArrayToUInt(char *Buffer, uint8_t startAdd)
+{
+  uint16_t temp = Buffer[startAdd+1];
+  temp = (temp<<8) | Buffer[startAdd];
+  return temp; 
+  }
+
+//-- Convert byte array (From C# app) to Signed Integer
+int16_t ByteArrayToInt(char *Buffer, uint8_t startAdd)
+{
+  int16_t temp = Buffer[startAdd+1];
+  temp = (temp<<8) | Buffer[startAdd];
+  return temp; 
+  }
+
+//-- Convert byte array (From C# app) to Float
+float ByteArrayToFloat(char *Buffer, uint8_t startAdd)
+{
+  char temp[4]; 
+  temp[0]= Buffer[startAdd];
+  temp[1]= Buffer[startAdd+1];
+  temp[2]= Buffer[startAdd+2];
+  temp[3]= Buffer[startAdd+3];
+  return *((float*)(temp));;
+  }
\ No newline at end of file