ti bisogna il phaserunner
Dependencies: mbed PID mbed-rtos
Diff: Peripherien/Regler.cpp
- Revision:
- 7:15e6fc689368
- Child:
- 8:1655d27152e6
diff -r a80300ee574d -r 15e6fc689368 Peripherien/Regler.cpp --- /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