Regler.cpp

00001 #include "Regler.h"
00002 
00003 
00004 Regler::Regler(Phaserunner& vorderrad, Phaserunner& hinterrad, Phaserunner& linkspedal, Phaserunner& rechtspedal, Encoder& encoder):
00005     vorderrad(vorderrad), hinterrad(hinterrad), linkspedal(linkspedal), rechtspedal(rechtspedal), encoder(encoder), gegenmomentLinks(GEG_LINKS_PIN), gegenmomentRechts(GEG_RECHTS_PIN), pid(pid) {
00006 
00007     
00008     this->torqueRatio = 50;
00009     this->recupRatio = 50;
00010     
00011     // Set default Modus
00012     this->Modus_ = ERGO; 
00013     
00014     // Setup Regler
00015     this->pid.setMode(1);                       // Automode
00016     this->Ts_ = 0.005;                          // Zeit interval
00017     this->Kp_ = 1.0;                            // Proportional Gain
00018     this->Ki_ = 30.0;                           // Integral Gain
00019     this->pid.setInterval(this->Ts_);           // Set Ts
00020     this->pid.setTunings(this->Kp_,this->Ki_,0);// Set Parameters (Kd = 0)
00021     this->setPoint_ = 60.0;                     // Set Default SetPont to 60 RPM 
00022     
00023     // Default Ellipse Parameters
00024     a_ = 1.0f; // % of Torque Max 0..1
00025     b_ = 0.6f; // % of Torque Max 0..1
00026     beta_ = 0.52f; // 30°
00027     
00028     //this->ticker.attach(this,&Regler::ReglerCalculate,0.005);
00029 }
00030 
00031 void Regler::setSpeed(uint8_t speed){
00032 
00033 }
00034 
00035 void Regler::setTorqueProportion(uint8_t torqueRatio){
00036     this->torqueRatio = torqueRatio > 100 ? 100 : torqueRatio;
00037 }
00038 
00039 void Regler::setRecuperationProportion(uint8_t recupRatio){
00040     this->recupRatio = recupRatio > 100 ? 100 : recupRatio;
00041 }
00042 
00043 void Regler::setTorqueMotors(uint8_t torque){
00044     if( this->torqueRatio >= 50 ){
00045         this->hinterrad.setTorque(torque);
00046         this->vorderrad.setTorque(torque * (100-this->torqueRatio) / this->torqueRatio);
00047     }
00048     else{
00049         this->vorderrad.setTorque(torque);
00050         this->hinterrad.setTorque(torque * this->torqueRatio / this->torqueRatio);
00051     }
00052 }
00053 
00054 void Regler::setTorquePedals(uint8_t torque){
00055     this->linkspedal.setTorque(torque);
00056     this->rechtspedal.setTorque(torque);
00057 }
00058 
00059 void Regler::setRecuperationMotors(uint8_t recuperation){ //TODO: Fix
00060     if( this->recupRatio >= 50 ){
00061         this->hinterrad.setRecuperation(recuperation);
00062         this->vorderrad.setRecuperation(recuperation * (100-this->recupRatio) / this->recupRatio);
00063     }
00064     else{
00065         this->vorderrad.setRecuperation(recuperation);
00066         this->hinterrad.setRecuperation(recuperation * this->recupRatio / this->recupRatio);
00067     }
00068 }
00069 
00070 void Regler::setRecuperationPedals(uint8_t recuperation){
00071     this->rechtspedal.setRecuperation(recuperation);
00072     this->linkspedal.setRecuperation(recuperation);
00073 }
00074 
00075 void Regler::setEllipseParameters(float a, float b, float beta){
00076     this->a_ = a;
00077     this->b_ = b;
00078     this->beta_ = beta; 
00079 }
00080 
00081 void Regler::setReglerParameters(float Kp, float Ki, float Ts){
00082     this->Kp_ = Kp;
00083     this->Ki_ = Ki;
00084     this->Ts_ = Ts; 
00085 }
00086 
00087 void Regler::setReglerSetPoint(float Setpoint){
00088     this->setPoint_ = Setpoint; 
00089 }
00090 void Regler::setErgoStufe(float ErgoStufe){
00091     this->ErgoStufe_ = ErgoStufe;
00092 }
00093     
00094 void Regler::setPedStufe(float PedStufe){
00095     this->PedStufe_ = PedStufe;   
00096 }
00097 
00098 void Regler::setPedFactor(float PedFactor){
00099     this->PedFactor_ = PedFactor;   
00100 }
00101 
00102 void Regler::setModus(float Modus){
00103     this->Modus_ = Modus;
00104 }
00105 
00106 void Regler::setZero(){
00107     bool init = false;
00108     while(!encoder.reset()){
00109         if(!init){
00110            rechtspedal.setTorque(2); // Setzt für kurze Zeit einen grosseren Moment
00111            init = true;
00112            wait(0.005);
00113            rechtspedal.setTorque(3);// Setzt für den Rest der Zeit einen kleinen Moment
00114         }
00115     }
00116     rechtspedal.setTorque(0);               // Setzt Moment auf Null nach dem Reset
00117     wait(0.1);
00118 }
00119 
00120 void Regler::ReglerCalculate(void){
00121     float Angle, RPM, Acc;                          // Value form Encoder
00122     float F_RPM, F_Acc;                             // Filtered Values
00123     static float F_RPMOld = 0.0f, F_AccOld = 0.0f;  // Old Value
00124     float T_ab = 0.005;                             // Abtastzeit: 5ms
00125     float F_ab = 1/T_ab;                            // Abtastfrequenz
00126     float Omega_cRPM = 2*PI*F_ab/150;               // 150 Mal kleiner als die Abtastfrequenz
00127     float sfRPM = (Omega_cRPM*T_ab)/(1+Omega_cRPM*T_ab);//smoothing factor Lowpass RPM
00128     float Omega_cAcc = 2*PI*F_ab/200;               // 200 Mal kleiner als die Abtastfrequenz
00129     float sfAcc = (Omega_cAcc*T_ab)/(1+Omega_cAcc*T_ab);//smoothing factor Lowpass Acceleration
00130     float R, a, b, beta, phi;                       // Ellipse paramter
00131     float ReglerWert;                               // Ausgangswert des Reglers
00132     static bool  richtung1= false, richtung2 = false;
00133     // Set local value
00134     a=this->a_;
00135     b=this->b_;
00136     beta=this->beta_;
00137     
00138     //Read Value from Encoder
00139     Angle = encoder.readAngle();
00140     RPM = encoder.readRPM();
00141     Acc = encoder.readAcceleration();
00142     
00143     // LowPass Filter RPM
00144     F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld;
00145             
00146     // LowPass Filter Acceleration
00147     F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld;
00148     
00149     // Ellipse
00150     phi = Angle;  
00151     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
00152     
00153     // PI Regler
00154     pid.setTunings(this->Kp_,this->Ki_,0);
00155     pid.setSetPoint(this->setPoint_);
00156     pid.setProcessValue(F_RPM);             // Neue Ist-Wert für den Regler
00157     pid.compute();                          // Der Regler berechnet den neuen Wert
00158     ReglerWert = pid.getCalculation();
00159     
00160     
00161     // Write Recuperation value to Phaserunner via AnalogOut
00162     gegenmomentLinks.write(0.02);
00163     gegenmomentRechts.write(0.02);
00164     
00165     // Hilfemoment für Rücktritt steuern
00166     if(F_RPM < -5.0f && !richtung1){
00167             rechtspedal.setTorque(0.03*4096);
00168             richtung1 = true;
00169             richtung2 = false;
00170     }
00171     else if(F_RPM > -3.0f && !richtung2){
00172             rechtspedal.setTorque(0);
00173             richtung1 = false;
00174             richtung2 = true;
00175     }
00176     
00177     // Store Old Values
00178     F_RPMOld = F_RPM;
00179     F_AccOld = F_Acc;
00180 }