eBike
ti bisogna il phaserunner
Dependencies: mbed PID mbed-rtos
Diff: main.cpp
- Revision:
- 9:56aed8c6779f
- Parent:
- 8:1655d27152e6
- Child:
- 10:b7a44c4a4ef6
--- a/main.cpp Fri May 17 14:35:54 2019 +0000
+++ b/main.cpp Wed May 29 17:05:34 2019 +0000
@@ -9,12 +9,15 @@
-
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);
@@ -35,18 +38,23 @@
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, leistung;
+ 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];
@@ -70,28 +78,34 @@
pc.printf("Siamo accesi\n\r");
wait(1);
- linkspedal.sendBuffer(156,5000);
- rechtspedal.sendBuffer(156,5000);
- wait(0.05);
- linkspedal.sendBuffer(154,int(80*40.96));
- rechtspedal.sendBuffer(154,int(80*40.96));
-
linkspedal.sendBuffer(495,0);
rechtspedal.sendBuffer(495,0);
+ vorderrad.sendBuffer(495,0);
+ hinterrad.sendBuffer(495,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/750; // 750 Mal kleiner als die 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_Value, F_ValueOld = 0.0f;
+ 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
@@ -102,7 +116,7 @@
float PedaleFreq = 0.0f,PedaleFreqOld = 0.0f, PedaleFreqFil, PedaleFreqFilOld=0.0f; // in rad/s
// PID instance
- float Kp = 1.0, Ki = 30.0, Kd = 0.0;
+ float Kp = 1.2, Ki = 40, Kd = 0.0;
PID pid(Kp, Ki, Kd, dt);
pid.setInputLimits(0.0,200.0);
pid.setOutputLimits(1.0,100.0);
@@ -110,20 +124,27 @@
// Antretvariablen
uint16_t verlauf=0;
- float tau=0.9; // tau für exponentiel funktion in s : 0.9s;
+ 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.sendBuffer(495,0.2*4096);
+ rechtspedal.sendBuffer(495,0.8*4096);
init = true;
- wait(0.005);
+ wait(0.04);
rechtspedal.sendBuffer(495,0.03*4096);
}
}
- rechtspedal.sendBuffer(495,0);
+ rechtspedal.sendBuffer(495,0);
+ wait(0.1);
init = false;
+ float leistungsfaktor = 4.8; // Faktor zwischen Elektrischeleistung und mechanische Leistung %
+
while(1){
// while(i<4000){
@@ -134,10 +155,10 @@
Acc = encoder.readAcceleration();
strom = ((Strom.read()*3.3f)-2.5f)/0.025f;
spannung = (Spannung.read()*42.95f);
- leistung = strom*spannung-leistungsOffset;
+ leistung1 = strom*spannung-leistungsOffset;
// LowPass Filter leistung
- F_Value = sf * leistung + (1-sf)*F_ValueOld;
+ F_Leistung1 = sf * leistung1 + (1-sf)*F_Leistung1Old;
// LowPass Filter RPM
F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld;
@@ -148,9 +169,9 @@
// 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(80.0);
+ pid.setSetPoint(70.0);
+ pid.setProcessValue(F_RPM);
- pid.setProcessValue(F_RPM);
@@ -160,50 +181,149 @@
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 = 8;
+ daumenValue = 10;
if(daumenValue < 1) daumenValue = 1;
- rec = ((float)daumenValue)+(R*pid.getCalculation());
+ rec = ((float)daumenValue + pid.getCalculation())*R;
// Antret
- if(F_RPM > 2.0 && !init1) init1 = true;
+ if(!Bremsen && !init1){
+ init1 = true;
+ }
+ else recAntret = 100.0;
if(!init && init1){
- recAntret = 80*exp(-(dt*verlauf)/ tau);
+ recAntret = 100*exp(-(dt*verlauf)/ tau);
verlauf++;
- if(recAntret < 5) init = true; // Use ellipse when rec_start < 5%
+ if(recAntret < rec) init = true; // Use just the ellipse when rec_start < 10%
}
- if(!init) rec = R * recAntret;
+ 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.008f)); // offset for 0.2V -> ~0.02
- GegenMomLinks.write(0.02+(rec*0.008f)); // offset for 0.2V -> ~0.02
+ 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.sendBuffer(495,0.03*4096);
+ richtung1 = true;
+ richtung2 = false;
+ }
+ else if(F_RPM > -3.0f && !richtung2){
+ rechtspedal.sendBuffer(495,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))) {
+ // write torque to phaserunner;
+ //vorderrad.sendBuffer(477,uint16_t((80+(torque))*40.96f));
+ vorderrad.sendBuffer(495,uint16_t((0.10+(torque/100.0f))*4096));
+ //hinterrad.sendBuffer(495,uint16_t((0.20+(torque/100.0f))*4096));
+ //vorderrad.sendBuffer(495,0);
+ hinterrad.sendBuffer(495,0);
+ write = 10;
+ torqueOld = torque;
+ printf("State: torque\n\r");
+
+ }
+ else write = 0;
+ writeNull = false;
- /*
- MessungP[i] = F_Value;
- MessungFreq[i] = F_RPM;
- MessungPID[i] = rec;
- */
+
+ }
+ else if(!writeNull){
+ writeNull = true;
+ write = -10;
+ // write 0 to phaserunner
+ vorderrad.sendBuffer(495,0);
+ hinterrad.sendBuffer(495,0);
+
+ }
+ else write = 0;
+
+ if((daumen.getValue() > Phaserunner::MIN_RECUPERATION)&& !recup){
+ vorderrad.sendBuffer(495,0);
+ hinterrad.sendBuffer(495,0);
+ wait_ms(1);
+ vorderrad.sendBuffer(497,uint16_t((float)daumen.getValue()/100.0f*4096));
+ hinterrad.sendBuffer(497,uint16_t((float)daumen.getValue()/100.0f*4096));
+ 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.sendBuffer(495,0);
+ hinterrad.sendBuffer(495,0);
+ wait_ms(1);
+ vorderrad.sendBuffer(497,uint16_t(0.10f*4096));
+ hinterrad.sendBuffer(497,uint16_t(0.10f*4096));
+ 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_ValueOld = F_Value;
+ F_Leistung1Old = F_Leistung1;
F_RPMOld = F_RPM;
F_AccOld = F_Acc;
+ F_PMechOld = F_PMech;
+ BremsenOld = Bremsen;
+ BremsenOld2 = BremsenOld;
+
i++;
- wait(0.005);// Set to 5 ms
- /* }
- printf("[");
+
+ wait(dt);// Set to 5 ms
+ /*}
+ pc.printf("[");
for(i=0;i<3999;i++){
- printf("%.3f,%.3f,%.3f;...\n\r",MessungP[i],MessungFreq[i],MessungPID[i]);
+ pc.printf("%.2f,%.2f,%.2f,%d;...\n\r",MessungP[i],MessungFreq[i],MessungPID[i],writeArr[i]);
}
- printf("%.3f,%.3f,%.3f];\n\r",MessungP[3999],MessungFreq[3999],MessungPID[3999]);
+ pc.printf("%.2f,%.2f,%.2f,%d];\n\r",MessungP[3999],MessungFreq[3999],MessungPID[3999],writeArr[3999]);
i=0;
- */
+ */
+}
+
}
-
-
-
-}
\ No newline at end of file