eBike / Mbed 2 deprecated ENCODER_TEST3_peddep

ti bisogna il phaserunner

Dependencies:   mbed PID mbed-rtos

Peripherien/Regler.cpp@8:1655d27152e6, 2019-05-17 (annotated)

Committer:
EpicG10
Date:
Fri May 17 14:35:54 2019 +0000
Revision:
8:1655d27152e6
Parent:
7:15e6fc689368
Child:
9:56aed8c6779f
Fehlt noch die analogOut

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EpicG10 7:15e6fc689368 1 #include "Regler.h"
EpicG10 7:15e6fc689368 2
EpicG10 7:15e6fc689368 3 Regler::Regler(Phaserunner& vorderrad, Phaserunner& hinterrad, Phaserunner& linkspedal, Phaserunner& rechtspedal, Encoder& encoder):
EpicG10 7:15e6fc689368 4 vorderrad(vorderrad), hinterrad(hinterrad), linkspedal(linkspedal), rechtspedal(rechtspedal), encoder(encoder){
EpicG10 7:15e6fc689368 5
EpicG10 7:15e6fc689368 6 this->torqueRatio = 50;
EpicG10 7:15e6fc689368 7 this->recupRatio = 50;
EpicG10 8:1655d27152e6 8 // Setup Regler
EpicG10 8:1655d27152e6 9 this->pid.setMode(1); // Automode
EpicG10 8:1655d27152e6 10 this->Ts_ = 0.005; // Zeit interval
EpicG10 8:1655d27152e6 11 this->Kp_ = 1.0; // Proportional Gain
EpicG10 8:1655d27152e6 12 this->Ki_ = 30.0; // Integral Gain
EpicG10 8:1655d27152e6 13 this->pid.setInterval(this->Ts_); // Set Ts
EpicG10 8:1655d27152e6 14 this->pid.setTunings(this->Kp_,this->Ki_,0);// Set Parameters (Kd = 0)
EpicG10 8:1655d27152e6 15 this->setPoint_ = 60.0; // Set Default SetPont to 60 RPM
EpicG10 8:1655d27152e6 16
EpicG10 8:1655d27152e6 17 // Default Ellipse Parameters
EpicG10 8:1655d27152e6 18 a_ = 1.0f; // % of Torque Max 0..1
EpicG10 8:1655d27152e6 19 b_ = 0.6f; // % of Torque Max 0..1
EpicG10 8:1655d27152e6 20 beta_ = 0.52f; // 30°
EpicG10 8:1655d27152e6 21
EpicG10 7:15e6fc689368 22 this->ticker.attach(this,&Regler::ReglerCalculate,0.005);
EpicG10 7:15e6fc689368 23 }
EpicG10 7:15e6fc689368 24
EpicG10 7:15e6fc689368 25 void Regler::setSpeed(uint8_t speed){
EpicG10 7:15e6fc689368 26
EpicG10 7:15e6fc689368 27 }
EpicG10 7:15e6fc689368 28
EpicG10 7:15e6fc689368 29 void Regler::setTorqueProportion(uint8_t torqueRatio){
EpicG10 8:1655d27152e6 30 this->torqueRatio = torqueRatio > 100 ? 100 : torqueRatio;
EpicG10 7:15e6fc689368 31 }
EpicG10 7:15e6fc689368 32
EpicG10 7:15e6fc689368 33 void Regler::setRecuperationProportion(uint8_t recupRatio){
EpicG10 7:15e6fc689368 34 this->recupRatio = recupRatio > 100 ? 100 : recupRatio;
EpicG10 7:15e6fc689368 35 }
EpicG10 7:15e6fc689368 36
EpicG10 7:15e6fc689368 37 void Regler::setTorqueMotors(uint8_t torque){
EpicG10 7:15e6fc689368 38 if( this->torqueRatio >= 50 ){
EpicG10 7:15e6fc689368 39 this->hinterrad.setTorque(torque);
EpicG10 7:15e6fc689368 40 this->vorderrad.setTorque(torque * (100-this->torqueRatio) / this->torqueRatio);
EpicG10 7:15e6fc689368 41 }
EpicG10 7:15e6fc689368 42 else{
EpicG10 7:15e6fc689368 43 this->vorderrad.setTorque(torque);
EpicG10 7:15e6fc689368 44 this->hinterrad.setTorque(torque * this->torqueRatio / this->torqueRatio);
EpicG10 7:15e6fc689368 45 }
EpicG10 7:15e6fc689368 46 }
EpicG10 7:15e6fc689368 47
EpicG10 7:15e6fc689368 48 void Regler::setTorquePedals(uint8_t torque){
EpicG10 7:15e6fc689368 49 this->linkspedal.setTorque(torque);
EpicG10 7:15e6fc689368 50 this->rechtspedal.setTorque(torque);
EpicG10 7:15e6fc689368 51 }
EpicG10 7:15e6fc689368 52
EpicG10 7:15e6fc689368 53 void Regler::setRecuperationMotors(uint8_t recuperation){ //TODO: Fix
EpicG10 7:15e6fc689368 54 if( this->recupRatio >= 50 ){
EpicG10 7:15e6fc689368 55 this->hinterrad.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 56 this->vorderrad.setRecuperation(recuperation * (100-this->recupRatio) / this->recupRatio);
EpicG10 7:15e6fc689368 57 }
EpicG10 7:15e6fc689368 58 else{
EpicG10 7:15e6fc689368 59 this->vorderrad.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 60 this->hinterrad.setRecuperation(recuperation * this->recupRatio / this->recupRatio);
EpicG10 7:15e6fc689368 61 }
EpicG10 7:15e6fc689368 62 }
EpicG10 7:15e6fc689368 63
EpicG10 7:15e6fc689368 64 void Regler::setRecuperationPedals(uint8_t recuperation){
EpicG10 7:15e6fc689368 65 this->rechtspedal.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 66 this->linkspedal.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 67 }
EpicG10 7:15e6fc689368 68
EpicG10 8:1655d27152e6 69 void Regler::setEllipseParameters(float a, float b, float beta){
EpicG10 8:1655d27152e6 70 this->a_ = a;
EpicG10 8:1655d27152e6 71 this->b_ = b;
EpicG10 8:1655d27152e6 72 this->beta_ = beta;
EpicG10 8:1655d27152e6 73 }
EpicG10 8:1655d27152e6 74
EpicG10 8:1655d27152e6 75 void Regler::setReglerParameters(float Kp, float Ki, float Ts){
EpicG10 8:1655d27152e6 76 this->Kp_ = Kp;
EpicG10 8:1655d27152e6 77 this->Ki_ = Ki;
EpicG10 8:1655d27152e6 78 this->Ts_ = Ts;
EpicG10 8:1655d27152e6 79 }
EpicG10 8:1655d27152e6 80
EpicG10 8:1655d27152e6 81 void Regler::setReglerSetPoint(float Setpoint){
EpicG10 8:1655d27152e6 82 this->setPoint_ = Setpoint;
EpicG10 8:1655d27152e6 83 }
EpicG10 8:1655d27152e6 84 void Regler::setErgoStufe(float ErgoStufe){
EpicG10 8:1655d27152e6 85 this->ErgoStufe_ = ErgoStufe;
EpicG10 8:1655d27152e6 86 }
EpicG10 8:1655d27152e6 87
EpicG10 8:1655d27152e6 88 void Regler::setPedStufe(float PedStufe){
EpicG10 8:1655d27152e6 89 this->PedStufe_ = PedStufe;
EpicG10 8:1655d27152e6 90 }
EpicG10 8:1655d27152e6 91
EpicG10 8:1655d27152e6 92 void Regler::setPedFactor(float PedFactor){
EpicG10 8:1655d27152e6 93 this->PedFactor_ = PedFactor;
EpicG10 8:1655d27152e6 94 }
EpicG10 8:1655d27152e6 95
EpicG10 7:15e6fc689368 96 void Regler::ReglerCalculate(void){
EpicG10 7:15e6fc689368 97 float Angle, RPM, Acc; // Value form Encoder
EpicG10 7:15e6fc689368 98 float F_RPM, F_Acc; // Filtered Values
EpicG10 7:15e6fc689368 99 static float F_RPMOld = 0.0f, F_AccOld = 0.0f; // Old Value
EpicG10 7:15e6fc689368 100 float T_ab = 0.005; // Abtastzeit: 5ms
EpicG10 7:15e6fc689368 101 float F_ab = 1/T_ab; // Abtastfrequenz
EpicG10 7:15e6fc689368 102 float Omega_cRPM = 2*PI*F_ab/150; // 150 Mal kleiner als die Abtastfrequenz
EpicG10 7:15e6fc689368 103 float sfRPM = (Omega_cRPM*T_ab)/(1+Omega_cRPM*T_ab);//smoothing factor Lowpass RPM
EpicG10 7:15e6fc689368 104 float Omega_cAcc = 2*PI*F_ab/200; // 200 Mal kleiner als die Abtastfrequenz
EpicG10 7:15e6fc689368 105 float sfAcc = (Omega_cAcc*T_ab)/(1+Omega_cAcc*T_ab);//smoothing factor Lowpass Acceleration
EpicG10 7:15e6fc689368 106 float R, a, b, beta, phi; // Ellipse paramter
EpicG10 8:1655d27152e6 107 float ReglerWert; // Ausgangswert des Reglers
EpicG10 8:1655d27152e6 108 // Set local value
EpicG10 8:1655d27152e6 109 a=this->a_;
EpicG10 8:1655d27152e6 110 b=this->b_;
EpicG10 8:1655d27152e6 111 beta=this->beta_;
EpicG10 7:15e6fc689368 112
EpicG10 7:15e6fc689368 113 //Read Value from Encoder
EpicG10 7:15e6fc689368 114 Angle = encoder.readAngle();
EpicG10 7:15e6fc689368 115 RPM = encoder.readRPM();
EpicG10 7:15e6fc689368 116 Acc = encoder.readAcceleration();
EpicG10 8:1655d27152e6 117
EpicG10 7:15e6fc689368 118 // LowPass Filter RPM
EpicG10 7:15e6fc689368 119 F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld;
EpicG10 7:15e6fc689368 120
EpicG10 8:1655d27152e6 121 // LowPass Filter Acceleration
EpicG10 7:15e6fc689368 122 F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld;
EpicG10 7:15e6fc689368 123
EpicG10 7:15e6fc689368 124 // Ellipse
EpicG10 7:15e6fc689368 125 phi = Angle;
EpicG10 7:15e6fc689368 126 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 127
EpicG10 8:1655d27152e6 128 // PI Regler
EpicG10 8:1655d27152e6 129 pid.setTunings(this->Kp_,this->Ki_,0);
EpicG10 8:1655d27152e6 130 pid.setSetPoint(this->setPoint_);
EpicG10 8:1655d27152e6 131 pid.setProcessValue(F_RPM); // Neue Ist-Wert für den Regler
EpicG10 8:1655d27152e6 132 ReglerWert = pid.compute(); // Der Regler berechnet den neuen Wert
EpicG10 8:1655d27152e6 133
EpicG10 7:15e6fc689368 134
EpicG10 7:15e6fc689368 135
EpicG10 7:15e6fc689368 136
EpicG10 7:15e6fc689368 137 // Store Old Values
EpicG10 7:15e6fc689368 138 F_RPMOld = F_RPM;
EpicG10 7:15e6fc689368 139 F_AccOld = F_Acc;
EpicG10 7:15e6fc689368 140 }