ti bisogna il phaserunner
Dependencies: mbed PID mbed-rtos
main.cpp
- Committer:
- beacon
- Date:
- 2019-06-04
- Revision:
- 11:39bd79605827
- Parent:
- 10:b7a44c4a4ef6
File content as of revision 11:39bd79605827:
#include "mbed.h" #include "Encoder.h" #include "math.h" #include "Phaserunner.h" #include "Daumenbetaetigung.h" #include "Handgriffbetaetigung.h" #include "PID.h" #define PI 3.14159 int main(){ RawSerial motorV(PA_0, PA_1, 115200); RawSerial motorH(PC_6, PC_7, 115200); RawSerial pedalL(PC_10, PC_11, 115200); RawSerial pedalR(PC_12, PD_2, 115200); Phaserunner vorderrad(motorV); Phaserunner hinterrad(motorH); Phaserunner linkspedal(pedalL); Phaserunner rechtspedal(pedalR); Daumenbetaetigung daumen; Handgriffbetaetigung gasGlied; AnalogIn Strom(PB_0); AnalogIn Spannung(PC_1); AnalogOut GegenMomLinks(PA_4); AnalogOut GegenMomRechts(PA_5); DigitalOut PedalL_ON_OFF(PC_14); DigitalOut PedalR_ON_OFF(PC_15); DigitalIn Taste(PC_8); PinName Hallsensor(PA_13); Encoder encoder(Hallsensor); Serial pc(USBTX,USBRX); pc.baud(19200); DigitalOut led(LED2); DigitalOut ON_OFF(PH_1); DigitalOut Abvorne(PB_7); DigitalOut Abhinten(PC_13); DigitalIn Bremsen(PA_14); Bremsen.mode(PullDown); AnalogIn daumen1(A5); //qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqq ON_OFF = true; uint16_t rot,rotOld,daumenValue; double angle_rad, angleOld=0; float a=0.0f,b=0.0f,beta=0.0f; // Ellipse Parameters float R=0.0f, phi=0.0f, Rold=0.0f; float strom, spannung, leistung1, leistung2; //1:Elektrische Lesitung Batterie, 2:Mechanische (M*omega), 3: Leistung aus den Phaserunner float leistungsOffset = 15.0f; // Leerlauf Leistung float RPM, Acc; // Value form Encoder float MessungP[4001], MessungFreq[4001],MessungAngle[4001], MessungPID[4001]; float rec=0.0f; // recuperation 0..100% float recAntret; uint32_t i=0; led = false; pc.printf("Hello bike\n\r"); PedalL_ON_OFF = true; PedalR_ON_OFF = true; wait(0.2); PedalL_ON_OFF = false; PedalR_ON_OFF = false; wait(0.2); PedalL_ON_OFF = true; PedalR_ON_OFF = true; float LeerlaufLeistung = 15.0f; pc.printf("Siamo accesi\n\r"); wait(1); linkspedal.setTorque(0); rechtspedal.setTorque(0); vorderrad.setTorque(0); hinterrad.setTorque(0); wait(0.2); // Pedelec Values float torque, torqueOld=0.0f; // Torque an den Räder float pedFaktor; // Faktor zwischen PedaleLeistung und Moment an der Rädern // Lowpass Filter Leistung float T_ab = 0.005; // Abtastzeit: 5ms float F_ab = 1/T_ab; // Abtastfrequenz float Omega_c = 2*PI*F_ab/1000; // 1000 Mal kleiner als die Abtastfrequenz float sf = (Omega_c*T_ab)/(1+Omega_c*T_ab);//smoothing factor float F_Leistung1, F_Leistung1Old = 0.0f; // Lowpass Filter RPM float Omega_cRPM = 2*PI*F_ab/150; // 150 Mal kleiner als die Abtastfrequenz float sfRPM = (Omega_cRPM*T_ab)/(1+Omega_cRPM*T_ab);//smoothing factor float F_RPM, F_RPMOld = 0.0f; // Lowpass Filter Mech.Leistung float Omega_cPMech = 2*PI*F_ab/500; // 500 Mal kleiner als die Abtastfrequenz float sfPMech = (Omega_cPMech*T_ab)/(1+Omega_cPMech*T_ab);//smoothing factor float F_PMech, F_PMechOld = 0.0f; // Lowpass Filter Acceleration float Omega_cAcc = 2*PI*F_ab/200; // 200 Mal kleiner als die Abtastfrequenz float sfAcc = (Omega_cAcc*T_ab)/(1+Omega_cAcc*T_ab);//smoothing factor float F_Acc, F_AccOld = 0.0f; //Diskrete Ableitung float dt = 0.005f; //5ms float PedaleFreq = 0.0f,PedaleFreqOld = 0.0f, PedaleFreqFil, PedaleFreqFilOld=0.0f; // in rad/s // PID instance float Kp = 1.2, Ki = 40, Kd = 0.0; PID pid(Kp, Ki, Kd, dt, true); pid.setInputLimits(0.0,200.0); pid.setOutputLimits(1.0,100.0); pid.setMode(1); //Regulator // Antretvariablen uint16_t verlauf=0; float tau=0.8f; // tau für exponentiel funktion in s : 0.8s; bool init1= false, init = false; bool richtung1= false, richtung2 = false; bool writeNull = false; // write 0 to phaseerunner one time int8_t write=0, writeArr[4001]; bool BremsenOld = false,BremsenOld2 = false; while(!encoder.reset()){ if(!init){ rechtspedal.setTorque(80); init = true; wait(0.04); rechtspedal.setTorque(3); } } rechtspedal.setTorque(0); wait(0.1); init = false; float leistungsfaktor = 4.8; // Faktor zwischen Elektrischeleistung und mechanische Leistung % while(1){ // while(i<4000){ // Measure data form sensors angle_rad = encoder.readAngle(); RPM = encoder.readRPM(); Acc = encoder.readAcceleration(); strom = ((Strom.read()*3.3f)-2.5f)/0.025f; spannung = (Spannung.read()*42.95f); leistung1 = strom*spannung-leistungsOffset; // LowPass Filter leistung F_Leistung1 = sf * leistung1 + (1-sf)*F_Leistung1Old; // LowPass Filter RPM F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld; // LowPass Filter ACC F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld; // Regulator //if(F_RPM > 1.1 * pid.getSetPoint()) pid.setSetPoint(F_RPM); //else if (F_RPM < 0.9f * pid.getSetPoint()) pid.setSetPoint(F_RPM); pid.setSetPoint(70.0); pid.setProcessValue(F_RPM); // Ellipse a = 1.0f; // % of Torque Max 0..1 b = 0.6f; // % of Torque Max 0..1 beta = 0.52f; // 30° phi = angle_rad; 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 daumenValue = 10; if(daumenValue < 1) daumenValue = 1; rec = ((float)daumenValue + pid.getCalculation())*R; // Antret if(!Bremsen && !init1){ init1 = true; } else recAntret = 100.0; if(!init && init1){ recAntret = 100*exp(-(dt*verlauf)/ tau); verlauf++; if(recAntret < rec) init = true; // Use just the ellipse when rec_start < 10% } if(!init) rec = recAntret; else{ // if(F_Acc > 5.0){ rec = rec ; //} } // Mechanische Leistung in % berechnet leistung2 = 4.0*PI*F_RPM/60.0*rec; // 2 * omega * Gegenmoment in % leistung2 = leistung2/leistungsfaktor; // LowPass Filter Mech Leistung F_PMech = sfPMech * leistung2 + (1-sfPMech)*F_PMechOld; if(rec < 0.0) rec = 0.0; GegenMomRechts.write(0.02+(rec*0.01f)); // offset for 0.2V -> ~0.02 GegenMomLinks.write(0.02+(rec*0.01f)); // offset for 0.2V -> ~0.02 // Hilfemoment für Rücktritt steuern if(F_RPM < -5.0f && !richtung1){ rechtspedal.setTorque(3); richtung1 = true; richtung2 = false; } else if(F_RPM > -3.0f && !richtung2){ rechtspedal.setTorque(0); richtung1 = false; richtung2 = true; } pedFaktor = 2.0f; static bool gebremst = false; static bool recup = false; if(F_RPM > 1.0f && !Bremsen && !recup){ torque = (F_PMech / 300.0 * 100)*pedFaktor; if(i%10 == 0 && (torque < (torqueOld*0.95) || torque > (torqueOld * 1.05))) { vorderrad.setRecuperation(0); hinterrad.setRecuperation(0); // write torque to phaserunner; //vorderrad.setRecuperation(uint16_t((80+(torque))*40.96f)); vorderrad.setTorque(uint16_t(10+torque)); //hinterrad.setTorque(uint16_t(20+torque)); //vorderrad.setTorque(0); hinterrad.setTorque(0); write = 10; torqueOld = torque; printf("State: torque\n\r"); } else write = 0; writeNull = false; } else if(!writeNull){ writeNull = true; write = -10; // write 0 to phaserunner vorderrad.setTorque(0); hinterrad.setTorque(0); } else write = 0; if((daumen.getValue() > Phaserunner::MIN_RECUPERATION)&& !recup){ vorderrad.setTorque(0); hinterrad.setTorque(0); wait_ms(5); vorderrad.setRecuperation(daumen); hinterrad.setRecuperation(daumen); printf("State: recup\n\r"); recup = true; } else if(daumen.getValue() < Phaserunner::MIN_RECUPERATION){ recup = false; } if(Bremsen && !gebremst){ write = -5; // write 0 to phaserunner vorderrad.setTorque(0); hinterrad.setTorque(0); wait_ms(5); vorderrad.setRecuperation(10); hinterrad.setRecuperation(10); printf("State: bremsen\n\r"); gebremst = true; } else if(!Bremsen){ gebremst = false; } int wert1 = daumen; float wert2 = daumen1.read(); float u; if(i%200 == 0){ i = 0; //vorderrad.readBuffer(262); u = vorderrad.getVoltage(); printf("P=%.3f, Torque=%.3f , RPM_Ped =%.3f, U=%.2f\n\r",F_PMech,torque,F_RPM,u); //printf("Daumen1: %d, Daumen2: %.2f\n\r",wert1,wert2); } //MessungP[i] = F_PMech; //MessungPID[i] = torque; //MessungFreq[i] = F_RPM; //writeArr[i] = write; // Store Old Values angleOld = angle_rad; F_Leistung1Old = F_Leistung1; F_RPMOld = F_RPM; F_AccOld = F_Acc; F_PMechOld = F_PMech; BremsenOld = Bremsen; BremsenOld2 = BremsenOld; i++; wait(dt);// Set to 5 ms /*} pc.printf("["); for(i=0;i<3999;i++){ pc.printf("%.2f,%.2f,%.2f,%d;...\n\r",MessungP[i],MessungFreq[i],MessungPID[i],writeArr[i]); } pc.printf("%.2f,%.2f,%.2f,%d];\n\r",MessungP[3999],MessungFreq[3999],MessungPID[3999],writeArr[3999]); i=0; */ } }