Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

driving.h@33:0a1e29085b79, 2021-11-22 (annotated)

Committer:
quarren42
Date:
Mon Nov 22 21:54:27 2021 +0000
Revision:
33:0a1e29085b79
Parent:
32:ec8c9a82d9fc
Child:
34:cb9a0cec2feb
added lap counter code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
quarren42 1:c324a2849500 1 #define TI 0.001f //1kHz sample time
quarren42 1:c324a2849500 2 #define KP_LEFT 0.0379f //Proportional gain
quarren42 1:c324a2849500 3 #define KI_LEFT 0.7369f //Integral Gain
quarren42 1:c324a2849500 4
quarren42 1:c324a2849500 5 //#define KP_LEFT 16.1754f
quarren42 1:c324a2849500 6 //#define KI_LEFT 314.7632f
quarren42 1:c324a2849500 7
quarren42 1:c324a2849500 8 //#define KP_RIGHT 0.0379f
quarren42 1:c324a2849500 9 //#define KI_RIGHT 0.7369f
quarren42 1:c324a2849500 10
quarren42 1:c324a2849500 11 #define KP_RIGHT 0.0463f
quarren42 1:c324a2849500 12 #define KI_RIGHT 0.8981f
quarren42 1:c324a2849500 13
quarren42 18:831c1e03d83e 14 #define freq 25000.0f
quarren42 1:c324a2849500 15
quarren42 1:c324a2849500 16 #define fullBatScalar 0.5873f
quarren42 1:c324a2849500 17 #define speedSensorScalar 2.5f
quarren42 21:9e6ddb590103 18 #define battDividerScalar 4.2;
quarren42 6:d2bd68ba99c9 19
quarren42 30:ab358e8a9e6a 20 #define constSpeed = 0.17;
quarren42 30:ab358e8a9e6a 21
quarren42 8:cca7647cdb4b 22 AnalogIn pot1(PTB2); //was PTB0
aalawfi 16:8cd4dd323941 23 // Motor Left is PTE20
aalawfi 16:8cd4dd323941 24 // Motor right is pte 21
quarren42 8:cca7647cdb4b 25 PwmOut motorLeft(PTE20); //was PTB1
quarren42 8:cca7647cdb4b 26 PwmOut motorRight(PTE21); //was PTB2
aalawfi 16:8cd4dd323941 27 // left is ptb3
aalawfi 16:8cd4dd323941 28 // right is ptc 2
aalawfi 3:25c6bf0abc00 29 AnalogIn speedSensorLeft(PTB3);
aalawfi 3:25c6bf0abc00 30 AnalogIn speedSensorRight(PTC2);
aalawfi 3:25c6bf0abc00 31 DigitalOut ledRed(LED1);
aalawfi 3:25c6bf0abc00 32 AnalogIn battInput(PTC1);
aalawfi 16:8cd4dd323941 33 // left brake is pta12
aalawfi 16:8cd4dd323941 34 // right brake is pta4
aalawfi 3:25c6bf0abc00 35 DigitalOut brakeLeft(PTA12);
quarren42 8:cca7647cdb4b 36 DigitalOut brakeRight(PTA4); //was PTD4
aalawfi 3:25c6bf0abc00 37
aalawfi 3:25c6bf0abc00 38 float pot1Voltage;
aalawfi 3:25c6bf0abc00 39 float dutyCycleLeft;
aalawfi 3:25c6bf0abc00 40 float tempDutyCycleLeft = 0;
aalawfi 3:25c6bf0abc00 41 float tempDutyCycleRight = 0;
aalawfi 3:25c6bf0abc00 42 float dutyCycleRight;
aalawfi 3:25c6bf0abc00 43 float speedLeftVolt;
aalawfi 3:25c6bf0abc00 44 float speedRightVolt;
aalawfi 3:25c6bf0abc00 45 float batteryVoltage;
aalawfi 3:25c6bf0abc00 46 float avgCellVoltage;
aalawfi 3:25c6bf0abc00 47
aalawfi 3:25c6bf0abc00 48 float errorAreaLeft = 0;
aalawfi 3:25c6bf0abc00 49 float errorAreaRight = 0;
aalawfi 3:25c6bf0abc00 50 float errorAreaLeftPrev = 0;
aalawfi 3:25c6bf0abc00 51 float errorAreaRightPrev = 0;
aalawfi 3:25c6bf0abc00 52 float errorLeft = 0;
aalawfi 3:25c6bf0abc00 53 float errorRight = 0;
aalawfi 3:25c6bf0abc00 54 float setpointLeft = 0.0; //target speed, 0.0 to 1.0
aalawfi 3:25c6bf0abc00 55 float setpointRight = 0.0;
aalawfi 3:25c6bf0abc00 56 float controllerOutputLeft = 0;
aalawfi 3:25c6bf0abc00 57 float controllerOutputRight = 0;
aalawfi 3:25c6bf0abc00 58 float x;
aalawfi 3:25c6bf0abc00 59
quarren42 30:ab358e8a9e6a 60 Timer t;
aalawfi 31:d570f957e083 61 Timer laptimer;
quarren42 30:ab358e8a9e6a 62
aalawfi 3:25c6bf0abc00 63 bool clampLeft = false;
aalawfi 3:25c6bf0abc00 64 bool clampRight = false;
quarren42 30:ab358e8a9e6a 65 bool override = false;
aalawfi 3:25c6bf0abc00 66
aalawfi 3:25c6bf0abc00 67 bool brakeLeftBool = false;
aalawfi 3:25c6bf0abc00 68 bool brakeRightBool = false;
aalawfi 3:25c6bf0abc00 69
quarren42 33:0a1e29085b79 70 double lapTimerCount = 0;
quarren42 33:0a1e29085b79 71 double lapTimerFinal = 0;
quarren42 33:0a1e29085b79 72
aalawfi 13:0091da3021df 73 volatile bool are_brakes_activated; // Used for debugging
aalawfi 11:210a33ee774f 74 void disable_brakes(void){
aalawfi 13:0091da3021df 75 are_brakes_activated = false;
aalawfi 11:210a33ee774f 76 brakeLeft=0;
aalawfi 11:210a33ee774f 77 brakeRight=0;
aalawfi 11:210a33ee774f 78 };
aalawfi 11:210a33ee774f 79 void enable_brakes(void) {
aalawfi 13:0091da3021df 80 are_brakes_activated = true;
aalawfi 11:210a33ee774f 81 brakeLeft = 1;
aalawfi 11:210a33ee774f 82 brakeRight = 1;
quarren42 30:ab358e8a9e6a 83 // errorAreaRight = 0.0;
quarren42 30:ab358e8a9e6a 84 // errorAreaLeft = 0.0;
aalawfi 11:210a33ee774f 85 };
quarren42 30:ab358e8a9e6a 86 void _tempBraking(void){
aalawfi 26:54ce9f642477 87
aalawfi 26:54ce9f642477 88 }
aalawfi 3:25c6bf0abc00 89
quarren42 30:ab358e8a9e6a 90
aalawfi 10:b0999f69c775 91 /*
aalawfi 10:b0999f69c775 92 Always set duty cycle to zero (disabling the motor), unless the motor is enabled
aalawfi 10:b0999f69c775 93 (motor_enabled = true);
aalawfi 10:b0999f69c775 94 BY default, the motor is disabled (duty cycle is zero)
aalawfi 10:b0999f69c775 95 */
aalawfi 10:b0999f69c775 96
aalawfi 9:5320c2dfb913 97 volatile bool motor_enabled = false;
aalawfi 5:636c3fe18aa8 98 void PI(void) { //motor PI interrupt
quarren42 17:d2c98ebda90b 99
quarren42 17:d2c98ebda90b 100 speedLeftVolt = (speedSensorLeft.read() * 3.3f);
quarren42 17:d2c98ebda90b 101 speedRightVolt = (speedSensorRight.read() * 3.3f);
quarren42 17:d2c98ebda90b 102
aalawfi 26:54ce9f642477 103
aalawfi 26:54ce9f642477 104 //_steering();
aalawfi 26:54ce9f642477 105
aalawfi 10:b0999f69c775 106 if(motor_enabled == true) {
aalawfi 31:d570f957e083 107 if(lap == 0 & counter < 7)
aalawfi 25:8bd029d58251 108 {
quarren42 30:ab358e8a9e6a 109 setpointLeft = 0.15;
quarren42 30:ab358e8a9e6a 110 setpointRight = 0.15;
aalawfi 25:8bd029d58251 111 }
aalawfi 31:d570f957e083 112 else if (counter == 7 & lap == 0)
quarren42 30:ab358e8a9e6a 113 {
aalawfi 31:d570f957e083 114 setpointLeft = 0.32; //0.4 is too fast on sraightaway
aalawfi 31:d570f957e083 115 setpointRight = 0.32; //0.25 is good
quarren42 30:ab358e8a9e6a 116 }
quarren42 30:ab358e8a9e6a 117 else if (counter == 1 & lap == 1)
aalawfi 25:8bd029d58251 118 {
aalawfi 31:d570f957e083 119 setpointLeft = 0.24;
aalawfi 31:d570f957e083 120 setpointRight = 0.24;
quarren42 30:ab358e8a9e6a 121 enable_brakes(); //uncomment out to brake on big turn
quarren42 30:ab358e8a9e6a 122 t.start();
aalawfi 31:d570f957e083 123 if (t.read_ms() > 250)
quarren42 30:ab358e8a9e6a 124 {
quarren42 30:ab358e8a9e6a 125 disable_brakes();
quarren42 30:ab358e8a9e6a 126 t.stop();
quarren42 30:ab358e8a9e6a 127 }
quarren42 30:ab358e8a9e6a 128 }
quarren42 30:ab358e8a9e6a 129 else if (counter == 4 & lap == 1)
quarren42 30:ab358e8a9e6a 130 {
aalawfi 31:d570f957e083 131 setpointLeft = 0.24;
aalawfi 31:d570f957e083 132 setpointRight = 0.24;
aalawfi 25:8bd029d58251 133 }
quarren42 30:ab358e8a9e6a 134
quarren42 30:ab358e8a9e6a 135 else if(counter == 5 && lap == 1)
aalawfi 25:8bd029d58251 136 {
aalawfi 31:d570f957e083 137 enable_brakes();
aalawfi 31:d570f957e083 138 t.start();
aalawfi 31:d570f957e083 139
aalawfi 31:d570f957e083 140 if (t.read_ms() > 50)
aalawfi 31:d570f957e083 141 {
aalawfi 31:d570f957e083 142 disable_brakes();
aalawfi 31:d570f957e083 143 t.stop();
aalawfi 31:d570f957e083 144 }
aalawfi 32:ec8c9a82d9fc 145 setpointLeft = 0.23;
aalawfi 32:ec8c9a82d9fc 146 setpointRight = 0.23;
aalawfi 32:ec8c9a82d9fc 147 }
aalawfi 32:ec8c9a82d9fc 148 else if(counter == 6 && lap == 1)
aalawfi 32:ec8c9a82d9fc 149 {
aalawfi 32:ec8c9a82d9fc 150 enable_brakes();
aalawfi 32:ec8c9a82d9fc 151 t.start();
aalawfi 32:ec8c9a82d9fc 152
aalawfi 32:ec8c9a82d9fc 153 if (t.read_ms() > 50)
aalawfi 32:ec8c9a82d9fc 154 {
aalawfi 32:ec8c9a82d9fc 155 disable_brakes();
aalawfi 32:ec8c9a82d9fc 156 t.stop();
aalawfi 32:ec8c9a82d9fc 157 }
aalawfi 32:ec8c9a82d9fc 158 setpointLeft = 0.23;
aalawfi 32:ec8c9a82d9fc 159 setpointRight = 0.23;
aalawfi 25:8bd029d58251 160 }
aalawfi 31:d570f957e083 161 else if(counter == 3 && lap == 1 )
aalawfi 31:d570f957e083 162 {
aalawfi 31:d570f957e083 163 enable_brakes();
aalawfi 31:d570f957e083 164 t.start();
aalawfi 31:d570f957e083 165
aalawfi 31:d570f957e083 166 if (t.read_ms() > 200)
aalawfi 31:d570f957e083 167 {
aalawfi 31:d570f957e083 168 disable_brakes();
aalawfi 31:d570f957e083 169 t.stop();
aalawfi 31:d570f957e083 170 }
aalawfi 31:d570f957e083 171 }
aalawfi 31:d570f957e083 172 else if(counter == 7 && lap == 1 )
aalawfi 31:d570f957e083 173 {
aalawfi 31:d570f957e083 174 enable_brakes();
aalawfi 31:d570f957e083 175 t.start();
aalawfi 31:d570f957e083 176
aalawfi 32:ec8c9a82d9fc 177 if (t.read_ms() > 100)
aalawfi 31:d570f957e083 178 {
aalawfi 31:d570f957e083 179 disable_brakes();
aalawfi 31:d570f957e083 180 t.stop();
aalawfi 31:d570f957e083 181 }
aalawfi 31:d570f957e083 182 }
quarren42 30:ab358e8a9e6a 183 else if (lap >= 2)
aalawfi 25:8bd029d58251 184 {
quarren42 30:ab358e8a9e6a 185 setpointLeft = 0.15;
quarren42 30:ab358e8a9e6a 186 setpointRight = 0.15;
aalawfi 25:8bd029d58251 187 }
quarren42 30:ab358e8a9e6a 188
quarren42 30:ab358e8a9e6a 189 else {
aalawfi 31:d570f957e083 190 setpointLeft = 0.24;
aalawfi 31:d570f957e083 191 setpointRight = 0.24;
quarren42 30:ab358e8a9e6a 192 }
quarren42 30:ab358e8a9e6a 193
quarren42 30:ab358e8a9e6a 194
quarren42 33:0a1e29085b79 195 if (lap == 1)
quarren42 33:0a1e29085b79 196 {
quarren42 33:0a1e29085b79 197 lapTimerCount = lapTimerCount + 1;
quarren42 33:0a1e29085b79 198 }
quarren42 33:0a1e29085b79 199 if (lap == 2 && counter == 0)
quarren42 33:0a1e29085b79 200 {
quarren42 33:0a1e29085b79 201 lapTimerFinal = lapTimerCount;
quarren42 33:0a1e29085b79 202 }
quarren42 33:0a1e29085b79 203
aalawfi 31:d570f957e083 204
aalawfi 23:4c743746533c 205 // setpointLeft = 0.1;
aalawfi 23:4c743746533c 206 // setpointRight = 0.1;
aalawfi 10:b0999f69c775 207 //--- Calculate Error ---
aalawfi 10:b0999f69c775 208 errorLeft = setpointLeft - (speedLeftVolt / 3.3f); //error and setpoint is defined as a percentage, from 0 to 1
aalawfi 10:b0999f69c775 209 errorRight = setpointRight - (speedRightVolt / 3.3f);
aalawfi 10:b0999f69c775 210 //--- Steady State Error Tolerace ---
aalawfi 10:b0999f69c775 211 if (abs(errorLeft) < 0.01){
aalawfi 10:b0999f69c775 212 errorLeft = 0.0;
aalawfi 10:b0999f69c775 213 }
aalawfi 10:b0999f69c775 214 if (abs(errorRight) < 0.01){
aalawfi 10:b0999f69c775 215 errorRight = 0.0;
aalawfi 10:b0999f69c775 216 }
aalawfi 10:b0999f69c775 217 //--- Calculate integral error ---
aalawfi 10:b0999f69c775 218 if (clampLeft == false){
aalawfi 5:636c3fe18aa8 219 errorAreaLeft = TI*errorLeft + errorAreaLeftPrev;
aalawfi 10:b0999f69c775 220 }
aalawfi 10:b0999f69c775 221 if (clampRight == false){
aalawfi 10:b0999f69c775 222 errorAreaRight = TI*errorRight + errorAreaRightPrev;
aalawfi 10:b0999f69c775 223 }
aalawfi 10:b0999f69c775 224 errorAreaLeftPrev = errorAreaLeft;
aalawfi 10:b0999f69c775 225 errorAreaRightPrev = errorAreaRight;
aalawfi 26:54ce9f642477 226
aalawfi 5:636c3fe18aa8 227 /*
aalawfi 5:636c3fe18aa8 228 if (errorAreaLeft > 0.1){
aalawfi 5:636c3fe18aa8 229 errorAreaLeft = 0.0;
aalawfi 5:636c3fe18aa8 230 }
aalawfi 5:636c3fe18aa8 231 p
aalawfi 5:636c3fe18aa8 232 if (errorAreaRight > 0.1){
aalawfi 5:636c3fe18aa8 233 errorAreaRight = 0.0;
aalawfi 5:636c3fe18aa8 234 }
aalawfi 5:636c3fe18aa8 235 */
aalawfi 5:636c3fe18aa8 236
aalawfi 5:636c3fe18aa8 237 //--- Calculate total error ---
aalawfi 10:b0999f69c775 238 controllerOutputLeft = KP_LEFT*errorLeft + KI_LEFT*errorAreaLeft;
aalawfi 10:b0999f69c775 239 controllerOutputRight = KP_RIGHT*errorRight + KI_RIGHT*errorAreaRight;
aalawfi 10:b0999f69c775 240 tempDutyCycleLeft = fullBatScalar * controllerOutputLeft;
aalawfi 10:b0999f69c775 241 tempDutyCycleRight = fullBatScalar * controllerOutputRight;
aalawfi 10:b0999f69c775 242 //--- Motor over-voltage safety ---
aalawfi 10:b0999f69c775 243 if (tempDutyCycleLeft > fullBatScalar){ //safety mechanism in case feedback breaks for any reason -
aalawfi 10:b0999f69c775 244 dutyCycleLeft = fullBatScalar; //will stop output from exceeding max duty cycle and damaging motor
aalawfi 10:b0999f69c775 245 } else {
aalawfi 10:b0999f69c775 246 dutyCycleLeft = tempDutyCycleLeft;
aalawfi 5:636c3fe18aa8 247 }
aalawfi 10:b0999f69c775 248 if (tempDutyCycleRight > fullBatScalar){
aalawfi 10:b0999f69c775 249 dutyCycleRight = fullBatScalar;
aalawfi 10:b0999f69c775 250 } else {
aalawfi 10:b0999f69c775 251 dutyCycleRight = tempDutyCycleRight;
aalawfi 10:b0999f69c775 252 }
aalawfi 10:b0999f69c775 253 //--- integral anti-windup ---
aalawfi 10:b0999f69c775 254 if (controllerOutputLeft > 1.0){
aalawfi 10:b0999f69c775 255 if (errorAreaLeft > 0.0){
aalawfi 10:b0999f69c775 256 clampLeft = true;
aalawfi 5:636c3fe18aa8 257 }
aalawfi 10:b0999f69c775 258 if (errorAreaLeft < 0.0){
aalawfi 10:b0999f69c775 259 clampLeft = false;
aalawfi 10:b0999f69c775 260 }
aalawfi 10:b0999f69c775 261 } else {
aalawfi 10:b0999f69c775 262 clampLeft = false;
aalawfi 5:636c3fe18aa8 263 }
aalawfi 5:636c3fe18aa8 264
aalawfi 5:636c3fe18aa8 265 if (controllerOutputRight > 1.0){
aalawfi 5:636c3fe18aa8 266 if (errorAreaRight > 0.0){
aalawfi 5:636c3fe18aa8 267 clampRight = true;
aalawfi 5:636c3fe18aa8 268 }
aalawfi 5:636c3fe18aa8 269 if (errorAreaRight < 0.0){
aalawfi 5:636c3fe18aa8 270 clampRight = false;
aalawfi 5:636c3fe18aa8 271 }
aalawfi 5:636c3fe18aa8 272 } else {
aalawfi 5:636c3fe18aa8 273 clampRight = false;
aalawfi 5:636c3fe18aa8 274 }
aalawfi 5:636c3fe18aa8 275
aalawfi 26:54ce9f642477 276
quarren42 17:d2c98ebda90b 277
quarren42 30:ab358e8a9e6a 278 //--- set motors to calculated output ---
quarren42 17:d2c98ebda90b 279 motorLeft.write(dutyCycleLeft); // 0.2 For debugging
quarren42 17:d2c98ebda90b 280 motorRight.write(dutyCycleRight);
aalawfi 9:5320c2dfb913 281
aalawfi 3:25c6bf0abc00 282 //--- motor braking ---
aalawfi 3:25c6bf0abc00 283 /*
aalawfi 3:25c6bf0abc00 284 if (controllerOutputLeft < 0.0){
aalawfi 3:25c6bf0abc00 285 brakeLeft = 1;
aalawfi 3:25c6bf0abc00 286 } else {
aalawfi 3:25c6bf0abc00 287 brakeLeft = 0;
aalawfi 3:25c6bf0abc00 288 }
aalawfi 3:25c6bf0abc00 289
aalawfi 3:25c6bf0abc00 290 if (controllerOutputRight < 0.0){
aalawfi 3:25c6bf0abc00 291 brakeRight = 1;
aalawfi 3:25c6bf0abc00 292 } else {
aalawfi 3:25c6bf0abc00 293 brakeRight = 0;
aalawfi 3:25c6bf0abc00 294 }
aalawfi 3:25c6bf0abc00 295 */
aalawfi 9:5320c2dfb913 296 }
aalawfi 10:b0999f69c775 297 else {
aalawfi 10:b0999f69c775 298 motorLeft.write(0.0);
aalawfi 10:b0999f69c775 299 motorRight.write(0.0);
aalawfi 10:b0999f69c775 300 }
aalawfi 3:25c6bf0abc00 301 }