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@9:5f032ca6c9b5, 2021-10-25 (annotated)

Committer:
aalawfi
Date:
Mon Oct 25 17:39:23 2021 +0000
Revision:
9:5f032ca6c9b5
Parent:
8:e4a147850ba4 
- fixed some bugs

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