Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

driving.h@10:b0999f69c775, 2021-10-26 (annotated)

Committer:
aalawfi
Date:
Tue Oct 26 02:19:07 2021 +0000
Revision:
10:b0999f69c775
Parent:
9:5320c2dfb913
Child:
11:210a33ee774f
- changed the if else statement for driving to follow the same structure as in steering (the steering structure was tested )

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