Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

main.cpp@1:c324a2849500, 2021-10-24 (annotated)

Committer:
quarren42
Date:
Sun Oct 24 18:15:30 2021 +0000
Revision:
1:c324a2849500
Parent:
0:0a6756c7e3ed
Child:
2:c857935f928e
final code before merging;

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