File content as of revision 8:1655d27152e6:

#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 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);
    
    PinName Hallsensor(PA_13);  
    Encoder encoder(Hallsensor); 
    
    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,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 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.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);
    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; 
    
    // 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
    
    // PID instance
    float Kp = 1.0, Ki = 30.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;
    
    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){
       // 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 leistung
            F_Value = sf * leistung + (1-sf)*F_ValueOld;
            
            // 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(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
            daumenValue = 8;
            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] = F_Value;
           MessungFreq[i] = F_RPM;
           MessungPID[i] = rec;
            */
            // Store Old Values
            angleOld = angle_rad;
            F_ValueOld = F_Value;
            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; 
       */
}



   
}