ti bisogna il phaserunner
Dependencies: mbed PID mbed-rtos
main.cpp@10:b7a44c4a4ef6, 2019-05-29 (annotated)
- Committer:
- EpicG10
- Date:
- Wed May 29 18:57:39 2019 +0000
- Revision:
- 10:b7a44c4a4ef6
- Parent:
- 9:56aed8c6779f
- Child:
- 11:39bd79605827
; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
EpicG10 | 0:8a660654d511 | 1 | #include "mbed.h" |
EpicG10 | 0:8a660654d511 | 2 | #include "Encoder.h" |
EpicG10 | 0:8a660654d511 | 3 | #include "math.h" |
EpicG10 | 0:8a660654d511 | 4 | #include "Phaserunner.h" |
EpicG10 | 6:a80300ee574d | 5 | #include "Daumenbetaetigung.h" |
EpicG10 | 7:15e6fc689368 | 6 | #include "Handgriffbetaetigung.h" |
EpicG10 | 7:15e6fc689368 | 7 | #include "PID.h" |
EpicG10 | 5:57fbb5d30d3d | 8 | #define PI 3.14159 |
EpicG10 | 6:a80300ee574d | 9 | |
EpicG10 | 6:a80300ee574d | 10 | |
EpicG10 | 6:a80300ee574d | 11 | |
EpicG10 | 0:8a660654d511 | 12 | int main(){ |
EpicG10 | 7:15e6fc689368 | 13 | |
EpicG10 | 9:56aed8c6779f | 14 | RawSerial motorV(PA_0, PA_1, 115200); |
EpicG10 | 9:56aed8c6779f | 15 | RawSerial motorH(PC_6, PC_7, 115200); |
EpicG10 | 6:a80300ee574d | 16 | RawSerial pedalL(PC_10, PC_11, 115200); |
EpicG10 | 6:a80300ee574d | 17 | RawSerial pedalR(PC_12, PD_2, 115200); |
EpicG10 | 9:56aed8c6779f | 18 | |
EpicG10 | 9:56aed8c6779f | 19 | Phaserunner vorderrad(motorV); |
EpicG10 | 9:56aed8c6779f | 20 | Phaserunner hinterrad(motorH); |
EpicG10 | 6:a80300ee574d | 21 | Phaserunner linkspedal(pedalL); |
EpicG10 | 6:a80300ee574d | 22 | Phaserunner rechtspedal(pedalR); |
EpicG10 | 6:a80300ee574d | 23 | |
EpicG10 | 6:a80300ee574d | 24 | Daumenbetaetigung daumen; |
EpicG10 | 6:a80300ee574d | 25 | Handgriffbetaetigung gasGlied; |
EpicG10 | 6:a80300ee574d | 26 | |
EpicG10 | 6:a80300ee574d | 27 | |
EpicG10 | 6:a80300ee574d | 28 | AnalogIn Strom(PB_0); |
EpicG10 | 6:a80300ee574d | 29 | AnalogIn Spannung(PC_1); |
EpicG10 | 6:a80300ee574d | 30 | AnalogOut GegenMomLinks(PA_4); |
EpicG10 | 6:a80300ee574d | 31 | AnalogOut GegenMomRechts(PA_5); |
EpicG10 | 6:a80300ee574d | 32 | |
EpicG10 | 0:8a660654d511 | 33 | DigitalOut PedalL_ON_OFF(PC_14); |
EpicG10 | 0:8a660654d511 | 34 | DigitalOut PedalR_ON_OFF(PC_15); |
EpicG10 | 6:a80300ee574d | 35 | DigitalIn Taste(PC_8); |
EpicG10 | 0:8a660654d511 | 36 | |
EpicG10 | 7:15e6fc689368 | 37 | PinName Hallsensor(PA_13); |
EpicG10 | 7:15e6fc689368 | 38 | Encoder encoder(Hallsensor); |
EpicG10 | 7:15e6fc689368 | 39 | |
EpicG10 | 0:8a660654d511 | 40 | Serial pc(USBTX,USBRX); |
EpicG10 | 9:56aed8c6779f | 41 | pc.baud(19200); |
EpicG10 | 0:8a660654d511 | 42 | DigitalOut led(LED2); |
EpicG10 | 0:8a660654d511 | 43 | DigitalOut ON_OFF(PH_1); |
EpicG10 | 0:8a660654d511 | 44 | |
EpicG10 | 0:8a660654d511 | 45 | DigitalOut Abvorne(PB_7); |
EpicG10 | 0:8a660654d511 | 46 | DigitalOut Abhinten(PC_13); |
EpicG10 | 9:56aed8c6779f | 47 | |
EpicG10 | 9:56aed8c6779f | 48 | DigitalIn Bremsen(PA_14); |
EpicG10 | 9:56aed8c6779f | 49 | Bremsen.mode(PullDown); |
EpicG10 | 9:56aed8c6779f | 50 | AnalogIn daumen1(A5); //qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqq |
EpicG10 | 9:56aed8c6779f | 51 | |
EpicG10 | 0:8a660654d511 | 52 | ON_OFF = true; |
EpicG10 | 7:15e6fc689368 | 53 | uint16_t rot,rotOld,daumenValue; |
EpicG10 | 6:a80300ee574d | 54 | double angle_rad, angleOld=0; |
EpicG10 | 6:a80300ee574d | 55 | float a=0.0f,b=0.0f,beta=0.0f; // Ellipse Parameters |
EpicG10 | 6:a80300ee574d | 56 | float R=0.0f, phi=0.0f, Rold=0.0f; |
EpicG10 | 9:56aed8c6779f | 57 | float strom, spannung, leistung1, leistung2; //1:Elektrische Lesitung Batterie, 2:Mechanische (M*omega), 3: Leistung aus den Phaserunner |
EpicG10 | 6:a80300ee574d | 58 | float leistungsOffset = 15.0f; // Leerlauf Leistung |
EpicG10 | 7:15e6fc689368 | 59 | float RPM, Acc; // Value form Encoder |
EpicG10 | 7:15e6fc689368 | 60 | float MessungP[4001], MessungFreq[4001],MessungAngle[4001], MessungPID[4001]; |
EpicG10 | 7:15e6fc689368 | 61 | float rec=0.0f; // recuperation 0..100% |
EpicG10 | 7:15e6fc689368 | 62 | float recAntret; |
EpicG10 | 6:a80300ee574d | 63 | uint32_t i=0; |
EpicG10 | 0:8a660654d511 | 64 | |
EpicG10 | 6:a80300ee574d | 65 | led = false; |
EpicG10 | 6:a80300ee574d | 66 | pc.printf("Hello bike\n\r"); |
EpicG10 | 0:8a660654d511 | 67 | |
EpicG10 | 7:15e6fc689368 | 68 | |
EpicG10 | 0:8a660654d511 | 69 | PedalL_ON_OFF = true; |
EpicG10 | 0:8a660654d511 | 70 | PedalR_ON_OFF = true; |
EpicG10 | 0:8a660654d511 | 71 | wait(0.2); |
EpicG10 | 0:8a660654d511 | 72 | PedalL_ON_OFF = false; |
EpicG10 | 0:8a660654d511 | 73 | PedalR_ON_OFF = false; |
EpicG10 | 0:8a660654d511 | 74 | wait(0.2); |
EpicG10 | 0:8a660654d511 | 75 | PedalL_ON_OFF = true; |
EpicG10 | 0:8a660654d511 | 76 | PedalR_ON_OFF = true; |
EpicG10 | 6:a80300ee574d | 77 | float LeerlaufLeistung = 15.0f; |
EpicG10 | 6:a80300ee574d | 78 | pc.printf("Siamo accesi\n\r"); |
EpicG10 | 6:a80300ee574d | 79 | wait(1); |
EpicG10 | 5:57fbb5d30d3d | 80 | |
EpicG10 | 7:15e6fc689368 | 81 | linkspedal.sendBuffer(495,0); |
EpicG10 | 7:15e6fc689368 | 82 | rechtspedal.sendBuffer(495,0); |
EpicG10 | 9:56aed8c6779f | 83 | vorderrad.sendBuffer(495,0); |
EpicG10 | 9:56aed8c6779f | 84 | hinterrad.sendBuffer(495,0); |
EpicG10 | 7:15e6fc689368 | 85 | wait(0.2); |
EpicG10 | 7:15e6fc689368 | 86 | |
EpicG10 | 9:56aed8c6779f | 87 | // Pedelec Values |
EpicG10 | 9:56aed8c6779f | 88 | float torque, torqueOld=0.0f; // Torque an den Räder |
EpicG10 | 9:56aed8c6779f | 89 | float pedFaktor; // Faktor zwischen PedaleLeistung und Moment an der Rädern |
EpicG10 | 9:56aed8c6779f | 90 | |
EpicG10 | 9:56aed8c6779f | 91 | |
EpicG10 | 7:15e6fc689368 | 92 | // Lowpass Filter Leistung |
EpicG10 | 6:a80300ee574d | 93 | float T_ab = 0.005; // Abtastzeit: 5ms |
EpicG10 | 6:a80300ee574d | 94 | float F_ab = 1/T_ab; // Abtastfrequenz |
EpicG10 | 9:56aed8c6779f | 95 | float Omega_c = 2*PI*F_ab/1000; // 1000 Mal kleiner als die Abtastfrequenz |
EpicG10 | 6:a80300ee574d | 96 | float sf = (Omega_c*T_ab)/(1+Omega_c*T_ab);//smoothing factor |
EpicG10 | 9:56aed8c6779f | 97 | float F_Leistung1, F_Leistung1Old = 0.0f; |
EpicG10 | 7:15e6fc689368 | 98 | |
EpicG10 | 7:15e6fc689368 | 99 | // Lowpass Filter RPM |
EpicG10 | 7:15e6fc689368 | 100 | float Omega_cRPM = 2*PI*F_ab/150; // 150 Mal kleiner als die Abtastfrequenz |
EpicG10 | 7:15e6fc689368 | 101 | float sfRPM = (Omega_cRPM*T_ab)/(1+Omega_cRPM*T_ab);//smoothing factor |
EpicG10 | 7:15e6fc689368 | 102 | float F_RPM, F_RPMOld = 0.0f; |
EpicG10 | 7:15e6fc689368 | 103 | |
EpicG10 | 9:56aed8c6779f | 104 | // Lowpass Filter Mech.Leistung |
EpicG10 | 9:56aed8c6779f | 105 | float Omega_cPMech = 2*PI*F_ab/500; // 500 Mal kleiner als die Abtastfrequenz |
EpicG10 | 9:56aed8c6779f | 106 | float sfPMech = (Omega_cPMech*T_ab)/(1+Omega_cPMech*T_ab);//smoothing factor |
EpicG10 | 9:56aed8c6779f | 107 | float F_PMech, F_PMechOld = 0.0f; |
EpicG10 | 9:56aed8c6779f | 108 | |
EpicG10 | 7:15e6fc689368 | 109 | // Lowpass Filter Acceleration |
EpicG10 | 7:15e6fc689368 | 110 | float Omega_cAcc = 2*PI*F_ab/200; // 200 Mal kleiner als die Abtastfrequenz |
EpicG10 | 7:15e6fc689368 | 111 | float sfAcc = (Omega_cAcc*T_ab)/(1+Omega_cAcc*T_ab);//smoothing factor |
EpicG10 | 7:15e6fc689368 | 112 | float F_Acc, F_AccOld = 0.0f; |
EpicG10 | 7:15e6fc689368 | 113 | |
EpicG10 | 6:a80300ee574d | 114 | //Diskrete Ableitung |
EpicG10 | 6:a80300ee574d | 115 | float dt = 0.005f; //5ms |
EpicG10 | 6:a80300ee574d | 116 | float PedaleFreq = 0.0f,PedaleFreqOld = 0.0f, PedaleFreqFil, PedaleFreqFilOld=0.0f; // in rad/s |
EpicG10 | 7:15e6fc689368 | 117 | |
EpicG10 | 7:15e6fc689368 | 118 | // PID instance |
EpicG10 | 9:56aed8c6779f | 119 | float Kp = 1.2, Ki = 40, Kd = 0.0; |
EpicG10 | 7:15e6fc689368 | 120 | PID pid(Kp, Ki, Kd, dt); |
EpicG10 | 7:15e6fc689368 | 121 | pid.setInputLimits(0.0,200.0); |
EpicG10 | 7:15e6fc689368 | 122 | pid.setOutputLimits(1.0,100.0); |
EpicG10 | 7:15e6fc689368 | 123 | pid.setMode(1); //Regulator |
EpicG10 | 7:15e6fc689368 | 124 | |
EpicG10 | 7:15e6fc689368 | 125 | // Antretvariablen |
EpicG10 | 7:15e6fc689368 | 126 | uint16_t verlauf=0; |
EpicG10 | 9:56aed8c6779f | 127 | float tau=0.8f; // tau für exponentiel funktion in s : 0.8s; |
EpicG10 | 7:15e6fc689368 | 128 | |
EpicG10 | 7:15e6fc689368 | 129 | bool init1= false, init = false; |
EpicG10 | 9:56aed8c6779f | 130 | bool richtung1= false, richtung2 = false; |
EpicG10 | 9:56aed8c6779f | 131 | bool writeNull = false; // write 0 to phaseerunner one time |
EpicG10 | 9:56aed8c6779f | 132 | int8_t write=0, writeArr[4001]; |
EpicG10 | 9:56aed8c6779f | 133 | bool BremsenOld = false,BremsenOld2 = false; |
EpicG10 | 6:a80300ee574d | 134 | |
EpicG10 | 7:15e6fc689368 | 135 | while(!encoder.reset()){ |
EpicG10 | 7:15e6fc689368 | 136 | if(!init){ |
EpicG10 | 9:56aed8c6779f | 137 | rechtspedal.sendBuffer(495,0.8*4096); |
EpicG10 | 7:15e6fc689368 | 138 | init = true; |
EpicG10 | 9:56aed8c6779f | 139 | wait(0.04); |
EpicG10 | 7:15e6fc689368 | 140 | rechtspedal.sendBuffer(495,0.03*4096); |
EpicG10 | 7:15e6fc689368 | 141 | } |
EpicG10 | 7:15e6fc689368 | 142 | } |
EpicG10 | 9:56aed8c6779f | 143 | rechtspedal.sendBuffer(495,0); |
EpicG10 | 9:56aed8c6779f | 144 | wait(0.1); |
EpicG10 | 7:15e6fc689368 | 145 | init = false; |
EpicG10 | 9:56aed8c6779f | 146 | float leistungsfaktor = 4.8; // Faktor zwischen Elektrischeleistung und mechanische Leistung % |
EpicG10 | 9:56aed8c6779f | 147 | |
EpicG10 | 6:a80300ee574d | 148 | while(1){ |
EpicG10 | 8:1655d27152e6 | 149 | // while(i<4000){ |
EpicG10 | 7:15e6fc689368 | 150 | |
EpicG10 | 7:15e6fc689368 | 151 | |
EpicG10 | 6:a80300ee574d | 152 | // Measure data form sensors |
EpicG10 | 6:a80300ee574d | 153 | angle_rad = encoder.readAngle(); |
EpicG10 | 7:15e6fc689368 | 154 | RPM = encoder.readRPM(); |
EpicG10 | 7:15e6fc689368 | 155 | Acc = encoder.readAcceleration(); |
EpicG10 | 6:a80300ee574d | 156 | strom = ((Strom.read()*3.3f)-2.5f)/0.025f; |
EpicG10 | 6:a80300ee574d | 157 | spannung = (Spannung.read()*42.95f); |
EpicG10 | 9:56aed8c6779f | 158 | leistung1 = strom*spannung-leistungsOffset; |
EpicG10 | 6:a80300ee574d | 159 | |
EpicG10 | 7:15e6fc689368 | 160 | // LowPass Filter leistung |
EpicG10 | 9:56aed8c6779f | 161 | F_Leistung1 = sf * leistung1 + (1-sf)*F_Leistung1Old; |
EpicG10 | 6:a80300ee574d | 162 | |
EpicG10 | 7:15e6fc689368 | 163 | // LowPass Filter RPM |
EpicG10 | 7:15e6fc689368 | 164 | F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld; |
EpicG10 | 7:15e6fc689368 | 165 | |
EpicG10 | 7:15e6fc689368 | 166 | // LowPass Filter ACC |
EpicG10 | 7:15e6fc689368 | 167 | F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld; |
EpicG10 | 6:a80300ee574d | 168 | |
EpicG10 | 7:15e6fc689368 | 169 | // Regulator |
EpicG10 | 7:15e6fc689368 | 170 | //if(F_RPM > 1.1 * pid.getSetPoint()) pid.setSetPoint(F_RPM); |
EpicG10 | 7:15e6fc689368 | 171 | //else if (F_RPM < 0.9f * pid.getSetPoint()) pid.setSetPoint(F_RPM); |
EpicG10 | 9:56aed8c6779f | 172 | pid.setSetPoint(70.0); |
EpicG10 | 9:56aed8c6779f | 173 | pid.setProcessValue(F_RPM); |
EpicG10 | 7:15e6fc689368 | 174 | |
EpicG10 | 7:15e6fc689368 | 175 | |
EpicG10 | 7:15e6fc689368 | 176 | |
EpicG10 | 7:15e6fc689368 | 177 | |
EpicG10 | 7:15e6fc689368 | 178 | // Ellipse |
EpicG10 | 7:15e6fc689368 | 179 | a = 1.0f; // % of Torque Max 0..1 |
EpicG10 | 7:15e6fc689368 | 180 | b = 0.6f; // % of Torque Max 0..1 |
EpicG10 | 7:15e6fc689368 | 181 | beta = 0.52f; // 30° |
EpicG10 | 6:a80300ee574d | 182 | phi = angle_rad; |
EpicG10 | 6:a80300ee574d | 183 | R = sqrt(pow(a,2) * pow(sin(beta + phi),2) + pow(b,2) * pow(cos(beta + phi),2)); // Torque in function of the Ellipse parameters |
EpicG10 | 9:56aed8c6779f | 184 | daumenValue = 10; |
EpicG10 | 7:15e6fc689368 | 185 | if(daumenValue < 1) daumenValue = 1; |
EpicG10 | 9:56aed8c6779f | 186 | rec = ((float)daumenValue + pid.getCalculation())*R; |
EpicG10 | 6:a80300ee574d | 187 | |
EpicG10 | 7:15e6fc689368 | 188 | // Antret |
EpicG10 | 9:56aed8c6779f | 189 | if(!Bremsen && !init1){ |
EpicG10 | 9:56aed8c6779f | 190 | init1 = true; |
EpicG10 | 9:56aed8c6779f | 191 | } |
EpicG10 | 9:56aed8c6779f | 192 | else recAntret = 100.0; |
EpicG10 | 7:15e6fc689368 | 193 | if(!init && init1){ |
EpicG10 | 9:56aed8c6779f | 194 | recAntret = 100*exp(-(dt*verlauf)/ tau); |
EpicG10 | 7:15e6fc689368 | 195 | verlauf++; |
EpicG10 | 9:56aed8c6779f | 196 | if(recAntret < rec) init = true; // Use just the ellipse when rec_start < 10% |
EpicG10 | 7:15e6fc689368 | 197 | } |
EpicG10 | 9:56aed8c6779f | 198 | if(!init) rec = recAntret; |
EpicG10 | 7:15e6fc689368 | 199 | else{ |
EpicG10 | 7:15e6fc689368 | 200 | // if(F_Acc > 5.0){ |
EpicG10 | 7:15e6fc689368 | 201 | rec = rec ; |
EpicG10 | 7:15e6fc689368 | 202 | //} |
EpicG10 | 7:15e6fc689368 | 203 | } |
EpicG10 | 9:56aed8c6779f | 204 | |
EpicG10 | 9:56aed8c6779f | 205 | // Mechanische Leistung in % berechnet |
EpicG10 | 9:56aed8c6779f | 206 | leistung2 = 4.0*PI*F_RPM/60.0*rec; // 2 * omega * Gegenmoment in % |
EpicG10 | 9:56aed8c6779f | 207 | leistung2 = leistung2/leistungsfaktor; |
EpicG10 | 9:56aed8c6779f | 208 | |
EpicG10 | 9:56aed8c6779f | 209 | // LowPass Filter Mech Leistung |
EpicG10 | 9:56aed8c6779f | 210 | F_PMech = sfPMech * leistung2 + (1-sfPMech)*F_PMechOld; |
EpicG10 | 9:56aed8c6779f | 211 | |
EpicG10 | 9:56aed8c6779f | 212 | |
EpicG10 | 7:15e6fc689368 | 213 | if(rec < 0.0) rec = 0.0; |
EpicG10 | 9:56aed8c6779f | 214 | GegenMomRechts.write(0.02+(rec*0.01f)); // offset for 0.2V -> ~0.02 |
EpicG10 | 9:56aed8c6779f | 215 | GegenMomLinks.write(0.02+(rec*0.01f)); // offset for 0.2V -> ~0.02 |
EpicG10 | 9:56aed8c6779f | 216 | |
EpicG10 | 9:56aed8c6779f | 217 | // Hilfemoment für Rücktritt steuern |
EpicG10 | 9:56aed8c6779f | 218 | if(F_RPM < -5.0f && !richtung1){ |
EpicG10 | 9:56aed8c6779f | 219 | rechtspedal.sendBuffer(495,0.03*4096); |
EpicG10 | 9:56aed8c6779f | 220 | richtung1 = true; |
EpicG10 | 9:56aed8c6779f | 221 | richtung2 = false; |
EpicG10 | 9:56aed8c6779f | 222 | } |
EpicG10 | 9:56aed8c6779f | 223 | else if(F_RPM > -3.0f && !richtung2){ |
EpicG10 | 9:56aed8c6779f | 224 | rechtspedal.sendBuffer(495,0); |
EpicG10 | 9:56aed8c6779f | 225 | richtung1 = false; |
EpicG10 | 9:56aed8c6779f | 226 | richtung2 = true; |
EpicG10 | 9:56aed8c6779f | 227 | } |
EpicG10 | 9:56aed8c6779f | 228 | |
EpicG10 | 9:56aed8c6779f | 229 | pedFaktor = 2.0f; |
EpicG10 | 9:56aed8c6779f | 230 | |
EpicG10 | 9:56aed8c6779f | 231 | static bool gebremst = false; |
EpicG10 | 9:56aed8c6779f | 232 | static bool recup = false; |
EpicG10 | 9:56aed8c6779f | 233 | |
EpicG10 | 9:56aed8c6779f | 234 | if(F_RPM > 1.0f && !Bremsen && !recup){ |
EpicG10 | 9:56aed8c6779f | 235 | torque = (F_PMech / 300.0 * 100)*pedFaktor; |
EpicG10 | 9:56aed8c6779f | 236 | if(i%10 == 0 && (torque < (torqueOld*0.95) || torque > (torqueOld * 1.05))) { |
EpicG10 | 10:b7a44c4a4ef6 | 237 | vorderrad.sendBuffer(497,0); |
EpicG10 | 10:b7a44c4a4ef6 | 238 | hinterrad.sendBuffer(497,0); |
EpicG10 | 10:b7a44c4a4ef6 | 239 | // write torque to phaserunner; |
EpicG10 | 10:b7a44c4a4ef6 | 240 | //vorderrad.sendBuffer(477,uint16_t((80+(torque))*40.96f)); |
EpicG10 | 10:b7a44c4a4ef6 | 241 | vorderrad.sendBuffer(495,uint16_t((0.10+(torque/100.0f))*4096)); |
EpicG10 | 10:b7a44c4a4ef6 | 242 | //hinterrad.sendBuffer(495,uint16_t((0.20+(torque/100.0f))*4096)); |
EpicG10 | 10:b7a44c4a4ef6 | 243 | //vorderrad.sendBuffer(495,0); |
EpicG10 | 10:b7a44c4a4ef6 | 244 | hinterrad.sendBuffer(495,0); |
EpicG10 | 10:b7a44c4a4ef6 | 245 | write = 10; |
EpicG10 | 9:56aed8c6779f | 246 | torqueOld = torque; |
EpicG10 | 9:56aed8c6779f | 247 | printf("State: torque\n\r"); |
EpicG10 | 9:56aed8c6779f | 248 | |
EpicG10 | 9:56aed8c6779f | 249 | } |
EpicG10 | 9:56aed8c6779f | 250 | else write = 0; |
EpicG10 | 9:56aed8c6779f | 251 | writeNull = false; |
EpicG10 | 7:15e6fc689368 | 252 | |
EpicG10 | 9:56aed8c6779f | 253 | |
EpicG10 | 9:56aed8c6779f | 254 | } |
EpicG10 | 9:56aed8c6779f | 255 | else if(!writeNull){ |
EpicG10 | 9:56aed8c6779f | 256 | writeNull = true; |
EpicG10 | 9:56aed8c6779f | 257 | write = -10; |
EpicG10 | 9:56aed8c6779f | 258 | // write 0 to phaserunner |
EpicG10 | 9:56aed8c6779f | 259 | vorderrad.sendBuffer(495,0); |
EpicG10 | 9:56aed8c6779f | 260 | hinterrad.sendBuffer(495,0); |
EpicG10 | 9:56aed8c6779f | 261 | |
EpicG10 | 9:56aed8c6779f | 262 | } |
EpicG10 | 9:56aed8c6779f | 263 | else write = 0; |
EpicG10 | 9:56aed8c6779f | 264 | |
EpicG10 | 9:56aed8c6779f | 265 | if((daumen.getValue() > Phaserunner::MIN_RECUPERATION)&& !recup){ |
EpicG10 | 9:56aed8c6779f | 266 | vorderrad.sendBuffer(495,0); |
EpicG10 | 9:56aed8c6779f | 267 | hinterrad.sendBuffer(495,0); |
EpicG10 | 10:b7a44c4a4ef6 | 268 | wait_ms(5); |
EpicG10 | 9:56aed8c6779f | 269 | vorderrad.sendBuffer(497,uint16_t((float)daumen.getValue()/100.0f*4096)); |
EpicG10 | 9:56aed8c6779f | 270 | hinterrad.sendBuffer(497,uint16_t((float)daumen.getValue()/100.0f*4096)); |
EpicG10 | 9:56aed8c6779f | 271 | printf("State: recup\n\r"); |
EpicG10 | 9:56aed8c6779f | 272 | recup = true; |
EpicG10 | 9:56aed8c6779f | 273 | } |
EpicG10 | 10:b7a44c4a4ef6 | 274 | else if(daumen.getValue() < Phaserunner::MIN_RECUPERATION){ |
EpicG10 | 10:b7a44c4a4ef6 | 275 | recup = false; |
EpicG10 | 10:b7a44c4a4ef6 | 276 | } |
EpicG10 | 9:56aed8c6779f | 277 | if(Bremsen && !gebremst){ |
EpicG10 | 9:56aed8c6779f | 278 | write = -5; |
EpicG10 | 9:56aed8c6779f | 279 | // write 0 to phaserunner |
EpicG10 | 9:56aed8c6779f | 280 | vorderrad.sendBuffer(495,0); |
EpicG10 | 9:56aed8c6779f | 281 | hinterrad.sendBuffer(495,0); |
EpicG10 | 10:b7a44c4a4ef6 | 282 | wait_ms(5); |
EpicG10 | 9:56aed8c6779f | 283 | vorderrad.sendBuffer(497,uint16_t(0.10f*4096)); |
EpicG10 | 9:56aed8c6779f | 284 | hinterrad.sendBuffer(497,uint16_t(0.10f*4096)); |
EpicG10 | 9:56aed8c6779f | 285 | printf("State: bremsen\n\r"); |
EpicG10 | 9:56aed8c6779f | 286 | gebremst = true; |
EpicG10 | 9:56aed8c6779f | 287 | } |
EpicG10 | 10:b7a44c4a4ef6 | 288 | else if(!Bremsen){ |
EpicG10 | 10:b7a44c4a4ef6 | 289 | gebremst = false; |
EpicG10 | 10:b7a44c4a4ef6 | 290 | } |
EpicG10 | 10:b7a44c4a4ef6 | 291 | |
EpicG10 | 9:56aed8c6779f | 292 | |
EpicG10 | 9:56aed8c6779f | 293 | |
EpicG10 | 9:56aed8c6779f | 294 | int wert1 = daumen; |
EpicG10 | 9:56aed8c6779f | 295 | float wert2 = daumen1.read(); |
EpicG10 | 9:56aed8c6779f | 296 | float u; |
EpicG10 | 9:56aed8c6779f | 297 | if(i%200 == 0){ |
EpicG10 | 9:56aed8c6779f | 298 | i = 0; |
EpicG10 | 9:56aed8c6779f | 299 | vorderrad.readBuffer(262); |
EpicG10 | 9:56aed8c6779f | 300 | u = vorderrad.getVoltage(); |
EpicG10 | 9:56aed8c6779f | 301 | printf("P=%.3f, Torque=%.3f , RPM_Ped =%.3f, U=%.2f\n\r",F_PMech,torque,F_RPM,u); |
EpicG10 | 9:56aed8c6779f | 302 | //printf("Daumen1: %d, Daumen2: %.2f\n\r",wert1,wert2); |
EpicG10 | 9:56aed8c6779f | 303 | } |
EpicG10 | 9:56aed8c6779f | 304 | |
EpicG10 | 9:56aed8c6779f | 305 | |
EpicG10 | 9:56aed8c6779f | 306 | //MessungP[i] = F_PMech; |
EpicG10 | 9:56aed8c6779f | 307 | //MessungPID[i] = torque; |
EpicG10 | 9:56aed8c6779f | 308 | //MessungFreq[i] = F_RPM; |
EpicG10 | 9:56aed8c6779f | 309 | //writeArr[i] = write; |
EpicG10 | 9:56aed8c6779f | 310 | |
EpicG10 | 6:a80300ee574d | 311 | // Store Old Values |
EpicG10 | 6:a80300ee574d | 312 | angleOld = angle_rad; |
EpicG10 | 9:56aed8c6779f | 313 | F_Leistung1Old = F_Leistung1; |
EpicG10 | 7:15e6fc689368 | 314 | F_RPMOld = F_RPM; |
EpicG10 | 7:15e6fc689368 | 315 | F_AccOld = F_Acc; |
EpicG10 | 9:56aed8c6779f | 316 | F_PMechOld = F_PMech; |
EpicG10 | 9:56aed8c6779f | 317 | BremsenOld = Bremsen; |
EpicG10 | 9:56aed8c6779f | 318 | BremsenOld2 = BremsenOld; |
EpicG10 | 9:56aed8c6779f | 319 | |
EpicG10 | 7:15e6fc689368 | 320 | i++; |
EpicG10 | 9:56aed8c6779f | 321 | |
EpicG10 | 9:56aed8c6779f | 322 | wait(dt);// Set to 5 ms |
EpicG10 | 9:56aed8c6779f | 323 | /*} |
EpicG10 | 9:56aed8c6779f | 324 | pc.printf("["); |
EpicG10 | 7:15e6fc689368 | 325 | for(i=0;i<3999;i++){ |
EpicG10 | 9:56aed8c6779f | 326 | pc.printf("%.2f,%.2f,%.2f,%d;...\n\r",MessungP[i],MessungFreq[i],MessungPID[i],writeArr[i]); |
EpicG10 | 7:15e6fc689368 | 327 | } |
EpicG10 | 9:56aed8c6779f | 328 | pc.printf("%.2f,%.2f,%.2f,%d];\n\r",MessungP[3999],MessungFreq[3999],MessungPID[3999],writeArr[3999]); |
EpicG10 | 7:15e6fc689368 | 329 | i=0; |
EpicG10 | 9:56aed8c6779f | 330 | */ |
EpicG10 | 9:56aed8c6779f | 331 | } |
EpicG10 | 9:56aed8c6779f | 332 | |
EpicG10 | 6:a80300ee574d | 333 | } |
EpicG10 | 6:a80300ee574d | 334 |