File content as of revision 6:a80300ee574d:

#include "mbed.h"
#include "Encoder.h"
#include "math.h"
#include "Phaserunner.h"
#include "Daumenbetaetigung.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);
    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);
    DigitalOut led(LED2);
    DigitalOut ON_OFF(PH_1);
    
    DigitalOut Abvorne(PB_7);
    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;

    led = false;
    pc.printf("Hello bike\n\r");

    float rec=0.0f;
    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);
    
    // 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
        }
        for(i=0;i<2000;i++){
            printf("%.3f,%.3f,%.3f;...\n\r",MessungP[i], MessungFreq[i], MessungRPM[i]);
        }
}



   
}