Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

main.cpp@2:06f128641b62, 2018-10-18 (annotated)

Committer:
shovelcat
Date:
Thu Oct 18 02:20:14 2018 +0000
Revision:
2:06f128641b62
Parent:
1:c6b4ccebd483
Child:
3:4da392d2bae8
tests on vehicule succeeded

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shovelcat 0:011f69c1276c 1 #include "mbed.h"
shovelcat 2:06f128641b62 2 #include "speedlimiter.hpp"
shovelcat 0:011f69c1276c 3
shovelcat 0:011f69c1276c 4 /*
shovelcat 0:011f69c1276c 5 ##############################
shovelcat 1:c6b4ccebd483 6 uC I/O
shovelcat 1:c6b4ccebd483 7 ##############################
shovelcat 1:c6b4ccebd483 8 */
shovelcat 2:06f128641b62 9 AnalogIn pedalInHi(PA_3);
shovelcat 2:06f128641b62 10 AnalogIn pedalInLo(PC_0);
shovelcat 2:06f128641b62 11 AnalogOut pedalOutHi(PA_4);
shovelcat 2:06f128641b62 12 AnalogOut pedalOutLo(PA_5);
shovelcat 1:c6b4ccebd483 13
shovelcat 1:c6b4ccebd483 14 // test i/o
shovelcat 1:c6b4ccebd483 15 InterruptIn b1(PC_13);
shovelcat 1:c6b4ccebd483 16 DigitalOut led(LED1);
shovelcat 1:c6b4ccebd483 17 DigitalOut canLed(LED2);
shovelcat 1:c6b4ccebd483 18 Serial pc(USBTX, USBRX); // for communication / debugging
shovelcat 1:c6b4ccebd483 19 CAN can1(PD_0, PD_1);
shovelcat 1:c6b4ccebd483 20
shovelcat 1:c6b4ccebd483 21 /*
shovelcat 1:c6b4ccebd483 22 ##############################
shovelcat 0:011f69c1276c 23 Constants
shovelcat 0:011f69c1276c 24 ##############################
shovelcat 0:011f69c1276c 25 */
shovelcat 2:06f128641b62 26 const float ADC_INPUT_MAX_VALUE = 3.3;
shovelcat 2:06f128641b62 27 const float ADC_OUTPUT_MAX_VALUE = 3.3;
shovelcat 0:011f69c1276c 28
shovelcat 0:011f69c1276c 29 const float PEDAL_HI_MIN_VALUE = 0.48;
shovelcat 0:011f69c1276c 30 const float PEDAL_HI_MAX_VALUE = 2.96;
shovelcat 0:011f69c1276c 31 const float PEDAL_LO_MIN_VALUE = 0.25;
shovelcat 0:011f69c1276c 32 const float PEDAL_LO_MAX_VALUE = 1.48;
shovelcat 0:011f69c1276c 33
shovelcat 1:c6b4ccebd483 34 const float SPEED_MAX = 40.0;
shovelcat 1:c6b4ccebd483 35
shovelcat 2:06f128641b62 36 const float DISABLE_ECO_ALGO_TRIGGER = 999;
shovelcat 2:06f128641b62 37
shovelcat 0:011f69c1276c 38 /*
shovelcat 0:011f69c1276c 39 ##############################
shovelcat 0:011f69c1276c 40 Static variables and functions
shovelcat 0:011f69c1276c 41 ##############################
shovelcat 0:011f69c1276c 42 */
shovelcat 2:06f128641b62 43 static float __reference_speed = DISABLE_ECO_ALGO_TRIGGER;
shovelcat 0:011f69c1276c 44 static float __measured_speed = 0;
shovelcat 0:011f69c1276c 45
shovelcat 0:011f69c1276c 46 static void setReferenceSpeed(const float speed) {
shovelcat 0:011f69c1276c 47 // TODO insert mutex here
shovelcat 0:011f69c1276c 48 __reference_speed = speed;
shovelcat 0:011f69c1276c 49 }
shovelcat 0:011f69c1276c 50
shovelcat 0:011f69c1276c 51 static void setMeasuredSpeed(const float speed) {
shovelcat 0:011f69c1276c 52 // TODO insert mutex here
shovelcat 0:011f69c1276c 53 __measured_speed = speed;
shovelcat 0:011f69c1276c 54 }
shovelcat 0:011f69c1276c 55
shovelcat 0:011f69c1276c 56 static float getReferenceSpeed() {
shovelcat 0:011f69c1276c 57 // TODO insert mutex here
shovelcat 0:011f69c1276c 58 return __reference_speed;
shovelcat 0:011f69c1276c 59 }
shovelcat 0:011f69c1276c 60 static float getMeasuredSpeed() {
shovelcat 0:011f69c1276c 61 // TODO insert mutex here
shovelcat 0:011f69c1276c 62 return __measured_speed;
shovelcat 0:011f69c1276c 63 }
shovelcat 0:011f69c1276c 64
shovelcat 0:011f69c1276c 65 /*
shovelcat 0:011f69c1276c 66 ##############################
shovelcat 0:011f69c1276c 67 Utility functions
shovelcat 0:011f69c1276c 68 ##############################
shovelcat 0:011f69c1276c 69 */
shovelcat 0:011f69c1276c 70
shovelcat 0:011f69c1276c 71 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
shovelcat 0:011f69c1276c 72 float boundValue(float value, const float lowerBound, const float upperBound) {
shovelcat 0:011f69c1276c 73 if(value < lowerBound) {
shovelcat 0:011f69c1276c 74 value = lowerBound;
shovelcat 0:011f69c1276c 75 }
shovelcat 0:011f69c1276c 76 else if(value > upperBound) {
shovelcat 0:011f69c1276c 77 value = upperBound;
shovelcat 0:011f69c1276c 78 }
shovelcat 0:011f69c1276c 79 return value;
shovelcat 0:011f69c1276c 80 }
shovelcat 0:011f69c1276c 81
shovelcat 0:011f69c1276c 82 // Returns "value/reference" as a percentage in decimal form (0.5 for 50%)
shovelcat 0:011f69c1276c 83 float toDecimalPercent(const float value, const float reference) {
shovelcat 0:011f69c1276c 84 return value/reference;
shovelcat 0:011f69c1276c 85 }
shovelcat 0:011f69c1276c 86
shovelcat 0:011f69c1276c 87 // Returns voltage read on analog input port chosen for pedal input 1
shovelcat 0:011f69c1276c 88 float readAdcPedalHi() {
shovelcat 2:06f128641b62 89 const float decPcValue = pedalInHi.read();
shovelcat 0:011f69c1276c 90 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 0:011f69c1276c 91 return voltage;
shovelcat 0:011f69c1276c 92 }
shovelcat 0:011f69c1276c 93
shovelcat 0:011f69c1276c 94 // Returns voltage read on analog input port chosen for pedal input 2
shovelcat 0:011f69c1276c 95 float readAdcPedalLo() {
shovelcat 2:06f128641b62 96 const float decPcValue = pedalInLo.read();
shovelcat 0:011f69c1276c 97 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 0:011f69c1276c 98 return voltage;
shovelcat 0:011f69c1276c 99 }
shovelcat 0:011f69c1276c 100
shovelcat 0:011f69c1276c 101
shovelcat 0:011f69c1276c 102 // Accepts a value in volts, converts to % and sets ADC for pedal output 1
shovelcat 0:011f69c1276c 103 void writeAdcPedalHi(const float voltage) {
shovelcat 0:011f69c1276c 104 const float boundedValue = boundValue(voltage, PEDAL_HI_MIN_VALUE, PEDAL_HI_MAX_VALUE);
shovelcat 0:011f69c1276c 105 const float decPcValue = toDecimalPercent(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 2:06f128641b62 106 pedalOutHi.write(decPcValue);
shovelcat 0:011f69c1276c 107 }
shovelcat 0:011f69c1276c 108
shovelcat 0:011f69c1276c 109 // Accepts a value in volts, converts to % and sets ADC for pedal output 2
shovelcat 0:011f69c1276c 110 void writeAdcPedalLo(const float voltage) {
shovelcat 0:011f69c1276c 111 const float boundedValue = boundValue(voltage, PEDAL_LO_MIN_VALUE, PEDAL_LO_MAX_VALUE);
shovelcat 0:011f69c1276c 112 const float decPcValue = toDecimalPercent(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 2:06f128641b62 113 pedalOutLo.write(decPcValue);
shovelcat 2:06f128641b62 114 }
shovelcat 2:06f128641b62 115
shovelcat 2:06f128641b62 116 float ecoDisabledAlgorithm() {
shovelcat 2:06f128641b62 117 const float value = readAdcPedalHi();
shovelcat 2:06f128641b62 118 pc.printf("%s : %f\n\r",__FUNCTION__, value);
shovelcat 2:06f128641b62 119 return value;
shovelcat 0:011f69c1276c 120 }
shovelcat 0:011f69c1276c 121
shovelcat 2:06f128641b62 122 float ecoEnabledAlgorithm() {
shovelcat 2:06f128641b62 123 pc.printf("%s\n\r",__FUNCTION__);
shovelcat 2:06f128641b62 124 // valeurs anterieurs
shovelcat 2:06f128641b62 125 static float x1 = 0.0;
shovelcat 2:06f128641b62 126 static float y1 = 0.0;
shovelcat 2:06f128641b62 127
shovelcat 2:06f128641b62 128 // constantes calcules avec matlab
shovelcat 2:06f128641b62 129 const float b0 = 10793.0;
shovelcat 2:06f128641b62 130 const float b1 = -8513.5;
shovelcat 2:06f128641b62 131 const float a1 = -1.0;
shovelcat 2:06f128641b62 132
shovelcat 2:06f128641b62 133 const float TRANS_k_gear = 20.5; //Gearbox ratio
shovelcat 2:06f128641b62 134 const float TRANS_eff = 0.91; // Efficiency of the transmission
shovelcat 2:06f128641b62 135 const float TRANS_R_wheel = 0.3175; // Radius of the wheel [m]
shovelcat 2:06f128641b62 136 const float TRANS_k = TRANS_k_gear/TRANS_R_wheel; // Global ratio of the transmission [m]
shovelcat 2:06f128641b62 137
shovelcat 2:06f128641b62 138 const float G_forceToTorque = (1/TRANS_k) * TRANS_eff;
shovelcat 2:06f128641b62 139
shovelcat 2:06f128641b62 140 // calculs
shovelcat 2:06f128641b62 141 const float x0 = getMeasuredSpeed();
shovelcat 2:06f128641b62 142 const float y0 = ( (-a1*y1) + (b0*x0) + (b1*x1) ) * G_forceToTorque;
shovelcat 2:06f128641b62 143
shovelcat 2:06f128641b62 144 // update des valeurs anterieurs
shovelcat 2:06f128641b62 145 x1 = x0;
shovelcat 2:06f128641b62 146 y1 = y0;
shovelcat 2:06f128641b62 147
shovelcat 2:06f128641b62 148 return y0;
shovelcat 2:06f128641b62 149 }
shovelcat 2:06f128641b62 150
shovelcat 2:06f128641b62 151
shovelcat 0:011f69c1276c 152 // main transfer function
shovelcat 1:c6b4ccebd483 153 void limvitCallback() {
shovelcat 2:06f128641b62 154 const float referenceSpeed = getReferenceSpeed();
shovelcat 2:06f128641b62 155 float outputAdcVoltageHi = 0;
shovelcat 2:06f128641b62 156 float outputAdcVoltageLo = 0;
shovelcat 1:c6b4ccebd483 157
shovelcat 2:06f128641b62 158 if(referenceSpeed == DISABLE_ECO_ALGO_TRIGGER) {
shovelcat 2:06f128641b62 159 outputAdcVoltageHi = ecoDisabledAlgorithm();
shovelcat 2:06f128641b62 160 }
shovelcat 2:06f128641b62 161 else {
shovelcat 2:06f128641b62 162 outputAdcVoltageHi = ecoEnabledAlgorithm();
shovelcat 2:06f128641b62 163 }
shovelcat 2:06f128641b62 164
shovelcat 2:06f128641b62 165 outputAdcVoltageLo = outputAdcVoltageHi / 2;
shovelcat 2:06f128641b62 166 writeAdcPedalHi(outputAdcVoltageHi);
shovelcat 2:06f128641b62 167 writeAdcPedalLo(outputAdcVoltageLo);
shovelcat 0:011f69c1276c 168 }
shovelcat 0:011f69c1276c 169
shovelcat 1:c6b4ccebd483 170 // Main loop to be run in thread
shovelcat 1:c6b4ccebd483 171 void limvitMain() {
shovelcat 1:c6b4ccebd483 172 // const float period = 0.001;
shovelcat 2:06f128641b62 173 const float period = 0.1;
shovelcat 1:c6b4ccebd483 174 Ticker limvitSync;
shovelcat 1:c6b4ccebd483 175 limvitSync.attach(&limvitCallback, period);
shovelcat 2:06f128641b62 176 while(1);
shovelcat 1:c6b4ccebd483 177 }
shovelcat 0:011f69c1276c 178
shovelcat 1:c6b4ccebd483 179 void canCallback() {
shovelcat 1:c6b4ccebd483 180 CANMessage msg;
shovelcat 1:c6b4ccebd483 181 canLed = true;
shovelcat 1:c6b4ccebd483 182 wait(1.0);
shovelcat 1:c6b4ccebd483 183 canLed = false;
shovelcat 1:c6b4ccebd483 184 int status = can1.read(msg);
shovelcat 1:c6b4ccebd483 185 if(status) {
shovelcat 1:c6b4ccebd483 186 pc.printf("CAN msg received: %x\n\r", msg.data);
shovelcat 1:c6b4ccebd483 187 }
shovelcat 1:c6b4ccebd483 188 else {
shovelcat 1:c6b4ccebd483 189 pc.printf("Error: %d\n\r", status);
shovelcat 1:c6b4ccebd483 190 }
shovelcat 1:c6b4ccebd483 191 }
shovelcat 1:c6b4ccebd483 192
shovelcat 1:c6b4ccebd483 193 void b1Callback() {
shovelcat 1:c6b4ccebd483 194 static bool state = false;
shovelcat 1:c6b4ccebd483 195 pc.printf("User pressed b1.\n\r");
shovelcat 1:c6b4ccebd483 196 if (state) {
shovelcat 1:c6b4ccebd483 197 // reset speed
shovelcat 1:c6b4ccebd483 198 setReferenceSpeed(0.0);
shovelcat 1:c6b4ccebd483 199 state = !state;
shovelcat 1:c6b4ccebd483 200 }
shovelcat 1:c6b4ccebd483 201 else {
shovelcat 1:c6b4ccebd483 202 setReferenceSpeed(30.0);
shovelcat 1:c6b4ccebd483 203 state = !state;
shovelcat 1:c6b4ccebd483 204 }
shovelcat 1:c6b4ccebd483 205 pc.printf("%s\n\r", can1.rderror());
shovelcat 1:c6b4ccebd483 206 canCallback();
shovelcat 1:c6b4ccebd483 207 }
shovelcat 0:011f69c1276c 208
shovelcat 0:011f69c1276c 209 int main() {
shovelcat 2:06f128641b62 210 // SpeedLimiter speedLimiter(PA_3, PC_0, PA_4, PA_5);
shovelcat 2:06f128641b62 211
shovelcat 1:c6b4ccebd483 212 pc.printf("\n\rAnalogIn example\n\r");
shovelcat 2:06f128641b62 213 // setReferenceSpeed(0);
shovelcat 0:011f69c1276c 214 setMeasuredSpeed(0);
shovelcat 1:c6b4ccebd483 215 b1.rise(&b1Callback);
shovelcat 0:011f69c1276c 216
shovelcat 1:c6b4ccebd483 217 // CAN test
shovelcat 1:c6b4ccebd483 218 can1.frequency(500000);
shovelcat 1:c6b4ccebd483 219 can1.attach(&canCallback, CAN::RxIrq);
shovelcat 1:c6b4ccebd483 220
shovelcat 1:c6b4ccebd483 221 limvitMain();
shovelcat 0:011f69c1276c 222 while(1) {
shovelcat 2:06f128641b62 223 pc.printf("Current reference speed: %f\n\r", getReferenceSpeed());
shovelcat 1:c6b4ccebd483 224 wait(1.0);
shovelcat 0:011f69c1276c 225 }
shovelcat 0:011f69c1276c 226 }
shovelcat 2:06f128641b62 227