Diff: Peripherien/Regler.cpp

Revision:

9:56aed8c6779f

Parent:

8:1655d27152e6

Child:

11:39bd79605827

--- a/Peripherien/Regler.cpp	Fri May 17 14:35:54 2019 +0000
+++ b/Peripherien/Regler.cpp	Wed May 29 17:05:34 2019 +0000
@@ -1,10 +1,16 @@
 #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){
+    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
@@ -12,14 +18,14 @@
     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  
+    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);
+    //this->ticker.attach(this,&Regler::ReglerCalculate,0.005);
 }
 
 void Regler::setSpeed(uint8_t speed){
@@ -93,6 +99,24 @@
     this->PedFactor_ = PedFactor;   
 }
 
+void Regler::setModus(float Modus){
+    this->Modus_ = Modus;
+}
+
+void Regler::setZero(){
+    bool init = false;
+    while(!encoder.reset()){
+        if(!init){
+           rechtspedal.sendBuffer(495,0.2*4096); // Setzt für kurze Zeit einen grosseren Moment
+           init = true;
+           wait(0.005);
+           rechtspedal.sendBuffer(495,0.03*4096);// Setzt für den Rest der Zeit einen kleinen Moment
+        }
+    }
+    rechtspedal.sendBuffer(495,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
@@ -104,7 +128,8 @@
     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
+    float ReglerWert;                               // Ausgangswert des Reglers
+    static bool  richtung1= false, richtung2 = false;
     // Set local value
     a=this->a_;
     b=this->b_;
@@ -129,10 +154,25 @@
     pid.setTunings(this->Kp_,this->Ki_,0);
     pid.setSetPoint(this->setPoint_);
     pid.setProcessValue(F_RPM);             // Neue Ist-Wert für den Regler
-    ReglerWert = pid.compute();             // Der Regler berechnet den neuen Wert
+    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.sendBuffer(495,0.03*4096);
+            richtung1 = true;
+            richtung2 = false;
+    }
+    else if(F_RPM > -3.0f && !richtung2){
+            rechtspedal.sendBuffer(495,0);
+            richtung1 = false;
+            richtung2 = true;
+    }
     
     // Store Old Values
     F_RPMOld = F_RPM;