eBike / Mbed 2 deprecated ENCODER_TEST3_peddep

ti bisogna il phaserunner

Dependencies:   mbed PID mbed-rtos

main.cpp@11:39bd79605827, 2019-06-04 (annotated)

Committer:
beacon
Date:
Tue Jun 04 19:03:39 2019 +0000
Revision:
11:39bd79605827
Parent:
10:b7a44c4a4ef6 
ti bisogna il phaserunner

Who changed what in which revision?

UserRevisionLine numberNew 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
beacon 11:39bd79605827 81 linkspedal.setTorque(0);
beacon 11:39bd79605827 82 rechtspedal.setTorque(0);
beacon 11:39bd79605827 83 vorderrad.setTorque(0);
beacon 11:39bd79605827 84 hinterrad.setTorque(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;
beacon 11:39bd79605827 120 PID pid(Kp, Ki, Kd, dt, true);
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){
beacon 11:39bd79605827 137 rechtspedal.setTorque(80);
EpicG10 7:15e6fc689368 138 init = true;
EpicG10 9:56aed8c6779f 139 wait(0.04);
beacon 11:39bd79605827 140 rechtspedal.setTorque(3);
EpicG10 7:15e6fc689368 141 }
EpicG10 7:15e6fc689368 142 }
beacon 11:39bd79605827 143 rechtspedal.setTorque(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){
beacon 11:39bd79605827 219 rechtspedal.setTorque(3);
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){
beacon 11:39bd79605827 224 rechtspedal.setTorque(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))) {
beacon 11:39bd79605827 237 vorderrad.setRecuperation(0);
beacon 11:39bd79605827 238 hinterrad.setRecuperation(0);
EpicG10 10:b7a44c4a4ef6 239 // write torque to phaserunner;
beacon 11:39bd79605827 240 //vorderrad.setRecuperation(uint16_t((80+(torque))*40.96f));
beacon 11:39bd79605827 241 vorderrad.setTorque(uint16_t(10+torque));
beacon 11:39bd79605827 242 //hinterrad.setTorque(uint16_t(20+torque));
beacon 11:39bd79605827 243 //vorderrad.setTorque(0);
beacon 11:39bd79605827 244 hinterrad.setTorque(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
beacon 11:39bd79605827 259 vorderrad.setTorque(0);
beacon 11:39bd79605827 260 hinterrad.setTorque(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){
beacon 11:39bd79605827 266 vorderrad.setTorque(0);
beacon 11:39bd79605827 267 hinterrad.setTorque(0);
EpicG10 10:b7a44c4a4ef6 268 wait_ms(5);
beacon 11:39bd79605827 269 vorderrad.setRecuperation(daumen);
beacon 11:39bd79605827 270 hinterrad.setRecuperation(daumen);
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
beacon 11:39bd79605827 280 vorderrad.setTorque(0);
beacon 11:39bd79605827 281 hinterrad.setTorque(0);
EpicG10 10:b7a44c4a4ef6 282 wait_ms(5);
beacon 11:39bd79605827 283 vorderrad.setRecuperation(10);
beacon 11:39bd79605827 284 hinterrad.setRecuperation(10);
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;
beacon 11:39bd79605827 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