Dependencies: mbed PID mbed-rtos
Diff: main.cpp
- Revision:
- 7:15e6fc689368
- Parent:
- 6:a80300ee574d
- Child:
- 8:1655d27152e6
--- a/main.cpp Thu May 09 11:50:43 2019 +0000
+++ b/main.cpp Thu May 16 20:42:39 2019 +0000
@@ -3,24 +3,16 @@
#include "math.h"
#include "Phaserunner.h"
#include "Daumenbetaetigung.h"
+#include "Handgriffbetaetigung.h"
+#include "PID.h"
#define PI 3.14159
-#include "Handgriffbetaetigung.h"
-bool reset = false;
-Encoder encoder;
-void Nullstelle()
-{
- float angle = encoder.readAngle();
- printf("angle%.3f\n\r", angle);
- encoder.reset();
- reset = true;
-}
int main(){
- Encoder encoder;
-
+
+
RawSerial pedalL(PC_10, PC_11, 115200);
RawSerial pedalR(PC_12, PD_2, 115200);
Phaserunner linkspedal(pedalL);
@@ -39,7 +31,9 @@
DigitalOut PedalR_ON_OFF(PC_15);
DigitalIn Taste(PC_8);
- InterruptIn NullStell(PA_13);
+ PinName Hallsensor(PA_13);
+ Encoder encoder(Hallsensor);
+
Serial pc(USBTX,USBRX);
DigitalOut led(LED2);
DigitalOut ON_OFF(PH_1);
@@ -48,19 +42,22 @@
DigitalOut Abhinten(PC_13);
ON_OFF = true;
- uint16_t rot,rotOld;
+ 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 leistungsOffset = 15.0f; // Leerlauf Leistung
- float MessungP[2001], MessungFil[2001],MessungFreq[2001], MessungRPM[2001];
+ 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");
- float rec=0.0f;
+
PedalL_ON_OFF = true;
PedalR_ON_OFF = true;
wait(0.2);
@@ -73,75 +70,134 @@
pc.printf("Siamo accesi\n\r");
wait(1);
- // Lowpass Filter
+ linkspedal.sendBuffer(156,4900);
+ rechtspedal.sendBuffer(156,4900);
+
+ linkspedal.sendBuffer(495,0);
+ rechtspedal.sendBuffer(495,0);
+ wait(0.2);
+
+ // 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 sf = (Omega_c*T_ab)/(1+Omega_c*T_ab);//smoothing factor
- float F_Value, F_ValueOld = 0.0f;
+ float F_Value, F_ValueOld = 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 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
- bool init= false;
+
+ // PID instance
+ float Kp = 1.0, Ki = 20.0, Kd = 0.0;
+ PID pid(Kp, Ki, Kd, dt);
+ pid.setInputLimits(0.0,200.0);
+ pid.setOutputLimits(1.0,100.0);
+ pid.setMode(1); //Regulator
+
+ // Antretvariablen
+ uint16_t verlauf=0;
+ float tau=0.9; // tau für exponentiel funktion in s : 0.9s;
+
+ bool init1= false, init = false;
- //NullStell.fall(NULL);
+ while(!encoder.reset()){
+ if(!init){
+ rechtspedal.sendBuffer(495,0.2*4096);
+ init = true;
+ wait(0.005);
+ rechtspedal.sendBuffer(495,0.03*4096);
+ }
+ }
+ rechtspedal.sendBuffer(495,0);
+ init = false;
while(1){
- for(i=0;i<2000;i++){
- //if(reset) NullStell.fall(NULL);
+ 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);
leistung = strom*spannung-leistungsOffset;
- // LowPass Filter
+ // LowPass Filter leistung
F_Value = sf * leistung + (1-sf)*F_ValueOld;
- // Diskrete Ableitung
- PedaleFreq = (angle_rad - angleOld) / dt;
+ // LowPass Filter RPM
+ F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld;
+
+ // LowPass Filter ACC
+ F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld;
- // Set first OldValue to actual value
- if(!init){
- PedaleFreqOld = PedaleFreq;
- PedaleFreqFilOld = PedaleFreq;
- init = true;
- }
- // Filter Nulldurchgang mit der Messung der Winkels und Frequenz Grenz [5,5]rad/s
- if(((PedaleFreq - PedaleFreqOld) > 5.0) || ((PedaleFreq - PedaleFreqOld) < -5.0f) ){
- PedaleFreqFil = PedaleFreqFilOld;
- }
- else{
- PedaleFreqFil = PedaleFreq;
- }
- a = 1.0f;
- b = 0.4f;
- beta = 0.0f;
+ // 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.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
- rec = daumen.getValue()*R;
-
- GegenMomRechts.write(rec*0.008f);
- GegenMomLinks.write(rec*0.008f);
- linkspedal.setTorque(0);
- rechtspedal.setTorque(0);
-
- // Save Values in Array
- MessungP[i]=leistung;
- //MessungFil[i] = F_Value;
- MessungFreq[i] = PedaleFreqFil;
- MessungRPM[i] = PedaleFreqFil / (2.0*PI) * 60.0;
+ daumenValue = 5;
+ if(daumenValue < 1) daumenValue = 1;
+ rec = ((float)daumenValue)+(R*pid.getCalculation());
+ // Antret
+ if(F_RPM > 2.0 && !init1) init1 = true;
+ if(!init && init1){
+ recAntret = 80*exp(-(dt*verlauf)/ tau);
+ verlauf++;
+ if(recAntret < 5) init = true; // Use ellipse when rec_start < 5%
+ }
+ if(!init) rec = R * recAntret;
+ else{
+ // if(F_Acc > 5.0){
+ rec = rec ;
+ //}
+ }
+ 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
+
+ MessungP[i] = pid.getSetPoint();
+ MessungFreq[i] = F_RPM;
+ MessungPID[i] = rec;
+
// Store Old Values
angleOld = angle_rad;
F_ValueOld = F_Value;
- PedaleFreqOld = PedaleFreq;
- PedaleFreqFilOld = PedaleFreqFil;
- wait(0.004);// Set to 5 ms
- }
- for(i=0;i<2000;i++){
- printf("%.3f,%.3f,%.3f;...\n\r",MessungP[i], MessungFreq[i], MessungRPM[i]);
- }
+ F_RPMOld = F_RPM;
+ F_AccOld = F_Acc;
+ i++;
+ wait(0.005);// Set to 5 ms
+ }
+ printf("[");
+ for(i=0;i<3999;i++){
+ printf("%.3f,%.3f,%.3f;...\n\r",MessungP[i],MessungFreq[i],MessungPID[i]);
+ }
+ printf("%.3f,%.3f,%.3f];\n\r",MessungP[3999],MessungFreq[3999],MessungPID[3999]);
+ i=0;
+
}