File content as of revision 10:b7a44c4a4ef6:

#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 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);
    
    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);
    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, 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];
    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(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/1000; // 1000 Mal kleiner als die Abtastfrequenz
    float sf = (Omega_c*T_ab)/(1+Omega_c*T_ab);//smoothing factor
    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
    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.2, Ki = 40, 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.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.8*4096);
           init = true;
           wait(0.04);
           rechtspedal.sendBuffer(495,0.03*4096);
        }
    }
    rechtspedal.sendBuffer(495,0);
    wait(0.1); 
    init = false;
     float leistungsfaktor = 4.8; // Faktor zwischen Elektrischeleistung und mechanische Leistung %

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);
            leistung1 = strom*spannung-leistungsOffset; 
            
            // LowPass Filter leistung
            F_Leistung1 = sf * leistung1 + (1-sf)*F_Leistung1Old;
            
            // 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(70.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 = 10;
            if(daumenValue < 1) daumenValue = 1;
            rec = ((float)daumenValue + pid.getCalculation())*R;
            
            // Antret
            if(!Bremsen && !init1){
                init1 = true;
            }
            else recAntret = 100.0;
            if(!init && init1){
                recAntret = 100*exp(-(dt*verlauf)/ tau);
                verlauf++;
                if(recAntret < rec) init = true; // Use just the ellipse when rec_start < 10%
            }
            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.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))) {
                    vorderrad.sendBuffer(497,0);
                    hinterrad.sendBuffer(497,0); 
                    // 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;

      
            }
            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(5);
               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(5);
               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_Leistung1Old = F_Leistung1;
            F_RPMOld = F_RPM;
            F_AccOld = F_Acc;
            F_PMechOld =  F_PMech;
            BremsenOld = Bremsen;
            BremsenOld2 = BremsenOld;

            i++;
            
            wait(dt);// Set to 5 ms
       /*}
       pc.printf("[");
       for(i=0;i<3999;i++){
            pc.printf("%.2f,%.2f,%.2f,%d;...\n\r",MessungP[i],MessungFreq[i],MessungPID[i],writeArr[i]);
       }
       pc.printf("%.2f,%.2f,%.2f,%d];\n\r",MessungP[3999],MessungFreq[3999],MessungPID[3999],writeArr[3999]);
       i=0; 
      */
}
  
}