Elct Car Team / Mbed 2 deprecated CarLab_v2

- This code combines steering and driving in one ticker - Fault check is in a ticker instead of while loop

Dependencies:   mbed MMA8451Q

main.cpp@2:c857935f928e, 2021-10-25 (annotated)

Committer:
aalawfi
Date:
Mon Oct 25 01:28:53 2021 +0000
Revision:
2:c857935f928e
Parent:
1:c324a2849500
Child:
3:25c6bf0abc00
- Added steering files ; - We must re-assign IO pins

Who changed what in which revision?

UserRevisionLine numberNew contents of line
quarren42 0:0a6756c7e3ed 1 #include "mbed.h"
quarren42 1:c324a2849500 2 #include "driving.h"
aalawfi 2:c857935f928e 3 #include "steering_header.h"
aalawfi 2:c857935f928e 4 #include "steering_methods.h"
aalawfi 2:c857935f928e 5 #include "state_control.h"
quarren42 1:c324a2849500 6 #include "MMA8451Q.h"
quarren42 0:0a6756c7e3ed 7 #include <iostream>
quarren42 0:0a6756c7e3ed 8 #define BLUETOOTHBAUDRATE 115200 //Communication rate of the micro-controller
quarren42 0:0a6756c7e3ed 9 //to the Bluetooth module
quarren42 0:0a6756c7e3ed 10 #define SERIALTXPORT PTE0 //Tx Pin (Sends information to serial device)
quarren42 0:0a6756c7e3ed 11 #define SERIALRXPORT PTE1 //Rx Pin (Receives information from serial
quarren42 1:c324a2849500 12
quarren42 1:c324a2849500 13 #define MMA8451_I2C_ADDRESS (0x1d<<1)
quarren42 1:c324a2849500 14
quarren42 0:0a6756c7e3ed 15 Serial pc(USBTX, USBRX); // tx, rx
quarren42 0:0a6756c7e3ed 16 Serial bt(SERIALTXPORT, SERIALRXPORT); //(TX, RX) Serial declaration for new serial
quarren42 0:0a6756c7e3ed 17
quarren42 1:c324a2849500 18 PinName const SDA = PTE25;
quarren42 1:c324a2849500 19 PinName const SCL = PTE24;
quarren42 1:c324a2849500 20
quarren42 1:c324a2849500 21 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
quarren42 1:c324a2849500 22 Ticker motorLoop;
quarren42 1:c324a2849500 23 Timer t;
quarren42 1:c324a2849500 24
quarren42 1:c324a2849500 25 PwmOut motorLeft(PTB1);
quarren42 1:c324a2849500 26 PwmOut motorRight(PTB2);
quarren42 1:c324a2849500 27 AnalogIn pot1(PTB0);
quarren42 1:c324a2849500 28 AnalogIn speedSensorLeft(PTB3);
quarren42 1:c324a2849500 29 AnalogIn speedSensorRight(PTC2);
quarren42 1:c324a2849500 30 DigitalOut ledRed(LED1);
quarren42 1:c324a2849500 31 AnalogIn battInput(PTC1);
quarren42 1:c324a2849500 32 DigitalOut brakeLeft(PTA12);
quarren42 1:c324a2849500 33 DigitalOut brakeRight(PTD4);
quarren42 1:c324a2849500 34
quarren42 1:c324a2849500 35 DigitalIn enableBrakeLeft(PTA4);
quarren42 1:c324a2849500 36 DigitalIn enableBrakeRight(PTA5);
quarren42 1:c324a2849500 37
quarren42 1:c324a2849500 38 float pot1Voltage;
quarren42 1:c324a2849500 39 float dutyCycleLeft;
quarren42 1:c324a2849500 40 float tempDutyCycleLeft = 0;
quarren42 1:c324a2849500 41 float tempDutyCycleRight = 0;
quarren42 1:c324a2849500 42 float dutyCycleRight;
quarren42 1:c324a2849500 43 float speedLeftVolt;
quarren42 1:c324a2849500 44 float speedRightVolt;
quarren42 1:c324a2849500 45 float batteryVoltage;
quarren42 1:c324a2849500 46 float avgCellVoltage;
quarren42 1:c324a2849500 47
quarren42 1:c324a2849500 48 float errorAreaLeft = 0;
quarren42 1:c324a2849500 49 float errorAreaRight = 0;
quarren42 1:c324a2849500 50 float errorAreaLeftPrev = 0;
quarren42 1:c324a2849500 51 float errorAreaRightPrev = 0;
quarren42 1:c324a2849500 52 float errorLeft = 0;
quarren42 1:c324a2849500 53 float errorRight = 0;
quarren42 1:c324a2849500 54 float setpointLeft = 0.0; //target speed, 0.0 to 1.0
quarren42 1:c324a2849500 55 float setpointRight = 0.0;
quarren42 1:c324a2849500 56 float controllerOutputLeft = 0;
quarren42 1:c324a2849500 57 float controllerOutputRight = 0;
quarren42 1:c324a2849500 58 float x;
quarren42 1:c324a2849500 59
quarren42 1:c324a2849500 60 char newInputChar;
quarren42 1:c324a2849500 61 int newInputInt;
quarren42 1:c324a2849500 62 bool clampLeft = false;
quarren42 1:c324a2849500 63 bool clampRight = false;
quarren42 1:c324a2849500 64
quarren42 1:c324a2849500 65 bool brakeLeftBool = false;
quarren42 1:c324a2849500 66 bool brakeRightBool = false;
quarren42 1:c324a2849500 67
quarren42 1:c324a2849500 68 volatile bool newData = false;
quarren42 1:c324a2849500 69
quarren42 1:c324a2849500 70 void btReceive() { //comment this out if it's fucked
quarren42 0:0a6756c7e3ed 71
quarren42 1:c324a2849500 72 static char buffer[6];
quarren42 1:c324a2849500 73 static int serialCount = 0;
quarren42 1:c324a2849500 74
quarren42 1:c324a2849500 75 {
quarren42 1:c324a2849500 76 char byteIn = bt.getc();
quarren42 1:c324a2849500 77 // bt.printf("Got %c",byteIn);
quarren42 1:c324a2849500 78 if (byteIn == 'n') {
quarren42 1:c324a2849500 79 buffer[serialCount] = 0;
quarren42 1:c324a2849500 80 //bt.printf("Got endl %c",byteIn);
quarren42 1:c324a2849500 81 int speed;
quarren42 1:c324a2849500 82 char type;
quarren42 1:c324a2849500 83 if (sscanf(buffer,"%c%i",&type,&speed) == 2) {
quarren42 1:c324a2849500 84 newInputChar = type;
quarren42 1:c324a2849500 85 // bt.printf("char: %c", type);
quarren42 1:c324a2849500 86 newInputInt = speed;
quarren42 1:c324a2849500 87 // bt.printf("speed: %i", speed);
quarren42 1:c324a2849500 88 newData = true;
quarren42 1:c324a2849500 89 }
quarren42 1:c324a2849500 90 serialCount = 0;
quarren42 1:c324a2849500 91 } else {
quarren42 1:c324a2849500 92 buffer[serialCount] = byteIn;
quarren42 1:c324a2849500 93 if (serialCount < 6)
quarren42 1:c324a2849500 94 serialCount++;
quarren42 1:c324a2849500 95 }
quarren42 1:c324a2849500 96 }
quarren42 1:c324a2849500 97 }
quarren42 0:0a6756c7e3ed 98
quarren42 1:c324a2849500 99 void PI() { //motor PI interrupt
quarren42 1:c324a2849500 100
quarren42 1:c324a2849500 101 //--- Calculate Error ---
quarren42 1:c324a2849500 102 errorLeft = setpointLeft - (speedLeftVolt / 3.3f); //error and setpoint is defined as a percentage, from 0 to 1
quarren42 1:c324a2849500 103 errorRight = setpointRight - (speedRightVolt / 3.3f);
quarren42 1:c324a2849500 104
quarren42 1:c324a2849500 105 //--- Steady State Error Tolerace ---
quarren42 1:c324a2849500 106 if (abs(errorLeft) < 0.01){
quarren42 1:c324a2849500 107 errorLeft = 0.0;
quarren42 1:c324a2849500 108 }
quarren42 1:c324a2849500 109 if (abs(errorRight) < 0.01){
quarren42 1:c324a2849500 110 errorRight = 0.0;
quarren42 1:c324a2849500 111 }
quarren42 1:c324a2849500 112 //--- Calculate integral error ---
quarren42 1:c324a2849500 113 if (clampLeft == false){
quarren42 1:c324a2849500 114 errorAreaLeft = TI*errorLeft + errorAreaLeftPrev;
quarren42 0:0a6756c7e3ed 115 }
quarren42 1:c324a2849500 116
quarren42 1:c324a2849500 117 if (clampRight == false){
quarren42 1:c324a2849500 118 errorAreaRight = TI*errorRight + errorAreaRightPrev;
quarren42 1:c324a2849500 119 }
quarren42 1:c324a2849500 120
quarren42 1:c324a2849500 121 errorAreaLeftPrev = errorAreaLeft;
quarren42 1:c324a2849500 122 errorAreaRightPrev = errorAreaRight;
quarren42 1:c324a2849500 123
quarren42 1:c324a2849500 124 /*
quarren42 1:c324a2849500 125 if (errorAreaLeft > 0.1){
quarren42 1:c324a2849500 126 errorAreaLeft = 0.0;
quarren42 1:c324a2849500 127 }
quarren42 1:c324a2849500 128 p
quarren42 1:c324a2849500 129 if (errorAreaRight > 0.1){
quarren42 1:c324a2849500 130 errorAreaRight = 0.0;
quarren42 1:c324a2849500 131 }
quarren42 1:c324a2849500 132 */
quarren42 1:c324a2849500 133
quarren42 1:c324a2849500 134 //--- Calculate total error ---
quarren42 1:c324a2849500 135 controllerOutputLeft = KP_LEFT*errorLeft + KI_LEFT*errorAreaLeft;
quarren42 1:c324a2849500 136 controllerOutputRight = KP_RIGHT*errorRight + KI_RIGHT*errorAreaRight;
quarren42 1:c324a2849500 137
quarren42 1:c324a2849500 138 tempDutyCycleLeft = fullBatScalar * controllerOutputLeft;
quarren42 1:c324a2849500 139 tempDutyCycleRight = fullBatScalar * controllerOutputRight;
quarren42 0:0a6756c7e3ed 140
quarren42 0:0a6756c7e3ed 141
quarren42 1:c324a2849500 142 //--- Motor over-voltage safety ---
quarren42 1:c324a2849500 143 if (tempDutyCycleLeft > fullBatScalar){ //safety mechanism in case feedback breaks for any reason -
quarren42 1:c324a2849500 144 dutyCycleLeft = fullBatScalar; //will stop output from exceeding max duty cycle and damaging motor
quarren42 1:c324a2849500 145 } else {
quarren42 1:c324a2849500 146 dutyCycleLeft = tempDutyCycleLeft;
quarren42 1:c324a2849500 147 }
quarren42 1:c324a2849500 148
quarren42 1:c324a2849500 149 if (tempDutyCycleRight > fullBatScalar){
quarren42 1:c324a2849500 150 dutyCycleRight = fullBatScalar;
quarren42 1:c324a2849500 151 } else {
quarren42 1:c324a2849500 152 dutyCycleRight = tempDutyCycleRight;
quarren42 1:c324a2849500 153 }
quarren42 1:c324a2849500 154
quarren42 1:c324a2849500 155
quarren42 1:c324a2849500 156 //--- integral anti-windup ---
quarren42 1:c324a2849500 157 if (controllerOutputLeft > 1.0){
quarren42 1:c324a2849500 158 if (errorAreaLeft > 0.0){
quarren42 1:c324a2849500 159 clampLeft = true;
quarren42 1:c324a2849500 160 }
quarren42 1:c324a2849500 161 if (errorAreaLeft < 0.0){
quarren42 1:c324a2849500 162 clampLeft = false;
quarren42 1:c324a2849500 163 }
quarren42 1:c324a2849500 164 } else {
quarren42 1:c324a2849500 165 clampLeft = false;
quarren42 1:c324a2849500 166 }
quarren42 1:c324a2849500 167
quarren42 1:c324a2849500 168 if (controllerOutputRight > 1.0){
quarren42 1:c324a2849500 169 if (errorAreaRight > 0.0){
quarren42 1:c324a2849500 170 clampRight = true;
quarren42 1:c324a2849500 171 }
quarren42 1:c324a2849500 172 if (errorAreaRight < 0.0){
quarren42 1:c324a2849500 173 clampRight = false;
quarren42 1:c324a2849500 174 }
quarren42 1:c324a2849500 175 } else {
quarren42 1:c324a2849500 176 clampRight = false;
quarren42 1:c324a2849500 177 }
quarren42 1:c324a2849500 178
quarren42 1:c324a2849500 179 //--- fucked up manual braking stuff ---
quarren42 1:c324a2849500 180 if (setpointLeft == 0.0 || brakeLeftBool == true)
quarren42 1:c324a2849500 181 {
quarren42 1:c324a2849500 182 errorAreaLeft = 0.0;
quarren42 1:c324a2849500 183 brakeLeft = 1;
quarren42 1:c324a2849500 184 } else {
quarren42 1:c324a2849500 185 brakeLeft = 0;
quarren42 1:c324a2849500 186 }
quarren42 1:c324a2849500 187
quarren42 1:c324a2849500 188 if (setpointRight == 0.0 || brakeRightBool == true)
quarren42 1:c324a2849500 189 {
quarren42 1:c324a2849500 190 errorAreaRight = 0.0;
quarren42 1:c324a2849500 191 brakeRight = 1;
quarren42 1:c324a2849500 192 } else {
quarren42 1:c324a2849500 193 brakeRight = 0;
quarren42 1:c324a2849500 194 }
quarren42 1:c324a2849500 195
quarren42 1:c324a2849500 196 //--- set motors to calculated output ---
quarren42 1:c324a2849500 197 motorLeft.write(dutyCycleLeft);
quarren42 1:c324a2849500 198 motorRight.write(dutyCycleRight);
quarren42 0:0a6756c7e3ed 199
quarren42 0:0a6756c7e3ed 200
quarren42 1:c324a2849500 201 //--- motor braking ---
quarren42 1:c324a2849500 202 /*
quarren42 1:c324a2849500 203 if (controllerOutputLeft < 0.0){
quarren42 1:c324a2849500 204 brakeLeft = 1;
quarren42 1:c324a2849500 205 } else {
quarren42 1:c324a2849500 206 brakeLeft = 0;
quarren42 1:c324a2849500 207 }
quarren42 1:c324a2849500 208
quarren42 1:c324a2849500 209 if (controllerOutputRight < 0.0){
quarren42 1:c324a2849500 210 brakeRight = 1;
quarren42 1:c324a2849500 211 } else {
quarren42 1:c324a2849500 212 brakeRight = 0;
quarren42 1:c324a2849500 213 }
quarren42 1:c324a2849500 214 */
quarren42 1:c324a2849500 215
quarren42 1:c324a2849500 216
quarren42 1:c324a2849500 217 }
quarren42 0:0a6756c7e3ed 218
quarren42 0:0a6756c7e3ed 219 int main() {
aalawfi 2:c857935f928e 220
aalawfi 2:c857935f928e 221
aalawfi 2:c857935f928e 222 state_update();
aalawfi 2:c857935f928e 223
aalawfi 2:c857935f928e 224 //Delcare Onboard LED with blue color
aalawfi 2:c857935f928e 225 DigitalOut led_b(LED_BLUE);
aalawfi 2:c857935f928e 226 //Set the period of the servo motor control signal
aalawfi 2:c857935f928e 227 servo_motor_pwm.period(1/SERVO_MOTOR_FREQ);
aalawfi 2:c857935f928e 228 //Center the servo motor
aalawfi 2:c857935f928e 229 servo_motor_pwm.write(CENTER_DUTY_CYCLE);
aalawfi 2:c857935f928e 230
aalawfi 2:c857935f928e 231 //Start the control systm using a Ticker object
aalawfi 2:c857935f928e 232 steering_control_ticker.attach(&steering_control, TI);
aalawfi 2:c857935f928e 233
aalawfi 2:c857935f928e 234
aalawfi 2:c857935f928e 235 // call landmark_counter wjen a landmark is detected
aalawfi 2:c857935f928e 236 left_landmark_sensor.rise(&landmark_counter);
aalawfi 2:c857935f928e 237 right_landmark_sensor.rise(&landmark_counter);
aalawfi 2:c857935f928e 238 // update status when the button is pressed
aalawfi 2:c857935f928e 239 stop_button.rise(&state_update);
aalawfi 2:c857935f928e 240 run_button.rise(&state_update);
aalawfi 2:c857935f928e 241 wait_button.rise(&state_update);
aalawfi 2:c857935f928e 242
aalawfi 2:c857935f928e 243
quarren42 0:0a6756c7e3ed 244 bt.baud(BLUETOOTHBAUDRATE);
quarren42 0:0a6756c7e3ed 245 //Sets the communication rate of the micro-controller to the Bluetooth module.
quarren42 0:0a6756c7e3ed 246 pc.printf("Hello World!\n");
quarren42 1:c324a2849500 247 bt.printf("Hello World!\n");
quarren42 1:c324a2849500 248
quarren42 1:c324a2849500 249 t.start();
quarren42 1:c324a2849500 250 float time = t.read();
quarren42 1:c324a2849500 251
quarren42 1:c324a2849500 252 //bt.attach(&btReceive);
quarren42 1:c324a2849500 253 motorLoop.attach(&PI,TI);
quarren42 1:c324a2849500 254
quarren42 1:c324a2849500 255 motorLeft.period(1/freq);
quarren42 1:c324a2849500 256 motorRight.period(1/freq);
quarren42 1:c324a2849500 257
quarren42 1:c324a2849500 258 //setpointLeft = 0.0; //target speed, 0.0 to 1.0
quarren42 1:c324a2849500 259 //setpointRight = 0.0;
quarren42 1:c324a2849500 260
quarren42 1:c324a2849500 261 setpointLeft = 0.7; //target speed, 0.0 to 1.0
quarren42 1:c324a2849500 262 setpointRight = 0.7;
quarren42 1:c324a2849500 263
quarren42 0:0a6756c7e3ed 264 //prints the string to the Tera-Term Console using the Bluetooth object ‘bt’.
quarren42 0:0a6756c7e3ed 265 while(1) {
aalawfi 2:c857935f928e 266
aalawfi 2:c857935f928e 267 pc.printf("\n\n");
aalawfi 2:c857935f928e 268
aalawfi 2:c857935f928e 269 pc.printf("\r\nSteering enabled? : %d",steering_enabled);
aalawfi 2:c857935f928e 270 pc.printf("\r\nCurrent duty cycle : %f ", current_duty_cycle);
aalawfi 2:c857935f928e 271 pc.printf("\r\nLeft duty cycle : %f ", left_distance_sensor.read());
aalawfi 2:c857935f928e 272 pc.printf("\r\nRight voltage : %f" , right_distance_sensor.read());
aalawfi 2:c857935f928e 273 pc.printf("\r\nCurrent duty cycle : %f ", current_duty_cycle);
aalawfi 2:c857935f928e 274 fault_check();
aalawfi 2:c857935f928e 275 wait(0.5);
aalawfi 2:c857935f928e 276 switch(current_state ){
aalawfi 2:c857935f928e 277 case STOP : pc.printf("\r\nCurrent state is stop"); break;
aalawfi 2:c857935f928e 278 case RUN: pc.printf("\r\nCurrent state is RUN"); break;
aalawfi 2:c857935f928e 279 case WAIT : pc.printf("\r\nCurrent state is WAIT"); break;
aalawfi 2:c857935f928e 280 };
aalawfi 2:c857935f928e 281 switch(fault_type) {
aalawfi 2:c857935f928e 282 case CLEAR : pc.printf("\r\nFault: CLEAR"); break;
aalawfi 2:c857935f928e 283 case OFF_TRACK: pc.printf("\r\nFault: OFF TRACK"); break;
aalawfi 2:c857935f928e 284 case BUMPER : pc.printf("\r\nFault: OBSTECALE"); break;
aalawfi 2:c857935f928e 285 case LOW_VOLTAGE : pc.printf("Fault: LOW VOLTAGE"); break;
aalawfi 2:c857935f928e 286 }
aalawfi 2:c857935f928e 287
quarren42 1:c324a2849500 288 x = abs(acc.getAccX());
quarren42 0:0a6756c7e3ed 289 pot1Voltage = pot1 * 3.3f;
quarren42 1:c324a2849500 290 batteryVoltage = battInput * 3.3 * battDividerScalar;
quarren42 1:c324a2849500 291 avgCellVoltage = batteryVoltage / 3.0;
quarren42 1:c324a2849500 292
quarren42 0:0a6756c7e3ed 293 //dutyCycleLeft = (pot1 * fullBatScalar);
quarren42 0:0a6756c7e3ed 294 //dutyCycleRight = (pot1 * fullBatScalar);
quarren42 0:0a6756c7e3ed 295
quarren42 1:c324a2849500 296 //speedLeft = (speedSensorLeft * speedSensorScalar);
quarren42 1:c324a2849500 297 //speedRight = (speedSensorRight * speedSensorScalar);
quarren42 1:c324a2849500 298
quarren42 1:c324a2849500 299 speedLeftVolt = (speedSensorLeft * 3.3f);
quarren42 1:c324a2849500 300 speedRightVolt = (speedSensorRight * 3.3f);
quarren42 1:c324a2849500 301
quarren42 1:c324a2849500 302 bt.printf("Duty Cycle = %1.2f ", dutyCycleLeft);
quarren42 1:c324a2849500 303 bt.printf("Speed (V) L,R = %1.2f", speedLeftVolt);
quarren42 1:c324a2849500 304 bt.printf(", %1.2f ", speedRightVolt);
quarren42 1:c324a2849500 305
quarren42 1:c324a2849500 306 bt.printf("Error L,R: %5.2f", errorLeft);
quarren42 1:c324a2849500 307 bt.printf(", %5.2f ", errorRight);
quarren42 1:c324a2849500 308
quarren42 1:c324a2849500 309 bt.printf("ErrorArea: %1.2f ", errorAreaLeft);
quarren42 1:c324a2849500 310 bt.printf("Output: %1.2f ", controllerOutputLeft);
quarren42 1:c324a2849500 311
quarren42 1:c324a2849500 312 bt.printf("Batt Voltage: %1.2f \n",avgCellVoltage);
quarren42 1:c324a2849500 313
quarren42 1:c324a2849500 314 //setpointLeft = pot1;
quarren42 1:c324a2849500 315 //setpointRight = pot1;
quarren42 0:0a6756c7e3ed 316
quarren42 0:0a6756c7e3ed 317
quarren42 1:c324a2849500 318 if (t.read() > 5){
quarren42 1:c324a2849500 319 setpointLeft = 0.0;
quarren42 1:c324a2849500 320 setpointRight = 0.0;
quarren42 1:c324a2849500 321 }
quarren42 1:c324a2849500 322
quarren42 1:c324a2849500 323 if (t.read_ms() > 5100){
quarren42 1:c324a2849500 324 setpointLeft = 0.2;
quarren42 1:c324a2849500 325 setpointRight = 0.2;
quarren42 1:c324a2849500 326 }
quarren42 0:0a6756c7e3ed 327
quarren42 1:c324a2849500 328 if (newData){
quarren42 1:c324a2849500 329 newData = false;
quarren42 1:c324a2849500 330 // bt.printf("Got %c %3i\n",newInputChar, newInputInt);
quarren42 1:c324a2849500 331
quarren42 1:c324a2849500 332 if (newInputChar == 'L')
quarren42 1:c324a2849500 333 setpointLeft = (newInputInt / 100.0f);
quarren42 1:c324a2849500 334 if (newInputChar == 'R')
quarren42 1:c324a2849500 335 setpointRight = (newInputInt / 100.0f);
quarren42 1:c324a2849500 336
quarren42 0:0a6756c7e3ed 337
quarren42 1:c324a2849500 338 //wait(0.1);
quarren42 1:c324a2849500 339 }
quarren42 1:c324a2849500 340 }
quarren42 1:c324a2849500 341 }
quarren42 1:c324a2849500 342