Sustainable Robotic Logistics / Mbed 2 deprecated ApexPID

Main Code

Dependencies:   DRV8833 PidControllerV3 mbed Buffer

Fork of ApexPID by James Batchelar

main.cpp@0:95384d72794f, 2018-05-07 (annotated)

Committer:
batchee7
Date:
Mon May 07 05:20:37 2018 +0000
Revision:
0:95384d72794f
IntialRelease

Who changed what in which revision?

UserRevisionLine numberNew contents of line
batchee7 0:95384d72794f 1 #include "mbed.h"
batchee7 0:95384d72794f 2 #include "BufferedSerial.h"
batchee7 0:95384d72794f 3 #include "PidController.h"
batchee7 0:95384d72794f 4 #include "DRV8833.h"
batchee7 0:95384d72794f 5 #include "RGB_LED.h"
batchee7 0:95384d72794f 6
batchee7 0:95384d72794f 7 #define WHEELCIRC 1.0f //-- Circumference of drive wheel for speed calc
batchee7 0:95384d72794f 8 #define PULSES_PER_REV 1806.96f //-- Pulses per revolution for encoder
batchee7 0:95384d72794f 9
batchee7 0:95384d72794f 10 struct messageToSend {
batchee7 0:95384d72794f 11 bool Status;
batchee7 0:95384d72794f 12 bool Position;
batchee7 0:95384d72794f 13 bool LeftMotorPID;
batchee7 0:95384d72794f 14 bool RightMotorPID;
batchee7 0:95384d72794f 15 bool TurningPID;
batchee7 0:95384d72794f 16 bool WatchDog;
batchee7 0:95384d72794f 17 } ;
batchee7 0:95384d72794f 18
batchee7 0:95384d72794f 19
batchee7 0:95384d72794f 20 //#############################################################################
batchee7 0:95384d72794f 21 // -- IO Definitions
batchee7 0:95384d72794f 22 //#############################################################################
batchee7 0:95384d72794f 23
batchee7 0:95384d72794f 24 DigitalIn mybutton(USER_BUTTON);
batchee7 0:95384d72794f 25 DigitalOut led(LED1);
batchee7 0:95384d72794f 26
batchee7 0:95384d72794f 27 BufferedSerial gui(PC_10, PC_11);
batchee7 0:95384d72794f 28 //Serial gui(USBTX, USBRX);
batchee7 0:95384d72794f 29
batchee7 0:95384d72794f 30
batchee7 0:95384d72794f 31 //#############################################################################
batchee7 0:95384d72794f 32 // -- Class Objects
batchee7 0:95384d72794f 33 //#############################################################################
batchee7 0:95384d72794f 34
batchee7 0:95384d72794f 35 PidController leftMtrPID;
batchee7 0:95384d72794f 36 PidController rightMtrPID;
batchee7 0:95384d72794f 37 RGB_LED Lights1(PA_5, PA_6, PA_7);
batchee7 0:95384d72794f 38 RGB_LED Lights2(PB_6, PC_7, PA_9);
batchee7 0:95384d72794f 39 DRV8833 LeftMotor(PC_6, PC_8, PC_4, PA_12, PULSES_PER_REV);
batchee7 0:95384d72794f 40 DRV8833 RightMotor(PC_9, PB_8, PC_5, PA_11, PULSES_PER_REV);
batchee7 0:95384d72794f 41
batchee7 0:95384d72794f 42 Ticker driveTicker;
batchee7 0:95384d72794f 43
batchee7 0:95384d72794f 44 //#############################################################################
batchee7 0:95384d72794f 45 // -- Global Variables
batchee7 0:95384d72794f 46 //#############################################################################
batchee7 0:95384d72794f 47 char txmsg[80];
batchee7 0:95384d72794f 48 char rxMSG[80];
batchee7 0:95384d72794f 49 char statusMSG[28]; //--
batchee7 0:95384d72794f 50
batchee7 0:95384d72794f 51
batchee7 0:95384d72794f 52 int msgcounter; //Used for trigger various message send requests
batchee7 0:95384d72794f 53 messageToSend sendRequests;
batchee7 0:95384d72794f 54 bool EnableDrive;
batchee7 0:95384d72794f 55 bool pidtuneL, pidtuneR;
batchee7 0:95384d72794f 56 float ManualSpeedCmd;
batchee7 0:95384d72794f 57 bool DriveAutomaticMode;
batchee7 0:95384d72794f 58 bool BrakeMotorReq;
batchee7 0:95384d72794f 59 int LeftLastEncCount, RightLastEncCount; //-- Last recorded encoder count for speed calc
batchee7 0:95384d72794f 60
batchee7 0:95384d72794f 61
batchee7 0:95384d72794f 62 //#############################################################################
batchee7 0:95384d72794f 63 // -- Function Prototypes
batchee7 0:95384d72794f 64 //#############################################################################
batchee7 0:95384d72794f 65 void driveSpeedISR(void);
batchee7 0:95384d72794f 66 float CalcDriveSpeed(int currentCount, int lastCount);
batchee7 0:95384d72794f 67 void messageManager(void);
batchee7 0:95384d72794f 68 uint16_t ByteArrayToUInt(char *Buffer, uint8_t startAdd);
batchee7 0:95384d72794f 69 int16_t ByteArrayToInt(char *Buffer, uint8_t startAdd);
batchee7 0:95384d72794f 70 float ByteArrayToFloat(char *Buffer, uint8_t startAdd);
batchee7 0:95384d72794f 71
batchee7 0:95384d72794f 72
batchee7 0:95384d72794f 73 //#############################################################################
batchee7 0:95384d72794f 74 // -- Main Program
batchee7 0:95384d72794f 75 //#############################################################################
batchee7 0:95384d72794f 76 int main()
batchee7 0:95384d72794f 77 {
batchee7 0:95384d72794f 78
batchee7 0:95384d72794f 79 gui.baud(115200); //--ESP Operates at Higher Baud Rate
batchee7 0:95384d72794f 80 driveTicker.attach(callback(&driveSpeedISR), 0.1); //-- Call every 100ms
batchee7 0:95384d72794f 81
batchee7 0:95384d72794f 82 led = 1; //-- Simple check to see that program downloaded and started correctly
batchee7 0:95384d72794f 83
batchee7 0:95384d72794f 84 //-- Initialise global vars
batchee7 0:95384d72794f 85 EnableDrive = false;
batchee7 0:95384d72794f 86 ManualSpeedCmd = 0.0;
batchee7 0:95384d72794f 87 DriveAutomaticMode = false;
batchee7 0:95384d72794f 88 BrakeMotorReq = false;
batchee7 0:95384d72794f 89 pidtuneL = pidtuneR = false;
batchee7 0:95384d72794f 90 LeftLastEncCount = RightLastEncCount = 0;
batchee7 0:95384d72794f 91
batchee7 0:95384d72794f 92 gui.printf("Start\n");
batchee7 0:95384d72794f 93 while(1) {
batchee7 0:95384d72794f 94 //led = feedback;
batchee7 0:95384d72794f 95 if (!mybutton) {
batchee7 0:95384d72794f 96 gui.printf("Bttn\n");
batchee7 0:95384d72794f 97 }
batchee7 0:95384d72794f 98
batchee7 0:95384d72794f 99 // -- Message Manager to ensure that messages being sent don't overwrite each other
batchee7 0:95384d72794f 100 messageManager();
batchee7 0:95384d72794f 101
batchee7 0:95384d72794f 102 // -- enough information for new command
batchee7 0:95384d72794f 103 if (gui.canReadLine()) {
batchee7 0:95384d72794f 104 gui.readLine(rxMSG);
batchee7 0:95384d72794f 105 switch (rxMSG[0]) {
batchee7 0:95384d72794f 106 case 'W':
batchee7 0:95384d72794f 107 sendRequests.WatchDog = true;
batchee7 0:95384d72794f 108 break;
batchee7 0:95384d72794f 109
batchee7 0:95384d72794f 110 case 'C':
batchee7 0:95384d72794f 111 EnableDrive = (rxMSG[2] > 0) ? true : false;
batchee7 0:95384d72794f 112 ManualSpeedCmd = ByteArrayToFloat(rxMSG, 3);
batchee7 0:95384d72794f 113 switch(rxMSG[1])
batchee7 0:95384d72794f 114 {
batchee7 0:95384d72794f 115 case 'A': DriveAutomaticMode = true;
batchee7 0:95384d72794f 116 leftMtrPID.EndDiag();
batchee7 0:95384d72794f 117 rightMtrPID.EndDiag();
batchee7 0:95384d72794f 118 break;
batchee7 0:95384d72794f 119
batchee7 0:95384d72794f 120 case 'M': DriveAutomaticMode = false;
batchee7 0:95384d72794f 121 leftMtrPID.EndDiag();
batchee7 0:95384d72794f 122 rightMtrPID.EndDiag();
batchee7 0:95384d72794f 123 break;
batchee7 0:95384d72794f 124
batchee7 0:95384d72794f 125 case 'L': if (!pidtuneL)
batchee7 0:95384d72794f 126 {
batchee7 0:95384d72794f 127 leftMtrPID.StartDiag();
batchee7 0:95384d72794f 128 pidtuneL = true;
batchee7 0:95384d72794f 129 }
batchee7 0:95384d72794f 130 break;
batchee7 0:95384d72794f 131
batchee7 0:95384d72794f 132 case 'R': if (!pidtuneR)
batchee7 0:95384d72794f 133 {
batchee7 0:95384d72794f 134 rightMtrPID.StartDiag();
batchee7 0:95384d72794f 135 pidtuneR = true;
batchee7 0:95384d72794f 136 }
batchee7 0:95384d72794f 137 break;
batchee7 0:95384d72794f 138 }
batchee7 0:95384d72794f 139 break;
batchee7 0:95384d72794f 140
batchee7 0:95384d72794f 141 case 'L':
batchee7 0:95384d72794f 142 leftMtrPID.UpdateSettings(0.0, ByteArrayToFloat(rxMSG, 13), ByteArrayToFloat(rxMSG, 17), ByteArrayToFloat(rxMSG, 21), ByteArrayToFloat(rxMSG, 5), ByteArrayToFloat(rxMSG, 9));
batchee7 0:95384d72794f 143 break;
batchee7 0:95384d72794f 144
batchee7 0:95384d72794f 145
batchee7 0:95384d72794f 146 case 'R':
batchee7 0:95384d72794f 147 rightMtrPID.UpdateSettings(0.0, ByteArrayToFloat(rxMSG, 13), ByteArrayToFloat(rxMSG, 17), ByteArrayToFloat(rxMSG, 21), ByteArrayToFloat(rxMSG, 5), ByteArrayToFloat(rxMSG, 9));
batchee7 0:95384d72794f 148 break;
batchee7 0:95384d72794f 149
batchee7 0:95384d72794f 150 // case 'T':
batchee7 0:95384d72794f 151 // bearingPID.UpdateSettings(0.0, ByteArrayToFloat(rxMSG, 13), ByteArrayToFloat(rxMSG, 17), ByteArrayToFloat(rxMSG, 21), ByteArrayToFloat(rxMSG, 5), ByteArrayToFloat(rxMSG, 9));
batchee7 0:95384d72794f 152 // break;
batchee7 0:95384d72794f 153 }
batchee7 0:95384d72794f 154 }
batchee7 0:95384d72794f 155 }// -- End of While
batchee7 0:95384d72794f 156 } //-- End of Main
batchee7 0:95384d72794f 157
batchee7 0:95384d72794f 158
batchee7 0:95384d72794f 159 //#############################################################################
batchee7 0:95384d72794f 160 // Attatch this to ticker interupt. Deals with all aspects of the Drive Wheels
batchee7 0:95384d72794f 161 //#############################################################################
batchee7 0:95384d72794f 162
batchee7 0:95384d72794f 163 void driveSpeedISR(void)
batchee7 0:95384d72794f 164 {
batchee7 0:95384d72794f 165 float leftSpeed, rightSpeed;
batchee7 0:95384d72794f 166 int leftPwm, rightPwm;
batchee7 0:95384d72794f 167
batchee7 0:95384d72794f 168 //-- Get Current Speeds
batchee7 0:95384d72794f 169 leftSpeed = CalcDriveSpeed(LeftMotor.getCount(), LeftLastEncCount);
batchee7 0:95384d72794f 170 rightSpeed = CalcDriveSpeed(RightMotor.getCount(), RightLastEncCount);
batchee7 0:95384d72794f 171
batchee7 0:95384d72794f 172 //-- Store Last Count to GLOBAL variabels
batchee7 0:95384d72794f 173 LeftLastEncCount = LeftMotor.getCount();
batchee7 0:95384d72794f 174 RightLastEncCount = RightMotor.getCount();
batchee7 0:95384d72794f 175
batchee7 0:95384d72794f 176
batchee7 0:95384d72794f 177
batchee7 0:95384d72794f 178 //-- Shiloh You will need todo some code here about the AUTO speed
batchee7 0:95384d72794f 179 //float speedSetpoint;
batchee7 0:95384d72794f 180 // if (DriveAutomaticMode) speedSetpoint = AutoSpeed;
batchee7 0:95384d72794f 181 //else speedSetpoint = ManualSpeedCmd;
batchee7 0:95384d72794f 182
batchee7 0:95384d72794f 183
batchee7 0:95384d72794f 184 //-- PID
batchee7 0:95384d72794f 185 leftPwm = (int)(leftMtrPID.Calculate(ManualSpeedCmd, leftSpeed));
batchee7 0:95384d72794f 186 rightPwm = (int)(rightMtrPID.Calculate(ManualSpeedCmd, rightSpeed));
batchee7 0:95384d72794f 187
batchee7 0:95384d72794f 188
batchee7 0:95384d72794f 189 //-- Outputs to motors
batchee7 0:95384d72794f 190 if (BrakeMotorReq)
batchee7 0:95384d72794f 191 {
batchee7 0:95384d72794f 192 LeftMotor.brake();
batchee7 0:95384d72794f 193 RightMotor.brake();
batchee7 0:95384d72794f 194 }
batchee7 0:95384d72794f 195 else
batchee7 0:95384d72794f 196 {
batchee7 0:95384d72794f 197 if (leftPwm > 0) {LeftMotor.forward(leftPwm);}
batchee7 0:95384d72794f 198 else if (leftPwm < 0) {LeftMotor.reverse(leftPwm);}
batchee7 0:95384d72794f 199 else LeftMotor.stop();
batchee7 0:95384d72794f 200
batchee7 0:95384d72794f 201 if (rightPwm > 0) {RightMotor.forward(rightPwm);}
batchee7 0:95384d72794f 202 else if (rightPwm < 0) {RightMotor.reverse(rightPwm);}
batchee7 0:95384d72794f 203 else RightMotor.stop();
batchee7 0:95384d72794f 204 }
batchee7 0:95384d72794f 205 return;
batchee7 0:95384d72794f 206 }
batchee7 0:95384d72794f 207
batchee7 0:95384d72794f 208 //#############################################################################
batchee7 0:95384d72794f 209 // -- Calculates the Current Wheel Speed (in mm/s)
batchee7 0:95384d72794f 210 //#############################################################################
batchee7 0:95384d72794f 211 float CalcDriveSpeed(int currentCount, int lastCount)
batchee7 0:95384d72794f 212 {
batchee7 0:95384d72794f 213 float deltaCount = currentCount - lastCount;
batchee7 0:95384d72794f 214
batchee7 0:95384d72794f 215 //-- In the event that your just getting flicker send back 0
batchee7 0:95384d72794f 216 if (abs(deltaCount) < 10) {
batchee7 0:95384d72794f 217 return 0.0f;
batchee7 0:95384d72794f 218 } else {
batchee7 0:95384d72794f 219 //_actualDistance += 109.956f*(deltaCount/PULSES_PER_REV);
batchee7 0:95384d72794f 220 return WHEELCIRC*10*(deltaCount/PULSES_PER_REV); // = Wheel Circ in mm multiply by 10 (as this is called every 100ms)
batchee7 0:95384d72794f 221 }
batchee7 0:95384d72794f 222 }
batchee7 0:95384d72794f 223
batchee7 0:95384d72794f 224 //#############################################################################
batchee7 0:95384d72794f 225 // To manage the sending of messages
batchee7 0:95384d72794f 226 //#############################################################################
batchee7 0:95384d72794f 227 void messageManager(void)
batchee7 0:95384d72794f 228 {
batchee7 0:95384d72794f 229 // **The order that theses IF statements are layed out defines the priority of message
batchee7 0:95384d72794f 230 if (sendRequests.WatchDog) {
batchee7 0:95384d72794f 231 gui.printf("W\n");
batchee7 0:95384d72794f 232 sendRequests.WatchDog = false;
batchee7 0:95384d72794f 233 return;
batchee7 0:95384d72794f 234 }
batchee7 0:95384d72794f 235
batchee7 0:95384d72794f 236 if (sendRequests.Status) {
batchee7 0:95384d72794f 237 gui.printf(statusMSG);
batchee7 0:95384d72794f 238 sendRequests.Status = false;
batchee7 0:95384d72794f 239 return;
batchee7 0:95384d72794f 240 }
batchee7 0:95384d72794f 241
batchee7 0:95384d72794f 242 if (sendRequests.Position) {
batchee7 0:95384d72794f 243 // gui.printf("W\n");
batchee7 0:95384d72794f 244 sendRequests.Position = false;
batchee7 0:95384d72794f 245 return;
batchee7 0:95384d72794f 246 }
batchee7 0:95384d72794f 247
batchee7 0:95384d72794f 248 if (sendRequests.LeftMotorPID) {
batchee7 0:95384d72794f 249 char temp[30];
batchee7 0:95384d72794f 250 temp[0] = 'L';
batchee7 0:95384d72794f 251 leftMtrPID.GetDiagnosticsMessage(temp+1);
batchee7 0:95384d72794f 252 temp[27]='\r';
batchee7 0:95384d72794f 253 temp[28]='\n';
batchee7 0:95384d72794f 254 gui.printf(temp);
batchee7 0:95384d72794f 255 sendRequests.LeftMotorPID = false;
batchee7 0:95384d72794f 256 return;
batchee7 0:95384d72794f 257 }
batchee7 0:95384d72794f 258
batchee7 0:95384d72794f 259 if (sendRequests.RightMotorPID) {
batchee7 0:95384d72794f 260 char temp[30];
batchee7 0:95384d72794f 261 temp[0] = 'R';
batchee7 0:95384d72794f 262 rightMtrPID.GetDiagnosticsMessage(temp+1);
batchee7 0:95384d72794f 263 temp[27]='\r';
batchee7 0:95384d72794f 264 temp[28]='\n';
batchee7 0:95384d72794f 265 gui.printf(temp);
batchee7 0:95384d72794f 266 sendRequests.RightMotorPID = false;
batchee7 0:95384d72794f 267 return;
batchee7 0:95384d72794f 268 }
batchee7 0:95384d72794f 269
batchee7 0:95384d72794f 270 if (sendRequests.TurningPID) {
batchee7 0:95384d72794f 271 // char[30] temp;
batchee7 0:95384d72794f 272 // temp[0] = "T";
batchee7 0:95384d72794f 273 // leftMtrPID.GetDiagnosticsMessage(temp+1)
batchee7 0:95384d72794f 274 // temp[27]='\r';
batchee7 0:95384d72794f 275 // temp[28]='\n';
batchee7 0:95384d72794f 276 // gui.printf(temp);
batchee7 0:95384d72794f 277 sendRequests.TurningPID = false;
batchee7 0:95384d72794f 278 return;
batchee7 0:95384d72794f 279 }
batchee7 0:95384d72794f 280 }
batchee7 0:95384d72794f 281
batchee7 0:95384d72794f 282 //#############################################################################
batchee7 0:95384d72794f 283 // helper functions
batchee7 0:95384d72794f 284 //#############################################################################
batchee7 0:95384d72794f 285 //-- Convert byte array (From C# app) to Unsigned Integer
batchee7 0:95384d72794f 286 uint16_t ByteArrayToUInt(char *Buffer, uint8_t startAdd)
batchee7 0:95384d72794f 287 {
batchee7 0:95384d72794f 288 uint16_t temp = Buffer[startAdd+1];
batchee7 0:95384d72794f 289 temp = (temp<<8) | Buffer[startAdd];
batchee7 0:95384d72794f 290 return temp;
batchee7 0:95384d72794f 291 }
batchee7 0:95384d72794f 292
batchee7 0:95384d72794f 293 //-- Convert byte array (From C# app) to Signed Integer
batchee7 0:95384d72794f 294 int16_t ByteArrayToInt(char *Buffer, uint8_t startAdd)
batchee7 0:95384d72794f 295 {
batchee7 0:95384d72794f 296 int16_t temp = Buffer[startAdd+1];
batchee7 0:95384d72794f 297 temp = (temp<<8) | Buffer[startAdd];
batchee7 0:95384d72794f 298 return temp;
batchee7 0:95384d72794f 299 }
batchee7 0:95384d72794f 300
batchee7 0:95384d72794f 301 //-- Convert byte array (From C# app) to Float
batchee7 0:95384d72794f 302 float ByteArrayToFloat(char *Buffer, uint8_t startAdd)
batchee7 0:95384d72794f 303 {
batchee7 0:95384d72794f 304 char temp[4];
batchee7 0:95384d72794f 305 temp[0]= Buffer[startAdd];
batchee7 0:95384d72794f 306 temp[1]= Buffer[startAdd+1];
batchee7 0:95384d72794f 307 temp[2]= Buffer[startAdd+2];
batchee7 0:95384d72794f 308 temp[3]= Buffer[startAdd+3];
batchee7 0:95384d72794f 309 return *((float*)(temp));;
batchee7 0:95384d72794f 310 }