ti bisogna il phaserunner
Dependencies: mbed PID mbed-rtos
Peripherien/Regler.cpp
- Committer:
- EpicG10
- Date:
- 2019-05-29
- Revision:
- 9:56aed8c6779f
- Parent:
- 8:1655d27152e6
- Child:
- 11:39bd79605827
File content as of revision 9:56aed8c6779f:
#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), 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 this->Kp_ = 1.0; // Proportional Gain 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 // 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); } void Regler::setSpeed(uint8_t speed){ } void Regler::setTorqueProportion(uint8_t torqueRatio){ this->torqueRatio = torqueRatio > 100 ? 100 : torqueRatio; } 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::setEllipseParameters(float a, float b, float beta){ this->a_ = a; this->b_ = b; this->beta_ = beta; } void Regler::setReglerParameters(float Kp, float Ki, float Ts){ this->Kp_ = Kp; this->Ki_ = Ki; this->Ts_ = Ts; } void Regler::setReglerSetPoint(float Setpoint){ this->setPoint_ = Setpoint; } void Regler::setErgoStufe(float ErgoStufe){ this->ErgoStufe_ = ErgoStufe; } void Regler::setPedStufe(float PedStufe){ this->PedStufe_ = PedStufe; } void Regler::setPedFactor(float PedFactor){ 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 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 float ReglerWert; // Ausgangswert des Reglers static bool richtung1= false, richtung2 = false; // Set local value a=this->a_; b=this->b_; beta=this->beta_; //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 Acceleration F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld; // Ellipse 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 // PI Regler pid.setTunings(this->Kp_,this->Ki_,0); pid.setSetPoint(this->setPoint_); pid.setProcessValue(F_RPM); // Neue Ist-Wert für den Regler 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; F_AccOld = F_Acc; }