Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

driving.h@28:1c2eb25d624e, 2021-11-17 (annotated)

Committer:
aalawfi
Date:
Wed Nov 17 21:37:29 2021 +0000
Revision:
28:1c2eb25d624e
Parent:
26:54ce9f642477
Child:
30:ab358e8a9e6a
0.17 speed => ~~ 17.35 s

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 8:cca7647cdb4b 20 AnalogIn pot1(PTB2); //was PTB0
aalawfi 16:8cd4dd323941 21 // Motor Left is PTE20
aalawfi 16:8cd4dd323941 22 // Motor right is pte 21
quarren42 8:cca7647cdb4b 23 PwmOut motorLeft(PTE20); //was PTB1
quarren42 8:cca7647cdb4b 24 PwmOut motorRight(PTE21); //was PTB2
aalawfi 16:8cd4dd323941 25 // left is ptb3
aalawfi 16:8cd4dd323941 26 // right is ptc 2
aalawfi 3:25c6bf0abc00 27 AnalogIn speedSensorLeft(PTB3);
aalawfi 3:25c6bf0abc00 28 AnalogIn speedSensorRight(PTC2);
aalawfi 3:25c6bf0abc00 29 DigitalOut ledRed(LED1);
aalawfi 3:25c6bf0abc00 30 AnalogIn battInput(PTC1);
aalawfi 16:8cd4dd323941 31 // left brake is pta12
aalawfi 16:8cd4dd323941 32 // right brake is pta4
aalawfi 3:25c6bf0abc00 33 DigitalOut brakeLeft(PTA12);
quarren42 8:cca7647cdb4b 34 DigitalOut brakeRight(PTA4); //was PTD4
aalawfi 3:25c6bf0abc00 35
aalawfi 3:25c6bf0abc00 36 float pot1Voltage;
aalawfi 3:25c6bf0abc00 37 float dutyCycleLeft;
aalawfi 3:25c6bf0abc00 38 float tempDutyCycleLeft = 0;
aalawfi 3:25c6bf0abc00 39 float tempDutyCycleRight = 0;
aalawfi 3:25c6bf0abc00 40 float dutyCycleRight;
aalawfi 3:25c6bf0abc00 41 float speedLeftVolt;
aalawfi 3:25c6bf0abc00 42 float speedRightVolt;
aalawfi 3:25c6bf0abc00 43 float batteryVoltage;
aalawfi 3:25c6bf0abc00 44 float avgCellVoltage;
aalawfi 3:25c6bf0abc00 45
aalawfi 3:25c6bf0abc00 46 float errorAreaLeft = 0;
aalawfi 3:25c6bf0abc00 47 float errorAreaRight = 0;
aalawfi 3:25c6bf0abc00 48 float errorAreaLeftPrev = 0;
aalawfi 3:25c6bf0abc00 49 float errorAreaRightPrev = 0;
aalawfi 3:25c6bf0abc00 50 float errorLeft = 0;
aalawfi 3:25c6bf0abc00 51 float errorRight = 0;
aalawfi 3:25c6bf0abc00 52 float setpointLeft = 0.0; //target speed, 0.0 to 1.0
aalawfi 3:25c6bf0abc00 53 float setpointRight = 0.0;
aalawfi 3:25c6bf0abc00 54 float controllerOutputLeft = 0;
aalawfi 3:25c6bf0abc00 55 float controllerOutputRight = 0;
aalawfi 3:25c6bf0abc00 56 float x;
aalawfi 3:25c6bf0abc00 57
aalawfi 3:25c6bf0abc00 58 bool clampLeft = false;
aalawfi 3:25c6bf0abc00 59 bool clampRight = false;
aalawfi 3:25c6bf0abc00 60
aalawfi 3:25c6bf0abc00 61 bool brakeLeftBool = false;
aalawfi 3:25c6bf0abc00 62 bool brakeRightBool = false;
aalawfi 3:25c6bf0abc00 63
aalawfi 13:0091da3021df 64 volatile bool are_brakes_activated; // Used for debugging
aalawfi 11:210a33ee774f 65 void disable_brakes(void){
aalawfi 13:0091da3021df 66 are_brakes_activated = false;
aalawfi 11:210a33ee774f 67 brakeLeft=0;
aalawfi 11:210a33ee774f 68 brakeRight=0;
aalawfi 11:210a33ee774f 69 };
aalawfi 11:210a33ee774f 70 void enable_brakes(void) {
aalawfi 13:0091da3021df 71 are_brakes_activated = true;
aalawfi 11:210a33ee774f 72 brakeLeft = 1;
aalawfi 11:210a33ee774f 73 brakeRight = 1;
quarren42 17:d2c98ebda90b 74 errorAreaRight = 0.0;
quarren42 17:d2c98ebda90b 75 errorAreaLeft = 0.0;
quarren42 17:d2c98ebda90b 76
aalawfi 11:210a33ee774f 77 };
aalawfi 26:54ce9f642477 78 void _steering(void){
aalawfi 26:54ce9f642477 79
aalawfi 26:54ce9f642477 80 }
aalawfi 3:25c6bf0abc00 81
aalawfi 10:b0999f69c775 82 /*
aalawfi 10:b0999f69c775 83 Always set duty cycle to zero (disabling the motor), unless the motor is enabled
aalawfi 10:b0999f69c775 84 (motor_enabled = true);
aalawfi 10:b0999f69c775 85 BY default, the motor is disabled (duty cycle is zero)
aalawfi 10:b0999f69c775 86 */
aalawfi 10:b0999f69c775 87
aalawfi 9:5320c2dfb913 88 volatile bool motor_enabled = false;
aalawfi 5:636c3fe18aa8 89 void PI(void) { //motor PI interrupt
quarren42 17:d2c98ebda90b 90
quarren42 17:d2c98ebda90b 91 speedLeftVolt = (speedSensorLeft.read() * 3.3f);
quarren42 17:d2c98ebda90b 92 speedRightVolt = (speedSensorRight.read() * 3.3f);
quarren42 17:d2c98ebda90b 93
aalawfi 26:54ce9f642477 94
aalawfi 26:54ce9f642477 95 //_steering();
aalawfi 26:54ce9f642477 96
aalawfi 10:b0999f69c775 97 if(motor_enabled == true) {
aalawfi 25:8bd029d58251 98 if(counter == 0 && lap == 0 )
aalawfi 25:8bd029d58251 99 {
aalawfi 28:1c2eb25d624e 100 setpointLeft = 0.17;
aalawfi 28:1c2eb25d624e 101 setpointRight = 0.17;
aalawfi 25:8bd029d58251 102 }
aalawfi 28:1c2eb25d624e 103 else if(counter == 0 && lap == 3 )
aalawfi 25:8bd029d58251 104 {
aalawfi 28:1c2eb25d624e 105 setpointLeft = 0.17;
aalawfi 28:1c2eb25d624e 106 setpointRight = 0.17;
aalawfi 25:8bd029d58251 107 }
aalawfi 28:1c2eb25d624e 108 else if(counter !=0 && lap == 3 )
aalawfi 25:8bd029d58251 109 {
aalawfi 28:1c2eb25d624e 110 setpointLeft = 0.17;
aalawfi 28:1c2eb25d624e 111 setpointRight = 0.17;
aalawfi 25:8bd029d58251 112
aalawfi 25:8bd029d58251 113 }
aalawfi 25:8bd029d58251 114 else
aalawfi 25:8bd029d58251 115 {
aalawfi 28:1c2eb25d624e 116 setpointLeft = 0.17;
aalawfi 28:1c2eb25d624e 117 setpointRight = 0.17;
aalawfi 25:8bd029d58251 118 }
aalawfi 23:4c743746533c 119 // setpointLeft = 0.1;
aalawfi 23:4c743746533c 120 // setpointRight = 0.1;
aalawfi 10:b0999f69c775 121 //--- Calculate Error ---
aalawfi 10:b0999f69c775 122 errorLeft = setpointLeft - (speedLeftVolt / 3.3f); //error and setpoint is defined as a percentage, from 0 to 1
aalawfi 10:b0999f69c775 123 errorRight = setpointRight - (speedRightVolt / 3.3f);
aalawfi 10:b0999f69c775 124 //--- Steady State Error Tolerace ---
aalawfi 10:b0999f69c775 125 if (abs(errorLeft) < 0.01){
aalawfi 10:b0999f69c775 126 errorLeft = 0.0;
aalawfi 10:b0999f69c775 127 }
aalawfi 10:b0999f69c775 128 if (abs(errorRight) < 0.01){
aalawfi 10:b0999f69c775 129 errorRight = 0.0;
aalawfi 10:b0999f69c775 130 }
aalawfi 10:b0999f69c775 131 //--- Calculate integral error ---
aalawfi 10:b0999f69c775 132 if (clampLeft == false){
aalawfi 5:636c3fe18aa8 133 errorAreaLeft = TI*errorLeft + errorAreaLeftPrev;
aalawfi 10:b0999f69c775 134 }
aalawfi 10:b0999f69c775 135 if (clampRight == false){
aalawfi 10:b0999f69c775 136 errorAreaRight = TI*errorRight + errorAreaRightPrev;
aalawfi 10:b0999f69c775 137 }
aalawfi 10:b0999f69c775 138 errorAreaLeftPrev = errorAreaLeft;
aalawfi 10:b0999f69c775 139 errorAreaRightPrev = errorAreaRight;
aalawfi 26:54ce9f642477 140
aalawfi 5:636c3fe18aa8 141 /*
aalawfi 5:636c3fe18aa8 142 if (errorAreaLeft > 0.1){
aalawfi 5:636c3fe18aa8 143 errorAreaLeft = 0.0;
aalawfi 5:636c3fe18aa8 144 }
aalawfi 5:636c3fe18aa8 145 p
aalawfi 5:636c3fe18aa8 146 if (errorAreaRight > 0.1){
aalawfi 5:636c3fe18aa8 147 errorAreaRight = 0.0;
aalawfi 5:636c3fe18aa8 148 }
aalawfi 5:636c3fe18aa8 149 */
aalawfi 5:636c3fe18aa8 150
aalawfi 5:636c3fe18aa8 151 //--- Calculate total error ---
aalawfi 10:b0999f69c775 152 controllerOutputLeft = KP_LEFT*errorLeft + KI_LEFT*errorAreaLeft;
aalawfi 10:b0999f69c775 153 controllerOutputRight = KP_RIGHT*errorRight + KI_RIGHT*errorAreaRight;
aalawfi 10:b0999f69c775 154 tempDutyCycleLeft = fullBatScalar * controllerOutputLeft;
aalawfi 10:b0999f69c775 155 tempDutyCycleRight = fullBatScalar * controllerOutputRight;
aalawfi 10:b0999f69c775 156 //--- Motor over-voltage safety ---
aalawfi 10:b0999f69c775 157 if (tempDutyCycleLeft > fullBatScalar){ //safety mechanism in case feedback breaks for any reason -
aalawfi 10:b0999f69c775 158 dutyCycleLeft = fullBatScalar; //will stop output from exceeding max duty cycle and damaging motor
aalawfi 10:b0999f69c775 159 } else {
aalawfi 10:b0999f69c775 160 dutyCycleLeft = tempDutyCycleLeft;
aalawfi 5:636c3fe18aa8 161 }
aalawfi 10:b0999f69c775 162 if (tempDutyCycleRight > fullBatScalar){
aalawfi 10:b0999f69c775 163 dutyCycleRight = fullBatScalar;
aalawfi 10:b0999f69c775 164 } else {
aalawfi 10:b0999f69c775 165 dutyCycleRight = tempDutyCycleRight;
aalawfi 10:b0999f69c775 166 }
aalawfi 10:b0999f69c775 167 //--- integral anti-windup ---
aalawfi 10:b0999f69c775 168 if (controllerOutputLeft > 1.0){
aalawfi 10:b0999f69c775 169 if (errorAreaLeft > 0.0){
aalawfi 10:b0999f69c775 170 clampLeft = true;
aalawfi 5:636c3fe18aa8 171 }
aalawfi 10:b0999f69c775 172 if (errorAreaLeft < 0.0){
aalawfi 10:b0999f69c775 173 clampLeft = false;
aalawfi 10:b0999f69c775 174 }
aalawfi 10:b0999f69c775 175 } else {
aalawfi 10:b0999f69c775 176 clampLeft = false;
aalawfi 5:636c3fe18aa8 177 }
aalawfi 5:636c3fe18aa8 178
aalawfi 5:636c3fe18aa8 179 if (controllerOutputRight > 1.0){
aalawfi 5:636c3fe18aa8 180 if (errorAreaRight > 0.0){
aalawfi 5:636c3fe18aa8 181 clampRight = true;
aalawfi 5:636c3fe18aa8 182 }
aalawfi 5:636c3fe18aa8 183 if (errorAreaRight < 0.0){
aalawfi 5:636c3fe18aa8 184 clampRight = false;
aalawfi 5:636c3fe18aa8 185 }
aalawfi 5:636c3fe18aa8 186 } else {
aalawfi 5:636c3fe18aa8 187 clampRight = false;
aalawfi 5:636c3fe18aa8 188 }
aalawfi 5:636c3fe18aa8 189
aalawfi 26:54ce9f642477 190
quarren42 17:d2c98ebda90b 191
aalawfi 5:636c3fe18aa8 192 //--- set motors to calculated output ---
quarren42 17:d2c98ebda90b 193 motorLeft.write(dutyCycleLeft); // 0.2 For debugging
quarren42 17:d2c98ebda90b 194 motorRight.write(dutyCycleRight);
aalawfi 9:5320c2dfb913 195
aalawfi 3:25c6bf0abc00 196 //--- motor braking ---
aalawfi 3:25c6bf0abc00 197 /*
aalawfi 3:25c6bf0abc00 198 if (controllerOutputLeft < 0.0){
aalawfi 3:25c6bf0abc00 199 brakeLeft = 1;
aalawfi 3:25c6bf0abc00 200 } else {
aalawfi 3:25c6bf0abc00 201 brakeLeft = 0;
aalawfi 3:25c6bf0abc00 202 }
aalawfi 3:25c6bf0abc00 203
aalawfi 3:25c6bf0abc00 204 if (controllerOutputRight < 0.0){
aalawfi 3:25c6bf0abc00 205 brakeRight = 1;
aalawfi 3:25c6bf0abc00 206 } else {
aalawfi 3:25c6bf0abc00 207 brakeRight = 0;
aalawfi 3:25c6bf0abc00 208 }
aalawfi 3:25c6bf0abc00 209 */
aalawfi 9:5320c2dfb913 210 }
aalawfi 10:b0999f69c775 211 else {
aalawfi 10:b0999f69c775 212 motorLeft.write(0.0);
aalawfi 10:b0999f69c775 213 motorRight.write(0.0);
aalawfi 10:b0999f69c775 214 }
aalawfi 3:25c6bf0abc00 215 }