Diff: Peripherien/Regler.cpp

Revision:

7:15e6fc689368

Child:

8:1655d27152e6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Peripherien/Regler.cpp	Thu May 16 20:42:39 2019 +0000
@@ -0,0 +1,91 @@
+#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){
+
+    this->torqueRatio = 50;
+    this->recupRatio = 50;
+    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 : recupRatio;
+}
+
+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::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
+    
+     
+    //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 ACC
+    F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld;
+    
+    // Ellipse
+    a = 1.0f; // % of Torque Max 0..1
+    b = 0.6f; // % of Torque Max 0..1
+    beta = 0.52f; // 30°
+    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
+    
+    
+    
+    
+    // Store Old Values
+    F_RPMOld = F_RPM;
+    F_AccOld = F_Acc;
+}
\ No newline at end of file