Diff: main.cpp

Revision:

6:a80300ee574d

Parent:

5:57fbb5d30d3d

Child:

7:15e6fc689368

--- a/main.cpp	Wed Apr 24 15:37:32 2019 +0000
+++ b/main.cpp	Thu May 09 11:50:43 2019 +0000
@@ -2,16 +2,42 @@
 #include "Encoder.h"
 #include "math.h"
 #include "Phaserunner.h"
+#include "Daumenbetaetigung.h"
 #define PI 3.14159
-Encoder encoder; 
-RawSerial pedalL(PC_10, PC_11, 115200);
-RawSerial pedalR(PC_12, PD_2, 115200); 
-Phaserunner linkspedal(pedalL);
-Phaserunner rechtspedal(pedalR); 
+#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);
+    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);
     
     InterruptIn NullStell(PA_13);  
     Serial pc(USBTX,USBRX);
@@ -22,12 +48,19 @@
     DigitalOut Abhinten(PC_13);
  
     ON_OFF = true;
+    uint16_t rot,rotOld;
+    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];
+    uint32_t i=0;
 
-    uint32_t pulses=0, pulsesOld=0, p,pOld;;
-    double angle_rad, angle_grad, angleOld=0;
-    float a=0.0f,b=0.0f,beta=0.0f; // Ellipse Parameters
-    float R=0.0f, phi=0.0f;
+    led = false;
+    pc.printf("Hello bike\n\r");
 
+    float rec=0.0f;
     PedalL_ON_OFF = true;
     PedalR_ON_OFF = true;
     wait(0.2);
@@ -36,47 +69,82 @@
     wait(0.2);
     PedalL_ON_OFF = true;
     PedalR_ON_OFF = true;
+    float LeerlaufLeistung = 15.0f;
+    pc.printf("Siamo accesi\n\r");
+    wait(1);
     
-
-    led = false;
-    pc.printf("Hello bike");
-    //NullStell.rise(&GetNull);
-    while(1){
-       
-        //rev = Encoder.getRevolutions();
-        pulses = encoder.read();
-        angle_rad = encoder.readAngle();
-        p = TIM3->CNT;
-
-        if(p != pOld){
-           pc.printf("p: %d\n\r",p);
-        }
-        if(pulses != pulsesOld){
-           pc.printf("pulses: %d\n\r",pulses);
-        }
-        if(angle_rad != angleOld){
-            pc.printf("Angle: %.3f\n\r",angle_rad * 180 / PI);
+    // Lowpass Filter
+    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;
+    //Diskrete Ableitung
+    float dt = 0.005f; //5ms
+    float PedaleFreq = 0.0f,PedaleFreqOld = 0.0f, PedaleFreqFil, PedaleFreqFilOld=0.0f; // in rad/s
+    bool init= false;
+    
+    //NullStell.fall(NULL);
+while(1){
+        for(i=0;i<2000;i++){
+            //if(reset) NullStell.fall(NULL);
+            // Measure data form sensors
+            angle_rad = encoder.readAngle();
+            strom = ((Strom.read()*3.3f)-2.5f)/0.025f; 
+            spannung = (Spannung.read()*42.95f);
+            leistung = strom*spannung-leistungsOffset; 
+            
+            // LowPass Filter
+            F_Value = sf * leistung + (1-sf)*F_ValueOld;
+            
+            // Diskrete Ableitung
+            PedaleFreq = (angle_rad - angleOld) / dt;
+            
+            // 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;
+            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;
+            
+            
+            // Store Old Values
+            angleOld = angle_rad;
+            F_ValueOld = F_Value;
+            PedaleFreqOld = PedaleFreq; 
+            PedaleFreqFilOld = PedaleFreqFil;
+            wait(0.004);// Set to 5 ms
         }
-        phi = angle_rad;
-        
-        // Calcolation of R 
-        float P_Factor = 0.5f;
-        a = 1.0f;
-        b = 0.5f;
-        beta = 0.0f;
-        R=sqrt(pow(a,2) * pow(sin(beta + phi),2) + pow(b,2) * pow(cos(beta + phi),2)); // Torque in function of the Elipse parameters
-        
-        //linkspedal.setTorque(R*P_Factor);
-        //rechtspedal.setTorque(R*P_Factor);
-        //pc.printf("Torque: %f\n\r",R*P_Factor);
-        angleOld = angle_rad;
-        pulsesOld = pulses;
-        pOld = p;
-        
-        wait_ms(20);
-        
-    }
-    
-    
-    
+        for(i=0;i<2000;i++){
+            printf("%.3f,%.3f,%.3f;...\n\r",MessungP[i], MessungFreq[i], MessungRPM[i]);
+        }
+}
+
+
+
+   
 }
\ No newline at end of file