eBike / Mbed 2 deprecated ENCODER_TEST3_peddep

ti bisogna il phaserunner

Dependencies:   mbed PID mbed-rtos

Peripherien/Regler.cpp@11:39bd79605827, 2019-06-04 (annotated)

Committer:
beacon
Date:
Tue Jun 04 19:03:39 2019 +0000
Revision:
11:39bd79605827
Parent:
9:56aed8c6779f 
ti bisogna il phaserunner

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EpicG10 7:15e6fc689368 1 #include "Regler.h"
EpicG10 7:15e6fc689368 2
EpicG10 9:56aed8c6779f 3
EpicG10 7:15e6fc689368 4 Regler::Regler(Phaserunner& vorderrad, Phaserunner& hinterrad, Phaserunner& linkspedal, Phaserunner& rechtspedal, Encoder& encoder):
EpicG10 9:56aed8c6779f 5 vorderrad(vorderrad), hinterrad(hinterrad), linkspedal(linkspedal), rechtspedal(rechtspedal), encoder(encoder), gegenmomentLinks(GEG_LINKS_PIN), gegenmomentRechts(GEG_RECHTS_PIN), pid(pid) {
EpicG10 7:15e6fc689368 6
EpicG10 9:56aed8c6779f 7
EpicG10 7:15e6fc689368 8 this->torqueRatio = 50;
EpicG10 7:15e6fc689368 9 this->recupRatio = 50;
EpicG10 9:56aed8c6779f 10
EpicG10 9:56aed8c6779f 11 // Set default Modus
EpicG10 9:56aed8c6779f 12 this->Modus_ = ERGO;
EpicG10 9:56aed8c6779f 13
EpicG10 8:1655d27152e6 14 // Setup Regler
EpicG10 8:1655d27152e6 15 this->pid.setMode(1); // Automode
EpicG10 8:1655d27152e6 16 this->Ts_ = 0.005; // Zeit interval
EpicG10 8:1655d27152e6 17 this->Kp_ = 1.0; // Proportional Gain
EpicG10 8:1655d27152e6 18 this->Ki_ = 30.0; // Integral Gain
EpicG10 8:1655d27152e6 19 this->pid.setInterval(this->Ts_); // Set Ts
EpicG10 8:1655d27152e6 20 this->pid.setTunings(this->Kp_,this->Ki_,0);// Set Parameters (Kd = 0)
EpicG10 9:56aed8c6779f 21 this->setPoint_ = 60.0; // Set Default SetPont to 60 RPM
EpicG10 8:1655d27152e6 22
EpicG10 8:1655d27152e6 23 // Default Ellipse Parameters
EpicG10 8:1655d27152e6 24 a_ = 1.0f; // % of Torque Max 0..1
EpicG10 8:1655d27152e6 25 b_ = 0.6f; // % of Torque Max 0..1
EpicG10 8:1655d27152e6 26 beta_ = 0.52f; // 30°
EpicG10 8:1655d27152e6 27
EpicG10 9:56aed8c6779f 28 //this->ticker.attach(this,&Regler::ReglerCalculate,0.005);
EpicG10 7:15e6fc689368 29 }
EpicG10 7:15e6fc689368 30
EpicG10 7:15e6fc689368 31 void Regler::setSpeed(uint8_t speed){
EpicG10 7:15e6fc689368 32
EpicG10 7:15e6fc689368 33 }
EpicG10 7:15e6fc689368 34
EpicG10 7:15e6fc689368 35 void Regler::setTorqueProportion(uint8_t torqueRatio){
EpicG10 8:1655d27152e6 36 this->torqueRatio = torqueRatio > 100 ? 100 : torqueRatio;
EpicG10 7:15e6fc689368 37 }
EpicG10 7:15e6fc689368 38
EpicG10 7:15e6fc689368 39 void Regler::setRecuperationProportion(uint8_t recupRatio){
EpicG10 7:15e6fc689368 40 this->recupRatio = recupRatio > 100 ? 100 : recupRatio;
EpicG10 7:15e6fc689368 41 }
EpicG10 7:15e6fc689368 42
EpicG10 7:15e6fc689368 43 void Regler::setTorqueMotors(uint8_t torque){
EpicG10 7:15e6fc689368 44 if( this->torqueRatio >= 50 ){
EpicG10 7:15e6fc689368 45 this->hinterrad.setTorque(torque);
EpicG10 7:15e6fc689368 46 this->vorderrad.setTorque(torque * (100-this->torqueRatio) / this->torqueRatio);
EpicG10 7:15e6fc689368 47 }
EpicG10 7:15e6fc689368 48 else{
EpicG10 7:15e6fc689368 49 this->vorderrad.setTorque(torque);
EpicG10 7:15e6fc689368 50 this->hinterrad.setTorque(torque * this->torqueRatio / this->torqueRatio);
EpicG10 7:15e6fc689368 51 }
EpicG10 7:15e6fc689368 52 }
EpicG10 7:15e6fc689368 53
EpicG10 7:15e6fc689368 54 void Regler::setTorquePedals(uint8_t torque){
EpicG10 7:15e6fc689368 55 this->linkspedal.setTorque(torque);
EpicG10 7:15e6fc689368 56 this->rechtspedal.setTorque(torque);
EpicG10 7:15e6fc689368 57 }
EpicG10 7:15e6fc689368 58
EpicG10 7:15e6fc689368 59 void Regler::setRecuperationMotors(uint8_t recuperation){ //TODO: Fix
EpicG10 7:15e6fc689368 60 if( this->recupRatio >= 50 ){
EpicG10 7:15e6fc689368 61 this->hinterrad.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 62 this->vorderrad.setRecuperation(recuperation * (100-this->recupRatio) / this->recupRatio);
EpicG10 7:15e6fc689368 63 }
EpicG10 7:15e6fc689368 64 else{
EpicG10 7:15e6fc689368 65 this->vorderrad.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 66 this->hinterrad.setRecuperation(recuperation * this->recupRatio / this->recupRatio);
EpicG10 7:15e6fc689368 67 }
EpicG10 7:15e6fc689368 68 }
EpicG10 7:15e6fc689368 69
EpicG10 7:15e6fc689368 70 void Regler::setRecuperationPedals(uint8_t recuperation){
EpicG10 7:15e6fc689368 71 this->rechtspedal.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 72 this->linkspedal.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 73 }
EpicG10 7:15e6fc689368 74
EpicG10 8:1655d27152e6 75 void Regler::setEllipseParameters(float a, float b, float beta){
EpicG10 8:1655d27152e6 76 this->a_ = a;
EpicG10 8:1655d27152e6 77 this->b_ = b;
EpicG10 8:1655d27152e6 78 this->beta_ = beta;
EpicG10 8:1655d27152e6 79 }
EpicG10 8:1655d27152e6 80
EpicG10 8:1655d27152e6 81 void Regler::setReglerParameters(float Kp, float Ki, float Ts){
EpicG10 8:1655d27152e6 82 this->Kp_ = Kp;
EpicG10 8:1655d27152e6 83 this->Ki_ = Ki;
EpicG10 8:1655d27152e6 84 this->Ts_ = Ts;
EpicG10 8:1655d27152e6 85 }
EpicG10 8:1655d27152e6 86
EpicG10 8:1655d27152e6 87 void Regler::setReglerSetPoint(float Setpoint){
EpicG10 8:1655d27152e6 88 this->setPoint_ = Setpoint;
EpicG10 8:1655d27152e6 89 }
EpicG10 8:1655d27152e6 90 void Regler::setErgoStufe(float ErgoStufe){
EpicG10 8:1655d27152e6 91 this->ErgoStufe_ = ErgoStufe;
EpicG10 8:1655d27152e6 92 }
EpicG10 8:1655d27152e6 93
EpicG10 8:1655d27152e6 94 void Regler::setPedStufe(float PedStufe){
EpicG10 8:1655d27152e6 95 this->PedStufe_ = PedStufe;
EpicG10 8:1655d27152e6 96 }
EpicG10 8:1655d27152e6 97
EpicG10 8:1655d27152e6 98 void Regler::setPedFactor(float PedFactor){
EpicG10 8:1655d27152e6 99 this->PedFactor_ = PedFactor;
EpicG10 8:1655d27152e6 100 }
EpicG10 8:1655d27152e6 101
EpicG10 9:56aed8c6779f 102 void Regler::setModus(float Modus){
EpicG10 9:56aed8c6779f 103 this->Modus_ = Modus;
EpicG10 9:56aed8c6779f 104 }
EpicG10 9:56aed8c6779f 105
EpicG10 9:56aed8c6779f 106 void Regler::setZero(){
EpicG10 9:56aed8c6779f 107 bool init = false;
EpicG10 9:56aed8c6779f 108 while(!encoder.reset()){
EpicG10 9:56aed8c6779f 109 if(!init){
beacon 11:39bd79605827 110 rechtspedal.setTorque(2); // Setzt für kurze Zeit einen grosseren Moment
EpicG10 9:56aed8c6779f 111 init = true;
EpicG10 9:56aed8c6779f 112 wait(0.005);
beacon 11:39bd79605827 113 rechtspedal.setTorque(3);// Setzt für den Rest der Zeit einen kleinen Moment
EpicG10 9:56aed8c6779f 114 }
EpicG10 9:56aed8c6779f 115 }
beacon 11:39bd79605827 116 rechtspedal.setTorque(0); // Setzt Moment auf Null nach dem Reset
EpicG10 9:56aed8c6779f 117 wait(0.1);
EpicG10 9:56aed8c6779f 118 }
EpicG10 9:56aed8c6779f 119
EpicG10 7:15e6fc689368 120 void Regler::ReglerCalculate(void){
EpicG10 7:15e6fc689368 121 float Angle, RPM, Acc; // Value form Encoder
EpicG10 7:15e6fc689368 122 float F_RPM, F_Acc; // Filtered Values
EpicG10 7:15e6fc689368 123 static float F_RPMOld = 0.0f, F_AccOld = 0.0f; // Old Value
EpicG10 7:15e6fc689368 124 float T_ab = 0.005; // Abtastzeit: 5ms
EpicG10 7:15e6fc689368 125 float F_ab = 1/T_ab; // Abtastfrequenz
EpicG10 7:15e6fc689368 126 float Omega_cRPM = 2*PI*F_ab/150; // 150 Mal kleiner als die Abtastfrequenz
EpicG10 7:15e6fc689368 127 float sfRPM = (Omega_cRPM*T_ab)/(1+Omega_cRPM*T_ab);//smoothing factor Lowpass RPM
EpicG10 7:15e6fc689368 128 float Omega_cAcc = 2*PI*F_ab/200; // 200 Mal kleiner als die Abtastfrequenz
EpicG10 7:15e6fc689368 129 float sfAcc = (Omega_cAcc*T_ab)/(1+Omega_cAcc*T_ab);//smoothing factor Lowpass Acceleration
EpicG10 7:15e6fc689368 130 float R, a, b, beta, phi; // Ellipse paramter
EpicG10 9:56aed8c6779f 131 float ReglerWert; // Ausgangswert des Reglers
EpicG10 9:56aed8c6779f 132 static bool richtung1= false, richtung2 = false;
EpicG10 8:1655d27152e6 133 // Set local value
EpicG10 8:1655d27152e6 134 a=this->a_;
EpicG10 8:1655d27152e6 135 b=this->b_;
EpicG10 8:1655d27152e6 136 beta=this->beta_;
EpicG10 7:15e6fc689368 137
EpicG10 7:15e6fc689368 138 //Read Value from Encoder
EpicG10 7:15e6fc689368 139 Angle = encoder.readAngle();
EpicG10 7:15e6fc689368 140 RPM = encoder.readRPM();
EpicG10 7:15e6fc689368 141 Acc = encoder.readAcceleration();
EpicG10 8:1655d27152e6 142
EpicG10 7:15e6fc689368 143 // LowPass Filter RPM
EpicG10 7:15e6fc689368 144 F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld;
EpicG10 7:15e6fc689368 145
EpicG10 8:1655d27152e6 146 // LowPass Filter Acceleration
EpicG10 7:15e6fc689368 147 F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld;
EpicG10 7:15e6fc689368 148
EpicG10 7:15e6fc689368 149 // Ellipse
EpicG10 7:15e6fc689368 150 phi = Angle;
EpicG10 7:15e6fc689368 151 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
EpicG10 7:15e6fc689368 152
EpicG10 8:1655d27152e6 153 // PI Regler
EpicG10 8:1655d27152e6 154 pid.setTunings(this->Kp_,this->Ki_,0);
EpicG10 8:1655d27152e6 155 pid.setSetPoint(this->setPoint_);
EpicG10 8:1655d27152e6 156 pid.setProcessValue(F_RPM); // Neue Ist-Wert für den Regler
EpicG10 9:56aed8c6779f 157 pid.compute(); // Der Regler berechnet den neuen Wert
EpicG10 9:56aed8c6779f 158 ReglerWert = pid.getCalculation();
EpicG10 8:1655d27152e6 159
EpicG10 7:15e6fc689368 160
EpicG10 9:56aed8c6779f 161 // Write Recuperation value to Phaserunner via AnalogOut
EpicG10 9:56aed8c6779f 162 gegenmomentLinks.write(0.02);
EpicG10 9:56aed8c6779f 163 gegenmomentRechts.write(0.02);
EpicG10 7:15e6fc689368 164
EpicG10 9:56aed8c6779f 165 // Hilfemoment für Rücktritt steuern
EpicG10 9:56aed8c6779f 166 if(F_RPM < -5.0f && !richtung1){
beacon 11:39bd79605827 167 rechtspedal.setTorque(0.03*4096);
EpicG10 9:56aed8c6779f 168 richtung1 = true;
EpicG10 9:56aed8c6779f 169 richtung2 = false;
EpicG10 9:56aed8c6779f 170 }
EpicG10 9:56aed8c6779f 171 else if(F_RPM > -3.0f && !richtung2){
beacon 11:39bd79605827 172 rechtspedal.setTorque(0);
EpicG10 9:56aed8c6779f 173 richtung1 = false;
EpicG10 9:56aed8c6779f 174 richtung2 = true;
EpicG10 9:56aed8c6779f 175 }
EpicG10 7:15e6fc689368 176
EpicG10 7:15e6fc689368 177 // Store Old Values
EpicG10 7:15e6fc689368 178 F_RPMOld = F_RPM;
EpicG10 7:15e6fc689368 179 F_AccOld = F_Acc;
EpicG10 7:15e6fc689368 180 }