Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

driving.h@5:636c3fe18aa8, 2021-10-25 (annotated)

Committer:
aalawfi
Date:
Mon Oct 25 02:14:11 2021 +0000
Revision:
5:636c3fe18aa8
Parent:
3:25c6bf0abc00
Child:
6:d2bd68ba99c9
- Attempted to extend state control to driving: I wrapped the PI controller in an if-else statement, if the motor is enabled, allow PI controll, if the motor is disabled, write duty cycle of zero.

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 5:636c3fe18aa8 61 volatile bool motor_enabled = true;
aalawfi 5:636c3fe18aa8 62 void PI(void) { //motor PI interrupt
aalawfi 5:636c3fe18aa8 63 if(motor_enabled == true) {
aalawfi 5:636c3fe18aa8 64 //--- Calculate Error ---
aalawfi 5:636c3fe18aa8 65 errorLeft = setpointLeft - (speedLeftVolt / 3.3f); //error and setpoint is defined as a percentage, from 0 to 1
aalawfi 5:636c3fe18aa8 66 errorRight = setpointRight - (speedRightVolt / 3.3f);
aalawfi 5:636c3fe18aa8 67
aalawfi 5:636c3fe18aa8 68 //--- Steady State Error Tolerace ---
aalawfi 5:636c3fe18aa8 69 if (abs(errorLeft) < 0.01){
aalawfi 5:636c3fe18aa8 70 errorLeft = 0.0;
aalawfi 5:636c3fe18aa8 71 }
aalawfi 5:636c3fe18aa8 72 if (abs(errorRight) < 0.01){
aalawfi 5:636c3fe18aa8 73 errorRight = 0.0;
aalawfi 5:636c3fe18aa8 74 }
aalawfi 5:636c3fe18aa8 75 //--- Calculate integral error ---
aalawfi 5:636c3fe18aa8 76 if (clampLeft == false){
aalawfi 5:636c3fe18aa8 77 errorAreaLeft = TI*errorLeft + errorAreaLeftPrev;
aalawfi 5:636c3fe18aa8 78 }
aalawfi 5:636c3fe18aa8 79
aalawfi 5:636c3fe18aa8 80 if (clampRight == false){
aalawfi 5:636c3fe18aa8 81 errorAreaRight = TI*errorRight + errorAreaRightPrev;
aalawfi 5:636c3fe18aa8 82 }
aalawfi 5:636c3fe18aa8 83
aalawfi 5:636c3fe18aa8 84 errorAreaLeftPrev = errorAreaLeft;
aalawfi 5:636c3fe18aa8 85 errorAreaRightPrev = errorAreaRight;
aalawfi 5:636c3fe18aa8 86
aalawfi 5:636c3fe18aa8 87 /*
aalawfi 5:636c3fe18aa8 88 if (errorAreaLeft > 0.1){
aalawfi 5:636c3fe18aa8 89 errorAreaLeft = 0.0;
aalawfi 5:636c3fe18aa8 90 }
aalawfi 5:636c3fe18aa8 91 p
aalawfi 5:636c3fe18aa8 92 if (errorAreaRight > 0.1){
aalawfi 5:636c3fe18aa8 93 errorAreaRight = 0.0;
aalawfi 5:636c3fe18aa8 94 }
aalawfi 5:636c3fe18aa8 95 */
aalawfi 5:636c3fe18aa8 96
aalawfi 5:636c3fe18aa8 97 //--- Calculate total error ---
aalawfi 5:636c3fe18aa8 98 controllerOutputLeft = KP_LEFT*errorLeft + KI_LEFT*errorAreaLeft;
aalawfi 5:636c3fe18aa8 99 controllerOutputRight = KP_RIGHT*errorRight + KI_RIGHT*errorAreaRight;
aalawfi 5:636c3fe18aa8 100
aalawfi 5:636c3fe18aa8 101 tempDutyCycleLeft = fullBatScalar * controllerOutputLeft;
aalawfi 5:636c3fe18aa8 102 tempDutyCycleRight = fullBatScalar * controllerOutputRight;
aalawfi 5:636c3fe18aa8 103
aalawfi 5:636c3fe18aa8 104
aalawfi 5:636c3fe18aa8 105 //--- Motor over-voltage safety ---
aalawfi 5:636c3fe18aa8 106 if (tempDutyCycleLeft > fullBatScalar){ //safety mechanism in case feedback breaks for any reason -
aalawfi 5:636c3fe18aa8 107 dutyCycleLeft = fullBatScalar; //will stop output from exceeding max duty cycle and damaging motor
aalawfi 5:636c3fe18aa8 108 } else {
aalawfi 5:636c3fe18aa8 109 dutyCycleLeft = tempDutyCycleLeft;
aalawfi 5:636c3fe18aa8 110 }
aalawfi 5:636c3fe18aa8 111
aalawfi 5:636c3fe18aa8 112 if (tempDutyCycleRight > fullBatScalar){
aalawfi 5:636c3fe18aa8 113 dutyCycleRight = fullBatScalar;
aalawfi 5:636c3fe18aa8 114 } else {
aalawfi 5:636c3fe18aa8 115 dutyCycleRight = tempDutyCycleRight;
aalawfi 5:636c3fe18aa8 116 }
aalawfi 5:636c3fe18aa8 117
aalawfi 5:636c3fe18aa8 118
aalawfi 5:636c3fe18aa8 119 //--- integral anti-windup ---
aalawfi 5:636c3fe18aa8 120 if (controllerOutputLeft > 1.0){
aalawfi 5:636c3fe18aa8 121 if (errorAreaLeft > 0.0){
aalawfi 5:636c3fe18aa8 122 clampLeft = true;
aalawfi 5:636c3fe18aa8 123 }
aalawfi 5:636c3fe18aa8 124 if (errorAreaLeft < 0.0){
aalawfi 5:636c3fe18aa8 125 clampLeft = false;
aalawfi 5:636c3fe18aa8 126 }
aalawfi 5:636c3fe18aa8 127 } else {
aalawfi 5:636c3fe18aa8 128 clampLeft = false;
aalawfi 5:636c3fe18aa8 129 }
aalawfi 5:636c3fe18aa8 130
aalawfi 5:636c3fe18aa8 131 if (controllerOutputRight > 1.0){
aalawfi 5:636c3fe18aa8 132 if (errorAreaRight > 0.0){
aalawfi 5:636c3fe18aa8 133 clampRight = true;
aalawfi 5:636c3fe18aa8 134 }
aalawfi 5:636c3fe18aa8 135 if (errorAreaRight < 0.0){
aalawfi 5:636c3fe18aa8 136 clampRight = false;
aalawfi 5:636c3fe18aa8 137 }
aalawfi 5:636c3fe18aa8 138 } else {
aalawfi 5:636c3fe18aa8 139 clampRight = false;
aalawfi 5:636c3fe18aa8 140 }
aalawfi 5:636c3fe18aa8 141
aalawfi 5:636c3fe18aa8 142 //--- fucked up manual braking stuff ---
aalawfi 5:636c3fe18aa8 143 if (setpointLeft == 0.0 || brakeLeftBool == true)
aalawfi 5:636c3fe18aa8 144 {
aalawfi 5:636c3fe18aa8 145 errorAreaLeft = 0.0;
aalawfi 5:636c3fe18aa8 146 brakeLeft = 1;
aalawfi 5:636c3fe18aa8 147 } else {
aalawfi 5:636c3fe18aa8 148 brakeLeft = 0;
aalawfi 5:636c3fe18aa8 149 }
aalawfi 5:636c3fe18aa8 150
aalawfi 5:636c3fe18aa8 151 if (setpointRight == 0.0 || brakeRightBool == true)
aalawfi 5:636c3fe18aa8 152 {
aalawfi 5:636c3fe18aa8 153 errorAreaRight = 0.0;
aalawfi 5:636c3fe18aa8 154 brakeRight = 1;
aalawfi 5:636c3fe18aa8 155 } else {
aalawfi 5:636c3fe18aa8 156 brakeRight = 0;
aalawfi 5:636c3fe18aa8 157 }
aalawfi 5:636c3fe18aa8 158
aalawfi 5:636c3fe18aa8 159 //--- set motors to calculated output ---
aalawfi 5:636c3fe18aa8 160 motorLeft.write(dutyCycleLeft);
aalawfi 5:636c3fe18aa8 161 motorRight.write(dutyCycleRight);
aalawfi 5:636c3fe18aa8 162 }
aalawfi 5:636c3fe18aa8 163 else if(motor_enabled == false) {
aalawfi 5:636c3fe18aa8 164 // Does duty cycle zero turn off the motors?
aalawfi 5:636c3fe18aa8 165 motorLeft.write(0);
aalawfi 5:636c3fe18aa8 166 motorRight.write(0);
aalawfi 5:636c3fe18aa8 167
aalawfi 3:25c6bf0abc00 168 }
aalawfi 3:25c6bf0abc00 169 //--- motor braking ---
aalawfi 3:25c6bf0abc00 170 /*
aalawfi 3:25c6bf0abc00 171 if (controllerOutputLeft < 0.0){
aalawfi 3:25c6bf0abc00 172 brakeLeft = 1;
aalawfi 3:25c6bf0abc00 173 } else {
aalawfi 3:25c6bf0abc00 174 brakeLeft = 0;
aalawfi 3:25c6bf0abc00 175 }
aalawfi 3:25c6bf0abc00 176
aalawfi 3:25c6bf0abc00 177 if (controllerOutputRight < 0.0){
aalawfi 3:25c6bf0abc00 178 brakeRight = 1;
aalawfi 3:25c6bf0abc00 179 } else {
aalawfi 3:25c6bf0abc00 180 brakeRight = 0;
aalawfi 3:25c6bf0abc00 181 }
aalawfi 3:25c6bf0abc00 182 */
aalawfi 3:25c6bf0abc00 183
aalawfi 3:25c6bf0abc00 184
aalawfi 3:25c6bf0abc00 185 }