Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

main.cpp@1:c6b4ccebd483, 2018-10-17 (annotated)

Committer:
shovelcat
Date:
Wed Oct 17 19:10:18 2018 +0000
Revision:
1:c6b4ccebd483
Parent:
0:011f69c1276c
Child:
2:06f128641b62
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shovelcat 0:011f69c1276c 1 #include "mbed.h"
shovelcat 0:011f69c1276c 2
shovelcat 0:011f69c1276c 3 /*
shovelcat 0:011f69c1276c 4 ##############################
shovelcat 1:c6b4ccebd483 5 uC I/O
shovelcat 1:c6b4ccebd483 6 ##############################
shovelcat 1:c6b4ccebd483 7 */
shovelcat 1:c6b4ccebd483 8 AnalogIn pedal_in_hi(PA_3);
shovelcat 1:c6b4ccebd483 9 AnalogIn pedal_in_lo(PC_0);
shovelcat 1:c6b4ccebd483 10 AnalogOut pedal_out_hi(PA_4);
shovelcat 1:c6b4ccebd483 11 AnalogOut pedal_out_lo(PA_5);
shovelcat 1:c6b4ccebd483 12
shovelcat 1:c6b4ccebd483 13 // test i/o
shovelcat 1:c6b4ccebd483 14 InterruptIn b1(PC_13);
shovelcat 1:c6b4ccebd483 15 DigitalOut led(LED1);
shovelcat 1:c6b4ccebd483 16 DigitalOut canLed(LED2);
shovelcat 1:c6b4ccebd483 17 Serial pc(USBTX, USBRX); // for communication / debugging
shovelcat 1:c6b4ccebd483 18 CAN can1(PD_0, PD_1);
shovelcat 1:c6b4ccebd483 19
shovelcat 1:c6b4ccebd483 20 /*
shovelcat 1:c6b4ccebd483 21 ##############################
shovelcat 0:011f69c1276c 22 Constants
shovelcat 0:011f69c1276c 23 ##############################
shovelcat 0:011f69c1276c 24 */
shovelcat 0:011f69c1276c 25 const float ADC_INPUT_MAX_VALUE = 5;
shovelcat 0:011f69c1276c 26 const float ADC_OUTPUT_MAX_VALUE = 5;
shovelcat 0:011f69c1276c 27
shovelcat 0:011f69c1276c 28 const float PEDAL_HI_MIN_VALUE = 0.48;
shovelcat 0:011f69c1276c 29 const float PEDAL_HI_MAX_VALUE = 2.96;
shovelcat 0:011f69c1276c 30 const float PEDAL_LO_MIN_VALUE = 0.25;
shovelcat 0:011f69c1276c 31 const float PEDAL_LO_MAX_VALUE = 1.48;
shovelcat 0:011f69c1276c 32
shovelcat 1:c6b4ccebd483 33 const float SPEED_MAX = 40.0;
shovelcat 1:c6b4ccebd483 34
shovelcat 0:011f69c1276c 35 /*
shovelcat 0:011f69c1276c 36 ##############################
shovelcat 0:011f69c1276c 37 Static variables and functions
shovelcat 0:011f69c1276c 38 ##############################
shovelcat 0:011f69c1276c 39 */
shovelcat 0:011f69c1276c 40 static float __reference_speed = 0;
shovelcat 0:011f69c1276c 41 static float __measured_speed = 0;
shovelcat 0:011f69c1276c 42
shovelcat 0:011f69c1276c 43 static void setReferenceSpeed(const float speed) {
shovelcat 0:011f69c1276c 44 // TODO insert mutex here
shovelcat 0:011f69c1276c 45 __reference_speed = speed;
shovelcat 0:011f69c1276c 46 }
shovelcat 0:011f69c1276c 47
shovelcat 0:011f69c1276c 48 static void setMeasuredSpeed(const float speed) {
shovelcat 0:011f69c1276c 49 // TODO insert mutex here
shovelcat 0:011f69c1276c 50 __measured_speed = speed;
shovelcat 0:011f69c1276c 51 }
shovelcat 0:011f69c1276c 52
shovelcat 0:011f69c1276c 53 static float getReferenceSpeed() {
shovelcat 0:011f69c1276c 54 // TODO insert mutex here
shovelcat 0:011f69c1276c 55 return __reference_speed;
shovelcat 0:011f69c1276c 56 }
shovelcat 0:011f69c1276c 57 static float getMeasuredSpeed() {
shovelcat 0:011f69c1276c 58 // TODO insert mutex here
shovelcat 0:011f69c1276c 59 return __measured_speed;
shovelcat 0:011f69c1276c 60 }
shovelcat 0:011f69c1276c 61
shovelcat 0:011f69c1276c 62 /*
shovelcat 0:011f69c1276c 63 ##############################
shovelcat 0:011f69c1276c 64 Utility functions
shovelcat 0:011f69c1276c 65 ##############################
shovelcat 0:011f69c1276c 66 */
shovelcat 0:011f69c1276c 67
shovelcat 0:011f69c1276c 68 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
shovelcat 0:011f69c1276c 69 float boundValue(float value, const float lowerBound, const float upperBound) {
shovelcat 0:011f69c1276c 70 if(value < lowerBound) {
shovelcat 0:011f69c1276c 71 value = lowerBound;
shovelcat 0:011f69c1276c 72 }
shovelcat 0:011f69c1276c 73 else if(value > upperBound) {
shovelcat 0:011f69c1276c 74 value = upperBound;
shovelcat 0:011f69c1276c 75 }
shovelcat 0:011f69c1276c 76 return value;
shovelcat 0:011f69c1276c 77 }
shovelcat 0:011f69c1276c 78
shovelcat 0:011f69c1276c 79 // Returns "value/reference" as a percentage in decimal form (0.5 for 50%)
shovelcat 0:011f69c1276c 80 float toDecimalPercent(const float value, const float reference) {
shovelcat 0:011f69c1276c 81 return value/reference;
shovelcat 0:011f69c1276c 82 }
shovelcat 0:011f69c1276c 83
shovelcat 0:011f69c1276c 84 // Returns voltage read on analog input port chosen for pedal input 1
shovelcat 0:011f69c1276c 85 float readAdcPedalHi() {
shovelcat 0:011f69c1276c 86 const float decPcValue = pedal_in_hi.read();
shovelcat 0:011f69c1276c 87 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 0:011f69c1276c 88 return voltage;
shovelcat 0:011f69c1276c 89 }
shovelcat 0:011f69c1276c 90
shovelcat 0:011f69c1276c 91 // Returns voltage read on analog input port chosen for pedal input 2
shovelcat 0:011f69c1276c 92 float readAdcPedalLo() {
shovelcat 0:011f69c1276c 93 const float decPcValue = pedal_in_lo.read();
shovelcat 0:011f69c1276c 94 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 0:011f69c1276c 95 return voltage;
shovelcat 0:011f69c1276c 96 }
shovelcat 0:011f69c1276c 97
shovelcat 0:011f69c1276c 98
shovelcat 0:011f69c1276c 99 // Accepts a value in volts, converts to % and sets ADC for pedal output 1
shovelcat 0:011f69c1276c 100 void writeAdcPedalHi(const float voltage) {
shovelcat 0:011f69c1276c 101 const float boundedValue = boundValue(voltage, PEDAL_HI_MIN_VALUE, PEDAL_HI_MAX_VALUE);
shovelcat 0:011f69c1276c 102 const float decPcValue = toDecimalPercent(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 0:011f69c1276c 103 pedal_out_hi.write(decPcValue);
shovelcat 0:011f69c1276c 104 }
shovelcat 0:011f69c1276c 105
shovelcat 0:011f69c1276c 106 // Accepts a value in volts, converts to % and sets ADC for pedal output 2
shovelcat 0:011f69c1276c 107 void writeAdcPedalLo(const float voltage) {
shovelcat 0:011f69c1276c 108 const float boundedValue = boundValue(voltage, PEDAL_LO_MIN_VALUE, PEDAL_LO_MAX_VALUE);
shovelcat 0:011f69c1276c 109 const float decPcValue = toDecimalPercent(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 0:011f69c1276c 110 pedal_out_lo.write(decPcValue);
shovelcat 0:011f69c1276c 111 }
shovelcat 0:011f69c1276c 112
shovelcat 0:011f69c1276c 113 // main transfer function
shovelcat 1:c6b4ccebd483 114 void limvitCallback() {
shovelcat 1:c6b4ccebd483 115 // const float T_gb_r = (0.3175*2/2.0)/20.5;
shovelcat 1:c6b4ccebd483 116 //
shovelcat 1:c6b4ccebd483 117 // // constantes du controleur IP (Ziegler-Nichols method)
shovelcat 1:c6b4ccebd483 118 // const float Ki = 0;
shovelcat 0:011f69c1276c 119 // const float Kp = 1.0;
shovelcat 1:c6b4ccebd483 120 //
shovelcat 1:c6b4ccebd483 121 // // facteur de conversion
shovelcat 1:c6b4ccebd483 122 //// const float pedal_HI_range = PEDAL_HI_MAX_VALUE - PEDAL_HI_MIN_VALUE;
shovelcat 0:011f69c1276c 123 //
shovelcat 1:c6b4ccebd483 124 // const float speedVoltageConversionFactor = PEDAL_HI_MAX_VALUE / SPEED_MAX;
shovelcat 1:c6b4ccebd483 125 // const float Kc = T_gb_r * speedVoltageConversionFactor;
shovelcat 1:c6b4ccebd483 126 //
shovelcat 1:c6b4ccebd483 127 // // algorithme
shovelcat 0:011f69c1276c 128 // const float Vref = getReferenceSpeed();
shovelcat 0:011f69c1276c 129 // const float Vmes = getMeasuredSpeed();
shovelcat 0:011f69c1276c 130 // const float err = Vref - Vmes;
shovelcat 0:011f69c1276c 131 // const float errKi = err * Ki;
shovelcat 0:011f69c1276c 132 // const float err2 = errKi - Vmes;
shovelcat 1:c6b4ccebd483 133 // const float udHi = err2 * Kp * Kc;
shovelcat 1:c6b4ccebd483 134 // const float udLo = (float)udHi / 2;
shovelcat 1:c6b4ccebd483 135 //
shovelcat 1:c6b4ccebd483 136 //// const float umHi = readAdcPedalHi();
shovelcat 1:c6b4ccebd483 137 //// const float umLo = readAdcPedalLo();
shovelcat 1:c6b4ccebd483 138 // writeAdcPedalHi(udHi);
shovelcat 1:c6b4ccebd483 139 //// writeAdcPedalLo(udLo);
shovelcat 1:c6b4ccebd483 140 // writeAdcPedalLo(Vref * speedVoltageConversionFactor);
shovelcat 1:c6b4ccebd483 141 //
shovelcat 1:c6b4ccebd483 142 //
shovelcat 1:c6b4ccebd483 143 // // test
shovelcat 1:c6b4ccebd483 144 // setMeasuredSpeed(udHi / speedVoltageConversionFactor);
shovelcat 1:c6b4ccebd483 145 //
shovelcat 1:c6b4ccebd483 146
shovelcat 1:c6b4ccebd483 147 const float tmpHi = readAdcPedalHi();
shovelcat 1:c6b4ccebd483 148 const float tmpLo = readAdcPedalLo();
shovelcat 1:c6b4ccebd483 149 // pc.printf("Callback\n\r");
shovelcat 1:c6b4ccebd483 150 // pc.printf("Hi: %f\n\r", tmpHi);
shovelcat 1:c6b4ccebd483 151 // pc.printf("Lo: %f\n\r", tmpLo);
shovelcat 1:c6b4ccebd483 152
shovelcat 1:c6b4ccebd483 153 // fonction suiveur
shovelcat 1:c6b4ccebd483 154 writeAdcPedalHi(tmpHi);
shovelcat 1:c6b4ccebd483 155 writeAdcPedalLo(tmpLo);
shovelcat 1:c6b4ccebd483 156
shovelcat 0:011f69c1276c 157 }
shovelcat 0:011f69c1276c 158
shovelcat 1:c6b4ccebd483 159 // Main loop to be run in thread
shovelcat 1:c6b4ccebd483 160 void limvitMain() {
shovelcat 1:c6b4ccebd483 161 // const float period = 0.001;
shovelcat 1:c6b4ccebd483 162 const float period = 0.5;
shovelcat 1:c6b4ccebd483 163 Ticker limvitSync;
shovelcat 1:c6b4ccebd483 164 limvitSync.attach(&limvitCallback, period);
shovelcat 1:c6b4ccebd483 165
shovelcat 1:c6b4ccebd483 166 // while(1) {
shovelcat 1:c6b4ccebd483 167 // led = !led;
shovelcat 1:c6b4ccebd483 168 // wait(1.0);
shovelcat 1:c6b4ccebd483 169 // }
shovelcat 1:c6b4ccebd483 170 }
shovelcat 0:011f69c1276c 171
shovelcat 1:c6b4ccebd483 172 void canCallback() {
shovelcat 1:c6b4ccebd483 173 CANMessage msg;
shovelcat 1:c6b4ccebd483 174 canLed = true;
shovelcat 1:c6b4ccebd483 175 wait(1.0);
shovelcat 1:c6b4ccebd483 176 canLed = false;
shovelcat 1:c6b4ccebd483 177 int status = can1.read(msg);
shovelcat 1:c6b4ccebd483 178 if(status) {
shovelcat 1:c6b4ccebd483 179 pc.printf("CAN msg received: %x\n\r", msg.data);
shovelcat 1:c6b4ccebd483 180 }
shovelcat 1:c6b4ccebd483 181 else {
shovelcat 1:c6b4ccebd483 182 pc.printf("Error: %d\n\r", status);
shovelcat 1:c6b4ccebd483 183 }
shovelcat 1:c6b4ccebd483 184 }
shovelcat 1:c6b4ccebd483 185
shovelcat 1:c6b4ccebd483 186 void b1Callback() {
shovelcat 1:c6b4ccebd483 187 static bool state = false;
shovelcat 1:c6b4ccebd483 188 pc.printf("User pressed b1.\n\r");
shovelcat 1:c6b4ccebd483 189 if (state) {
shovelcat 1:c6b4ccebd483 190 // reset speed
shovelcat 1:c6b4ccebd483 191 setReferenceSpeed(0.0);
shovelcat 1:c6b4ccebd483 192 state = !state;
shovelcat 1:c6b4ccebd483 193 }
shovelcat 1:c6b4ccebd483 194 else {
shovelcat 1:c6b4ccebd483 195 setReferenceSpeed(30.0);
shovelcat 1:c6b4ccebd483 196 state = !state;
shovelcat 1:c6b4ccebd483 197 }
shovelcat 1:c6b4ccebd483 198 pc.printf("%s\n\r", can1.rderror());
shovelcat 1:c6b4ccebd483 199 canCallback();
shovelcat 1:c6b4ccebd483 200 }
shovelcat 0:011f69c1276c 201
shovelcat 0:011f69c1276c 202 int main() {
shovelcat 1:c6b4ccebd483 203 pc.printf("\n\rAnalogIn example\n\r");
shovelcat 0:011f69c1276c 204 setReferenceSpeed(0);
shovelcat 0:011f69c1276c 205 setMeasuredSpeed(0);
shovelcat 1:c6b4ccebd483 206 b1.rise(&b1Callback);
shovelcat 0:011f69c1276c 207
shovelcat 1:c6b4ccebd483 208 // CAN test
shovelcat 1:c6b4ccebd483 209 can1.frequency(500000);
shovelcat 1:c6b4ccebd483 210 can1.attach(&canCallback, CAN::RxIrq);
shovelcat 1:c6b4ccebd483 211
shovelcat 1:c6b4ccebd483 212 limvitMain();
shovelcat 0:011f69c1276c 213 while(1) {
shovelcat 1:c6b4ccebd483 214 wait(1.0);
shovelcat 0:011f69c1276c 215 }
shovelcat 0:011f69c1276c 216 }