File content as of revision 0:95384d72794f:

#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));;
  }