File content as of revision 11:39bd79605827:

#include "Regler.h"


Regler::Regler(Phaserunner& vorderrad, Phaserunner& hinterrad, Phaserunner& linkspedal, Phaserunner& rechtspedal, Encoder& encoder):
    vorderrad(vorderrad), hinterrad(hinterrad), linkspedal(linkspedal), rechtspedal(rechtspedal), encoder(encoder), gegenmomentLinks(GEG_LINKS_PIN), gegenmomentRechts(GEG_RECHTS_PIN), pid(pid) {

    
    this->torqueRatio = 50;
    this->recupRatio = 50;
    
    // Set default Modus
    this->Modus_ = ERGO; 
    
    // Setup Regler
    this->pid.setMode(1);                       // Automode
    this->Ts_ = 0.005;                          // Zeit interval
    this->Kp_ = 1.0;                            // Proportional Gain
    this->Ki_ = 30.0;                           // Integral Gain
    this->pid.setInterval(this->Ts_);           // Set Ts
    this->pid.setTunings(this->Kp_,this->Ki_,0);// Set Parameters (Kd = 0)
    this->setPoint_ = 60.0;                     // Set Default SetPont to 60 RPM 
    
    // Default Ellipse Parameters
    a_ = 1.0f; // % of Torque Max 0..1
    b_ = 0.6f; // % of Torque Max 0..1
    beta_ = 0.52f; // 30°
    
    //this->ticker.attach(this,&Regler::ReglerCalculate,0.005);
}

void Regler::setSpeed(uint8_t speed){

}

void Regler::setTorqueProportion(uint8_t torqueRatio){
    this->torqueRatio = torqueRatio > 100 ? 100 : torqueRatio;
}

void Regler::setRecuperationProportion(uint8_t recupRatio){
    this->recupRatio = recupRatio > 100 ? 100 : recupRatio;
}

void Regler::setTorqueMotors(uint8_t torque){
    if( this->torqueRatio >= 50 ){
        this->hinterrad.setTorque(torque);
        this->vorderrad.setTorque(torque * (100-this->torqueRatio) / this->torqueRatio);
    }
    else{
        this->vorderrad.setTorque(torque);
        this->hinterrad.setTorque(torque * this->torqueRatio / this->torqueRatio);
    }
}

void Regler::setTorquePedals(uint8_t torque){
    this->linkspedal.setTorque(torque);
    this->rechtspedal.setTorque(torque);
}

void Regler::setRecuperationMotors(uint8_t recuperation){ //TODO: Fix
    if( this->recupRatio >= 50 ){
        this->hinterrad.setRecuperation(recuperation);
        this->vorderrad.setRecuperation(recuperation * (100-this->recupRatio) / this->recupRatio);
    }
    else{
        this->vorderrad.setRecuperation(recuperation);
        this->hinterrad.setRecuperation(recuperation * this->recupRatio / this->recupRatio);
    }
}

void Regler::setRecuperationPedals(uint8_t recuperation){
    this->rechtspedal.setRecuperation(recuperation);
    this->linkspedal.setRecuperation(recuperation);
}

void Regler::setEllipseParameters(float a, float b, float beta){
    this->a_ = a;
    this->b_ = b;
    this->beta_ = beta; 
}

void Regler::setReglerParameters(float Kp, float Ki, float Ts){
    this->Kp_ = Kp;
    this->Ki_ = Ki;
    this->Ts_ = Ts; 
}

void Regler::setReglerSetPoint(float Setpoint){
    this->setPoint_ = Setpoint; 
}
void Regler::setErgoStufe(float ErgoStufe){
    this->ErgoStufe_ = ErgoStufe;
}
    
void Regler::setPedStufe(float PedStufe){
    this->PedStufe_ = PedStufe;   
}

void Regler::setPedFactor(float PedFactor){
    this->PedFactor_ = PedFactor;   
}

void Regler::setModus(float Modus){
    this->Modus_ = Modus;
}

void Regler::setZero(){
    bool init = false;
    while(!encoder.reset()){
        if(!init){
           rechtspedal.setTorque(2); // Setzt für kurze Zeit einen grosseren Moment
           init = true;
           wait(0.005);
           rechtspedal.setTorque(3);// Setzt für den Rest der Zeit einen kleinen Moment
        }
    }
    rechtspedal.setTorque(0);               // Setzt Moment auf Null nach dem Reset
    wait(0.1);
}

void Regler::ReglerCalculate(void){
    float Angle, RPM, Acc;                          // Value form Encoder
    float F_RPM, F_Acc;                             // Filtered Values
    static float F_RPMOld = 0.0f, F_AccOld = 0.0f;  // Old Value
    float T_ab = 0.005;                             // Abtastzeit: 5ms
    float F_ab = 1/T_ab;                            // Abtastfrequenz
    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 Lowpass RPM
    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 Lowpass Acceleration
    float R, a, b, beta, phi;                       // Ellipse paramter
    float ReglerWert;                               // Ausgangswert des Reglers
    static bool  richtung1= false, richtung2 = false;
    // Set local value
    a=this->a_;
    b=this->b_;
    beta=this->beta_;
    
    //Read Value from Encoder
    Angle = encoder.readAngle();
    RPM = encoder.readRPM();
    Acc = encoder.readAcceleration();
    
    // LowPass Filter RPM
    F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld;
            
    // LowPass Filter Acceleration
    F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld;
    
    // Ellipse
    phi = Angle;  
    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
    
    // PI Regler
    pid.setTunings(this->Kp_,this->Ki_,0);
    pid.setSetPoint(this->setPoint_);
    pid.setProcessValue(F_RPM);             // Neue Ist-Wert für den Regler
    pid.compute();                          // Der Regler berechnet den neuen Wert
    ReglerWert = pid.getCalculation();
    
    
    // Write Recuperation value to Phaserunner via AnalogOut
    gegenmomentLinks.write(0.02);
    gegenmomentRechts.write(0.02);
    
    // Hilfemoment für Rücktritt steuern
    if(F_RPM < -5.0f && !richtung1){
            rechtspedal.setTorque(0.03*4096);
            richtung1 = true;
            richtung2 = false;
    }
    else if(F_RPM > -3.0f && !richtung2){
            rechtspedal.setTorque(0);
            richtung1 = false;
            richtung2 = true;
    }
    
    // Store Old Values
    F_RPMOld = F_RPM;
    F_AccOld = F_Acc;
}