eBike / Mbed 2 deprecated ENCODER_TEST3_peddep

ti bisogna il phaserunner

Dependencies:   mbed PID mbed-rtos

Peripherien/Regler.cpp@7:15e6fc689368, 2019-05-16 (annotated)

Committer:
EpicG10
Date:
Thu May 16 20:42:39 2019 +0000
Revision:
7:15e6fc689368
Child:
8:1655d27152e6
Implementation Regler (not finished)

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 7:15e6fc689368 8 this->ticker.attach(this,&Regler::ReglerCalculate,0.005);
EpicG10 7:15e6fc689368 9 }
EpicG10 7:15e6fc689368 10
EpicG10 7:15e6fc689368 11 void Regler::setSpeed(uint8_t speed){
EpicG10 7:15e6fc689368 12
EpicG10 7:15e6fc689368 13 }
EpicG10 7:15e6fc689368 14
EpicG10 7:15e6fc689368 15 void Regler::setTorqueProportion(uint8_t torqueRatio){
EpicG10 7:15e6fc689368 16 this->torqueRatio = torqueRatio > 100 ? 100 : recupRatio;
EpicG10 7:15e6fc689368 17 }
EpicG10 7:15e6fc689368 18
EpicG10 7:15e6fc689368 19 void Regler::setRecuperationProportion(uint8_t recupRatio){
EpicG10 7:15e6fc689368 20 this->recupRatio = recupRatio > 100 ? 100 : recupRatio;
EpicG10 7:15e6fc689368 21 }
EpicG10 7:15e6fc689368 22
EpicG10 7:15e6fc689368 23 void Regler::setTorqueMotors(uint8_t torque){
EpicG10 7:15e6fc689368 24 if( this->torqueRatio >= 50 ){
EpicG10 7:15e6fc689368 25 this->hinterrad.setTorque(torque);
EpicG10 7:15e6fc689368 26 this->vorderrad.setTorque(torque * (100-this->torqueRatio) / this->torqueRatio);
EpicG10 7:15e6fc689368 27 }
EpicG10 7:15e6fc689368 28 else{
EpicG10 7:15e6fc689368 29 this->vorderrad.setTorque(torque);
EpicG10 7:15e6fc689368 30 this->hinterrad.setTorque(torque * this->torqueRatio / this->torqueRatio);
EpicG10 7:15e6fc689368 31 }
EpicG10 7:15e6fc689368 32 }
EpicG10 7:15e6fc689368 33
EpicG10 7:15e6fc689368 34 void Regler::setTorquePedals(uint8_t torque){
EpicG10 7:15e6fc689368 35 this->linkspedal.setTorque(torque);
EpicG10 7:15e6fc689368 36 this->rechtspedal.setTorque(torque);
EpicG10 7:15e6fc689368 37 }
EpicG10 7:15e6fc689368 38
EpicG10 7:15e6fc689368 39 void Regler::setRecuperationMotors(uint8_t recuperation){ //TODO: Fix
EpicG10 7:15e6fc689368 40 if( this->recupRatio >= 50 ){
EpicG10 7:15e6fc689368 41 this->hinterrad.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 42 this->vorderrad.setRecuperation(recuperation * (100-this->recupRatio) / this->recupRatio);
EpicG10 7:15e6fc689368 43 }
EpicG10 7:15e6fc689368 44 else{
EpicG10 7:15e6fc689368 45 this->vorderrad.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 46 this->hinterrad.setRecuperation(recuperation * this->recupRatio / this->recupRatio);
EpicG10 7:15e6fc689368 47 }
EpicG10 7:15e6fc689368 48 }
EpicG10 7:15e6fc689368 49
EpicG10 7:15e6fc689368 50 void Regler::setRecuperationPedals(uint8_t recuperation){
EpicG10 7:15e6fc689368 51 this->rechtspedal.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 52 this->linkspedal.setRecuperation(recuperation);
EpicG10 7:15e6fc689368 53 }
EpicG10 7:15e6fc689368 54
EpicG10 7:15e6fc689368 55 void Regler::ReglerCalculate(void){
EpicG10 7:15e6fc689368 56 float Angle, RPM, Acc; // Value form Encoder
EpicG10 7:15e6fc689368 57 float F_RPM, F_Acc; // Filtered Values
EpicG10 7:15e6fc689368 58 static float F_RPMOld = 0.0f, F_AccOld = 0.0f; // Old Value
EpicG10 7:15e6fc689368 59 float T_ab = 0.005; // Abtastzeit: 5ms
EpicG10 7:15e6fc689368 60 float F_ab = 1/T_ab; // Abtastfrequenz
EpicG10 7:15e6fc689368 61 float Omega_cRPM = 2*PI*F_ab/150; // 150 Mal kleiner als die Abtastfrequenz
EpicG10 7:15e6fc689368 62 float sfRPM = (Omega_cRPM*T_ab)/(1+Omega_cRPM*T_ab);//smoothing factor Lowpass RPM
EpicG10 7:15e6fc689368 63 float Omega_cAcc = 2*PI*F_ab/200; // 200 Mal kleiner als die Abtastfrequenz
EpicG10 7:15e6fc689368 64 float sfAcc = (Omega_cAcc*T_ab)/(1+Omega_cAcc*T_ab);//smoothing factor Lowpass Acceleration
EpicG10 7:15e6fc689368 65 float R, a, b, beta, phi; // Ellipse paramter
EpicG10 7:15e6fc689368 66
EpicG10 7:15e6fc689368 67
EpicG10 7:15e6fc689368 68 //Read Value from Encoder
EpicG10 7:15e6fc689368 69 Angle = encoder.readAngle();
EpicG10 7:15e6fc689368 70 RPM = encoder.readRPM();
EpicG10 7:15e6fc689368 71 Acc = encoder.readAcceleration();
EpicG10 7:15e6fc689368 72 // LowPass Filter RPM
EpicG10 7:15e6fc689368 73 F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld;
EpicG10 7:15e6fc689368 74
EpicG10 7:15e6fc689368 75 // LowPass Filter ACC
EpicG10 7:15e6fc689368 76 F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld;
EpicG10 7:15e6fc689368 77
EpicG10 7:15e6fc689368 78 // Ellipse
EpicG10 7:15e6fc689368 79 a = 1.0f; // % of Torque Max 0..1
EpicG10 7:15e6fc689368 80 b = 0.6f; // % of Torque Max 0..1
EpicG10 7:15e6fc689368 81 beta = 0.52f; // 30°
EpicG10 7:15e6fc689368 82 phi = Angle;
EpicG10 7:15e6fc689368 83 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 84
EpicG10 7:15e6fc689368 85
EpicG10 7:15e6fc689368 86
EpicG10 7:15e6fc689368 87
EpicG10 7:15e6fc689368 88 // Store Old Values
EpicG10 7:15e6fc689368 89 F_RPMOld = F_RPM;
EpicG10 7:15e6fc689368 90 F_AccOld = F_Acc;
EpicG10 7:15e6fc689368 91 }