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

driving.h@3:25c6bf0abc00, 2021-10-25 (annotated)

Committer:
aalawfi
Date:
Mon Oct 25 01:39:55 2021 +0000
Revision:
3:25c6bf0abc00
Parent:
1:c324a2849500
Child:
5:636c3fe18aa8
- Created bluetooth.h and moved bluetooth related stuff over there. ; - Moved PI controller algorithm to driving.h

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 1:c324a2849500 14 #define freq 20000.0f
quarren42 1:c324a2849500 15
quarren42 1:c324a2849500 16 #define fullBatScalar 0.5873f
quarren42 1:c324a2849500 17 #define speedSensorScalar 2.5f
quarren42 1:c324a2849500 18 #define battDividerScalar 4.0;
aalawfi 3:25c6bf0abc00 19 PwmOut motorLeft(PTB1);
aalawfi 3:25c6bf0abc00 20 PwmOut motorRight(PTB2);
aalawfi 3:25c6bf0abc00 21 AnalogIn pot1(PTB0);
aalawfi 3:25c6bf0abc00 22 AnalogIn speedSensorLeft(PTB3);
aalawfi 3:25c6bf0abc00 23 AnalogIn speedSensorRight(PTC2);
aalawfi 3:25c6bf0abc00 24 DigitalOut ledRed(LED1);
aalawfi 3:25c6bf0abc00 25 AnalogIn battInput(PTC1);
aalawfi 3:25c6bf0abc00 26 DigitalOut brakeLeft(PTA12);
aalawfi 3:25c6bf0abc00 27 DigitalOut brakeRight(PTD4);
aalawfi 3:25c6bf0abc00 28
aalawfi 3:25c6bf0abc00 29 DigitalIn enableBrakeLeft(PTA4);
aalawfi 3:25c6bf0abc00 30 DigitalIn enableBrakeRight(PTA5);
aalawfi 3:25c6bf0abc00 31
aalawfi 3:25c6bf0abc00 32 float pot1Voltage;
aalawfi 3:25c6bf0abc00 33 float dutyCycleLeft;
aalawfi 3:25c6bf0abc00 34 float tempDutyCycleLeft = 0;
aalawfi 3:25c6bf0abc00 35 float tempDutyCycleRight = 0;
aalawfi 3:25c6bf0abc00 36 float dutyCycleRight;
aalawfi 3:25c6bf0abc00 37 float speedLeftVolt;
aalawfi 3:25c6bf0abc00 38 float speedRightVolt;
aalawfi 3:25c6bf0abc00 39 float batteryVoltage;
aalawfi 3:25c6bf0abc00 40 float avgCellVoltage;
aalawfi 3:25c6bf0abc00 41
aalawfi 3:25c6bf0abc00 42 float errorAreaLeft = 0;
aalawfi 3:25c6bf0abc00 43 float errorAreaRight = 0;
aalawfi 3:25c6bf0abc00 44 float errorAreaLeftPrev = 0;
aalawfi 3:25c6bf0abc00 45 float errorAreaRightPrev = 0;
aalawfi 3:25c6bf0abc00 46 float errorLeft = 0;
aalawfi 3:25c6bf0abc00 47 float errorRight = 0;
aalawfi 3:25c6bf0abc00 48 float setpointLeft = 0.0; //target speed, 0.0 to 1.0
aalawfi 3:25c6bf0abc00 49 float setpointRight = 0.0;
aalawfi 3:25c6bf0abc00 50 float controllerOutputLeft = 0;
aalawfi 3:25c6bf0abc00 51 float controllerOutputRight = 0;
aalawfi 3:25c6bf0abc00 52 float x;
aalawfi 3:25c6bf0abc00 53
aalawfi 3:25c6bf0abc00 54 bool clampLeft = false;
aalawfi 3:25c6bf0abc00 55 bool clampRight = false;
aalawfi 3:25c6bf0abc00 56
aalawfi 3:25c6bf0abc00 57 bool brakeLeftBool = false;
aalawfi 3:25c6bf0abc00 58 bool brakeRightBool = false;
aalawfi 3:25c6bf0abc00 59
aalawfi 3:25c6bf0abc00 60
aalawfi 3:25c6bf0abc00 61
aalawfi 3:25c6bf0abc00 62
aalawfi 3:25c6bf0abc00 63 void PI() { //motor PI interrupt
aalawfi 3:25c6bf0abc00 64
aalawfi 3:25c6bf0abc00 65 //--- Calculate Error ---
aalawfi 3:25c6bf0abc00 66 errorLeft = setpointLeft - (speedLeftVolt / 3.3f); //error and setpoint is defined as a percentage, from 0 to 1
aalawfi 3:25c6bf0abc00 67 errorRight = setpointRight - (speedRightVolt / 3.3f);
aalawfi 3:25c6bf0abc00 68
aalawfi 3:25c6bf0abc00 69 //--- Steady State Error Tolerace ---
aalawfi 3:25c6bf0abc00 70 if (abs(errorLeft) < 0.01){
aalawfi 3:25c6bf0abc00 71 errorLeft = 0.0;
aalawfi 3:25c6bf0abc00 72 }
aalawfi 3:25c6bf0abc00 73 if (abs(errorRight) < 0.01){
aalawfi 3:25c6bf0abc00 74 errorRight = 0.0;
aalawfi 3:25c6bf0abc00 75 }
aalawfi 3:25c6bf0abc00 76 //--- Calculate integral error ---
aalawfi 3:25c6bf0abc00 77 if (clampLeft == false){
aalawfi 3:25c6bf0abc00 78 errorAreaLeft = TI*errorLeft + errorAreaLeftPrev;
aalawfi 3:25c6bf0abc00 79 }
aalawfi 3:25c6bf0abc00 80
aalawfi 3:25c6bf0abc00 81 if (clampRight == false){
aalawfi 3:25c6bf0abc00 82 errorAreaRight = TI*errorRight + errorAreaRightPrev;
aalawfi 3:25c6bf0abc00 83 }
aalawfi 3:25c6bf0abc00 84
aalawfi 3:25c6bf0abc00 85 errorAreaLeftPrev = errorAreaLeft;
aalawfi 3:25c6bf0abc00 86 errorAreaRightPrev = errorAreaRight;
aalawfi 3:25c6bf0abc00 87
aalawfi 3:25c6bf0abc00 88 /*
aalawfi 3:25c6bf0abc00 89 if (errorAreaLeft > 0.1){
aalawfi 3:25c6bf0abc00 90 errorAreaLeft = 0.0;
aalawfi 3:25c6bf0abc00 91 }
aalawfi 3:25c6bf0abc00 92 p
aalawfi 3:25c6bf0abc00 93 if (errorAreaRight > 0.1){
aalawfi 3:25c6bf0abc00 94 errorAreaRight = 0.0;
aalawfi 3:25c6bf0abc00 95 }
aalawfi 3:25c6bf0abc00 96 */
aalawfi 3:25c6bf0abc00 97
aalawfi 3:25c6bf0abc00 98 //--- Calculate total error ---
aalawfi 3:25c6bf0abc00 99 controllerOutputLeft = KP_LEFT*errorLeft + KI_LEFT*errorAreaLeft;
aalawfi 3:25c6bf0abc00 100 controllerOutputRight = KP_RIGHT*errorRight + KI_RIGHT*errorAreaRight;
aalawfi 3:25c6bf0abc00 101
aalawfi 3:25c6bf0abc00 102 tempDutyCycleLeft = fullBatScalar * controllerOutputLeft;
aalawfi 3:25c6bf0abc00 103 tempDutyCycleRight = fullBatScalar * controllerOutputRight;
aalawfi 3:25c6bf0abc00 104
aalawfi 3:25c6bf0abc00 105
aalawfi 3:25c6bf0abc00 106 //--- Motor over-voltage safety ---
aalawfi 3:25c6bf0abc00 107 if (tempDutyCycleLeft > fullBatScalar){ //safety mechanism in case feedback breaks for any reason -
aalawfi 3:25c6bf0abc00 108 dutyCycleLeft = fullBatScalar; //will stop output from exceeding max duty cycle and damaging motor
aalawfi 3:25c6bf0abc00 109 } else {
aalawfi 3:25c6bf0abc00 110 dutyCycleLeft = tempDutyCycleLeft;
aalawfi 3:25c6bf0abc00 111 }
aalawfi 3:25c6bf0abc00 112
aalawfi 3:25c6bf0abc00 113 if (tempDutyCycleRight > fullBatScalar){
aalawfi 3:25c6bf0abc00 114 dutyCycleRight = fullBatScalar;
aalawfi 3:25c6bf0abc00 115 } else {
aalawfi 3:25c6bf0abc00 116 dutyCycleRight = tempDutyCycleRight;
aalawfi 3:25c6bf0abc00 117 }
aalawfi 3:25c6bf0abc00 118
aalawfi 3:25c6bf0abc00 119
aalawfi 3:25c6bf0abc00 120 //--- integral anti-windup ---
aalawfi 3:25c6bf0abc00 121 if (controllerOutputLeft > 1.0){
aalawfi 3:25c6bf0abc00 122 if (errorAreaLeft > 0.0){
aalawfi 3:25c6bf0abc00 123 clampLeft = true;
aalawfi 3:25c6bf0abc00 124 }
aalawfi 3:25c6bf0abc00 125 if (errorAreaLeft < 0.0){
aalawfi 3:25c6bf0abc00 126 clampLeft = false;
aalawfi 3:25c6bf0abc00 127 }
aalawfi 3:25c6bf0abc00 128 } else {
aalawfi 3:25c6bf0abc00 129 clampLeft = false;
aalawfi 3:25c6bf0abc00 130 }
aalawfi 3:25c6bf0abc00 131
aalawfi 3:25c6bf0abc00 132 if (controllerOutputRight > 1.0){
aalawfi 3:25c6bf0abc00 133 if (errorAreaRight > 0.0){
aalawfi 3:25c6bf0abc00 134 clampRight = true;
aalawfi 3:25c6bf0abc00 135 }
aalawfi 3:25c6bf0abc00 136 if (errorAreaRight < 0.0){
aalawfi 3:25c6bf0abc00 137 clampRight = false;
aalawfi 3:25c6bf0abc00 138 }
aalawfi 3:25c6bf0abc00 139 } else {
aalawfi 3:25c6bf0abc00 140 clampRight = false;
aalawfi 3:25c6bf0abc00 141 }
aalawfi 3:25c6bf0abc00 142
aalawfi 3:25c6bf0abc00 143 //--- fucked up manual braking stuff ---
aalawfi 3:25c6bf0abc00 144 if (setpointLeft == 0.0 || brakeLeftBool == true)
aalawfi 3:25c6bf0abc00 145 {
aalawfi 3:25c6bf0abc00 146 errorAreaLeft = 0.0;
aalawfi 3:25c6bf0abc00 147 brakeLeft = 1;
aalawfi 3:25c6bf0abc00 148 } else {
aalawfi 3:25c6bf0abc00 149 brakeLeft = 0;
aalawfi 3:25c6bf0abc00 150 }
aalawfi 3:25c6bf0abc00 151
aalawfi 3:25c6bf0abc00 152 if (setpointRight == 0.0 || brakeRightBool == true)
aalawfi 3:25c6bf0abc00 153 {
aalawfi 3:25c6bf0abc00 154 errorAreaRight = 0.0;
aalawfi 3:25c6bf0abc00 155 brakeRight = 1;
aalawfi 3:25c6bf0abc00 156 } else {
aalawfi 3:25c6bf0abc00 157 brakeRight = 0;
aalawfi 3:25c6bf0abc00 158 }
aalawfi 3:25c6bf0abc00 159
aalawfi 3:25c6bf0abc00 160 //--- set motors to calculated output ---
aalawfi 3:25c6bf0abc00 161 motorLeft.write(dutyCycleLeft);
aalawfi 3:25c6bf0abc00 162 motorRight.write(dutyCycleRight);
aalawfi 3:25c6bf0abc00 163
aalawfi 3:25c6bf0abc00 164
aalawfi 3:25c6bf0abc00 165 //--- motor braking ---
aalawfi 3:25c6bf0abc00 166 /*
aalawfi 3:25c6bf0abc00 167 if (controllerOutputLeft < 0.0){
aalawfi 3:25c6bf0abc00 168 brakeLeft = 1;
aalawfi 3:25c6bf0abc00 169 } else {
aalawfi 3:25c6bf0abc00 170 brakeLeft = 0;
aalawfi 3:25c6bf0abc00 171 }
aalawfi 3:25c6bf0abc00 172
aalawfi 3:25c6bf0abc00 173 if (controllerOutputRight < 0.0){
aalawfi 3:25c6bf0abc00 174 brakeRight = 1;
aalawfi 3:25c6bf0abc00 175 } else {
aalawfi 3:25c6bf0abc00 176 brakeRight = 0;
aalawfi 3:25c6bf0abc00 177 }
aalawfi 3:25c6bf0abc00 178 */
aalawfi 3:25c6bf0abc00 179
aalawfi 3:25c6bf0abc00 180
aalawfi 3:25c6bf0abc00 181 }