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?
User | Revision | Line number | New 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 | } |