ti bisogna il phaserunner
Dependencies: mbed PID mbed-rtos
Peripherien/Regler.cpp@9:56aed8c6779f, 2019-05-29 (annotated)
- Committer:
- EpicG10
- Date:
- Wed May 29 17:05:34 2019 +0000
- Revision:
- 9:56aed8c6779f
- Parent:
- 8:1655d27152e6
- Child:
- 11:39bd79605827
pedelec;
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 | 9:56aed8c6779f | 3 | |
EpicG10 | 7:15e6fc689368 | 4 | Regler::Regler(Phaserunner& vorderrad, Phaserunner& hinterrad, Phaserunner& linkspedal, Phaserunner& rechtspedal, Encoder& encoder): |
EpicG10 | 9:56aed8c6779f | 5 | vorderrad(vorderrad), hinterrad(hinterrad), linkspedal(linkspedal), rechtspedal(rechtspedal), encoder(encoder), gegenmomentLinks(GEG_LINKS_PIN), gegenmomentRechts(GEG_RECHTS_PIN), pid(pid) { |
EpicG10 | 7:15e6fc689368 | 6 | |
EpicG10 | 9:56aed8c6779f | 7 | |
EpicG10 | 7:15e6fc689368 | 8 | this->torqueRatio = 50; |
EpicG10 | 7:15e6fc689368 | 9 | this->recupRatio = 50; |
EpicG10 | 9:56aed8c6779f | 10 | |
EpicG10 | 9:56aed8c6779f | 11 | // Set default Modus |
EpicG10 | 9:56aed8c6779f | 12 | this->Modus_ = ERGO; |
EpicG10 | 9:56aed8c6779f | 13 | |
EpicG10 | 8:1655d27152e6 | 14 | // Setup Regler |
EpicG10 | 8:1655d27152e6 | 15 | this->pid.setMode(1); // Automode |
EpicG10 | 8:1655d27152e6 | 16 | this->Ts_ = 0.005; // Zeit interval |
EpicG10 | 8:1655d27152e6 | 17 | this->Kp_ = 1.0; // Proportional Gain |
EpicG10 | 8:1655d27152e6 | 18 | this->Ki_ = 30.0; // Integral Gain |
EpicG10 | 8:1655d27152e6 | 19 | this->pid.setInterval(this->Ts_); // Set Ts |
EpicG10 | 8:1655d27152e6 | 20 | this->pid.setTunings(this->Kp_,this->Ki_,0);// Set Parameters (Kd = 0) |
EpicG10 | 9:56aed8c6779f | 21 | this->setPoint_ = 60.0; // Set Default SetPont to 60 RPM |
EpicG10 | 8:1655d27152e6 | 22 | |
EpicG10 | 8:1655d27152e6 | 23 | // Default Ellipse Parameters |
EpicG10 | 8:1655d27152e6 | 24 | a_ = 1.0f; // % of Torque Max 0..1 |
EpicG10 | 8:1655d27152e6 | 25 | b_ = 0.6f; // % of Torque Max 0..1 |
EpicG10 | 8:1655d27152e6 | 26 | beta_ = 0.52f; // 30° |
EpicG10 | 8:1655d27152e6 | 27 | |
EpicG10 | 9:56aed8c6779f | 28 | //this->ticker.attach(this,&Regler::ReglerCalculate,0.005); |
EpicG10 | 7:15e6fc689368 | 29 | } |
EpicG10 | 7:15e6fc689368 | 30 | |
EpicG10 | 7:15e6fc689368 | 31 | void Regler::setSpeed(uint8_t speed){ |
EpicG10 | 7:15e6fc689368 | 32 | |
EpicG10 | 7:15e6fc689368 | 33 | } |
EpicG10 | 7:15e6fc689368 | 34 | |
EpicG10 | 7:15e6fc689368 | 35 | void Regler::setTorqueProportion(uint8_t torqueRatio){ |
EpicG10 | 8:1655d27152e6 | 36 | this->torqueRatio = torqueRatio > 100 ? 100 : torqueRatio; |
EpicG10 | 7:15e6fc689368 | 37 | } |
EpicG10 | 7:15e6fc689368 | 38 | |
EpicG10 | 7:15e6fc689368 | 39 | void Regler::setRecuperationProportion(uint8_t recupRatio){ |
EpicG10 | 7:15e6fc689368 | 40 | this->recupRatio = recupRatio > 100 ? 100 : recupRatio; |
EpicG10 | 7:15e6fc689368 | 41 | } |
EpicG10 | 7:15e6fc689368 | 42 | |
EpicG10 | 7:15e6fc689368 | 43 | void Regler::setTorqueMotors(uint8_t torque){ |
EpicG10 | 7:15e6fc689368 | 44 | if( this->torqueRatio >= 50 ){ |
EpicG10 | 7:15e6fc689368 | 45 | this->hinterrad.setTorque(torque); |
EpicG10 | 7:15e6fc689368 | 46 | this->vorderrad.setTorque(torque * (100-this->torqueRatio) / this->torqueRatio); |
EpicG10 | 7:15e6fc689368 | 47 | } |
EpicG10 | 7:15e6fc689368 | 48 | else{ |
EpicG10 | 7:15e6fc689368 | 49 | this->vorderrad.setTorque(torque); |
EpicG10 | 7:15e6fc689368 | 50 | this->hinterrad.setTorque(torque * this->torqueRatio / this->torqueRatio); |
EpicG10 | 7:15e6fc689368 | 51 | } |
EpicG10 | 7:15e6fc689368 | 52 | } |
EpicG10 | 7:15e6fc689368 | 53 | |
EpicG10 | 7:15e6fc689368 | 54 | void Regler::setTorquePedals(uint8_t torque){ |
EpicG10 | 7:15e6fc689368 | 55 | this->linkspedal.setTorque(torque); |
EpicG10 | 7:15e6fc689368 | 56 | this->rechtspedal.setTorque(torque); |
EpicG10 | 7:15e6fc689368 | 57 | } |
EpicG10 | 7:15e6fc689368 | 58 | |
EpicG10 | 7:15e6fc689368 | 59 | void Regler::setRecuperationMotors(uint8_t recuperation){ //TODO: Fix |
EpicG10 | 7:15e6fc689368 | 60 | if( this->recupRatio >= 50 ){ |
EpicG10 | 7:15e6fc689368 | 61 | this->hinterrad.setRecuperation(recuperation); |
EpicG10 | 7:15e6fc689368 | 62 | this->vorderrad.setRecuperation(recuperation * (100-this->recupRatio) / this->recupRatio); |
EpicG10 | 7:15e6fc689368 | 63 | } |
EpicG10 | 7:15e6fc689368 | 64 | else{ |
EpicG10 | 7:15e6fc689368 | 65 | this->vorderrad.setRecuperation(recuperation); |
EpicG10 | 7:15e6fc689368 | 66 | this->hinterrad.setRecuperation(recuperation * this->recupRatio / this->recupRatio); |
EpicG10 | 7:15e6fc689368 | 67 | } |
EpicG10 | 7:15e6fc689368 | 68 | } |
EpicG10 | 7:15e6fc689368 | 69 | |
EpicG10 | 7:15e6fc689368 | 70 | void Regler::setRecuperationPedals(uint8_t recuperation){ |
EpicG10 | 7:15e6fc689368 | 71 | this->rechtspedal.setRecuperation(recuperation); |
EpicG10 | 7:15e6fc689368 | 72 | this->linkspedal.setRecuperation(recuperation); |
EpicG10 | 7:15e6fc689368 | 73 | } |
EpicG10 | 7:15e6fc689368 | 74 | |
EpicG10 | 8:1655d27152e6 | 75 | void Regler::setEllipseParameters(float a, float b, float beta){ |
EpicG10 | 8:1655d27152e6 | 76 | this->a_ = a; |
EpicG10 | 8:1655d27152e6 | 77 | this->b_ = b; |
EpicG10 | 8:1655d27152e6 | 78 | this->beta_ = beta; |
EpicG10 | 8:1655d27152e6 | 79 | } |
EpicG10 | 8:1655d27152e6 | 80 | |
EpicG10 | 8:1655d27152e6 | 81 | void Regler::setReglerParameters(float Kp, float Ki, float Ts){ |
EpicG10 | 8:1655d27152e6 | 82 | this->Kp_ = Kp; |
EpicG10 | 8:1655d27152e6 | 83 | this->Ki_ = Ki; |
EpicG10 | 8:1655d27152e6 | 84 | this->Ts_ = Ts; |
EpicG10 | 8:1655d27152e6 | 85 | } |
EpicG10 | 8:1655d27152e6 | 86 | |
EpicG10 | 8:1655d27152e6 | 87 | void Regler::setReglerSetPoint(float Setpoint){ |
EpicG10 | 8:1655d27152e6 | 88 | this->setPoint_ = Setpoint; |
EpicG10 | 8:1655d27152e6 | 89 | } |
EpicG10 | 8:1655d27152e6 | 90 | void Regler::setErgoStufe(float ErgoStufe){ |
EpicG10 | 8:1655d27152e6 | 91 | this->ErgoStufe_ = ErgoStufe; |
EpicG10 | 8:1655d27152e6 | 92 | } |
EpicG10 | 8:1655d27152e6 | 93 | |
EpicG10 | 8:1655d27152e6 | 94 | void Regler::setPedStufe(float PedStufe){ |
EpicG10 | 8:1655d27152e6 | 95 | this->PedStufe_ = PedStufe; |
EpicG10 | 8:1655d27152e6 | 96 | } |
EpicG10 | 8:1655d27152e6 | 97 | |
EpicG10 | 8:1655d27152e6 | 98 | void Regler::setPedFactor(float PedFactor){ |
EpicG10 | 8:1655d27152e6 | 99 | this->PedFactor_ = PedFactor; |
EpicG10 | 8:1655d27152e6 | 100 | } |
EpicG10 | 8:1655d27152e6 | 101 | |
EpicG10 | 9:56aed8c6779f | 102 | void Regler::setModus(float Modus){ |
EpicG10 | 9:56aed8c6779f | 103 | this->Modus_ = Modus; |
EpicG10 | 9:56aed8c6779f | 104 | } |
EpicG10 | 9:56aed8c6779f | 105 | |
EpicG10 | 9:56aed8c6779f | 106 | void Regler::setZero(){ |
EpicG10 | 9:56aed8c6779f | 107 | bool init = false; |
EpicG10 | 9:56aed8c6779f | 108 | while(!encoder.reset()){ |
EpicG10 | 9:56aed8c6779f | 109 | if(!init){ |
EpicG10 | 9:56aed8c6779f | 110 | rechtspedal.sendBuffer(495,0.2*4096); // Setzt für kurze Zeit einen grosseren Moment |
EpicG10 | 9:56aed8c6779f | 111 | init = true; |
EpicG10 | 9:56aed8c6779f | 112 | wait(0.005); |
EpicG10 | 9:56aed8c6779f | 113 | rechtspedal.sendBuffer(495,0.03*4096);// Setzt für den Rest der Zeit einen kleinen Moment |
EpicG10 | 9:56aed8c6779f | 114 | } |
EpicG10 | 9:56aed8c6779f | 115 | } |
EpicG10 | 9:56aed8c6779f | 116 | rechtspedal.sendBuffer(495,0); // Setzt Moment auf Null nach dem Reset |
EpicG10 | 9:56aed8c6779f | 117 | wait(0.1); |
EpicG10 | 9:56aed8c6779f | 118 | } |
EpicG10 | 9:56aed8c6779f | 119 | |
EpicG10 | 7:15e6fc689368 | 120 | void Regler::ReglerCalculate(void){ |
EpicG10 | 7:15e6fc689368 | 121 | float Angle, RPM, Acc; // Value form Encoder |
EpicG10 | 7:15e6fc689368 | 122 | float F_RPM, F_Acc; // Filtered Values |
EpicG10 | 7:15e6fc689368 | 123 | static float F_RPMOld = 0.0f, F_AccOld = 0.0f; // Old Value |
EpicG10 | 7:15e6fc689368 | 124 | float T_ab = 0.005; // Abtastzeit: 5ms |
EpicG10 | 7:15e6fc689368 | 125 | float F_ab = 1/T_ab; // Abtastfrequenz |
EpicG10 | 7:15e6fc689368 | 126 | float Omega_cRPM = 2*PI*F_ab/150; // 150 Mal kleiner als die Abtastfrequenz |
EpicG10 | 7:15e6fc689368 | 127 | float sfRPM = (Omega_cRPM*T_ab)/(1+Omega_cRPM*T_ab);//smoothing factor Lowpass RPM |
EpicG10 | 7:15e6fc689368 | 128 | float Omega_cAcc = 2*PI*F_ab/200; // 200 Mal kleiner als die Abtastfrequenz |
EpicG10 | 7:15e6fc689368 | 129 | float sfAcc = (Omega_cAcc*T_ab)/(1+Omega_cAcc*T_ab);//smoothing factor Lowpass Acceleration |
EpicG10 | 7:15e6fc689368 | 130 | float R, a, b, beta, phi; // Ellipse paramter |
EpicG10 | 9:56aed8c6779f | 131 | float ReglerWert; // Ausgangswert des Reglers |
EpicG10 | 9:56aed8c6779f | 132 | static bool richtung1= false, richtung2 = false; |
EpicG10 | 8:1655d27152e6 | 133 | // Set local value |
EpicG10 | 8:1655d27152e6 | 134 | a=this->a_; |
EpicG10 | 8:1655d27152e6 | 135 | b=this->b_; |
EpicG10 | 8:1655d27152e6 | 136 | beta=this->beta_; |
EpicG10 | 7:15e6fc689368 | 137 | |
EpicG10 | 7:15e6fc689368 | 138 | //Read Value from Encoder |
EpicG10 | 7:15e6fc689368 | 139 | Angle = encoder.readAngle(); |
EpicG10 | 7:15e6fc689368 | 140 | RPM = encoder.readRPM(); |
EpicG10 | 7:15e6fc689368 | 141 | Acc = encoder.readAcceleration(); |
EpicG10 | 8:1655d27152e6 | 142 | |
EpicG10 | 7:15e6fc689368 | 143 | // LowPass Filter RPM |
EpicG10 | 7:15e6fc689368 | 144 | F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld; |
EpicG10 | 7:15e6fc689368 | 145 | |
EpicG10 | 8:1655d27152e6 | 146 | // LowPass Filter Acceleration |
EpicG10 | 7:15e6fc689368 | 147 | F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld; |
EpicG10 | 7:15e6fc689368 | 148 | |
EpicG10 | 7:15e6fc689368 | 149 | // Ellipse |
EpicG10 | 7:15e6fc689368 | 150 | phi = Angle; |
EpicG10 | 7:15e6fc689368 | 151 | 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 | 152 | |
EpicG10 | 8:1655d27152e6 | 153 | // PI Regler |
EpicG10 | 8:1655d27152e6 | 154 | pid.setTunings(this->Kp_,this->Ki_,0); |
EpicG10 | 8:1655d27152e6 | 155 | pid.setSetPoint(this->setPoint_); |
EpicG10 | 8:1655d27152e6 | 156 | pid.setProcessValue(F_RPM); // Neue Ist-Wert für den Regler |
EpicG10 | 9:56aed8c6779f | 157 | pid.compute(); // Der Regler berechnet den neuen Wert |
EpicG10 | 9:56aed8c6779f | 158 | ReglerWert = pid.getCalculation(); |
EpicG10 | 8:1655d27152e6 | 159 | |
EpicG10 | 7:15e6fc689368 | 160 | |
EpicG10 | 9:56aed8c6779f | 161 | // Write Recuperation value to Phaserunner via AnalogOut |
EpicG10 | 9:56aed8c6779f | 162 | gegenmomentLinks.write(0.02); |
EpicG10 | 9:56aed8c6779f | 163 | gegenmomentRechts.write(0.02); |
EpicG10 | 7:15e6fc689368 | 164 | |
EpicG10 | 9:56aed8c6779f | 165 | // Hilfemoment für Rücktritt steuern |
EpicG10 | 9:56aed8c6779f | 166 | if(F_RPM < -5.0f && !richtung1){ |
EpicG10 | 9:56aed8c6779f | 167 | rechtspedal.sendBuffer(495,0.03*4096); |
EpicG10 | 9:56aed8c6779f | 168 | richtung1 = true; |
EpicG10 | 9:56aed8c6779f | 169 | richtung2 = false; |
EpicG10 | 9:56aed8c6779f | 170 | } |
EpicG10 | 9:56aed8c6779f | 171 | else if(F_RPM > -3.0f && !richtung2){ |
EpicG10 | 9:56aed8c6779f | 172 | rechtspedal.sendBuffer(495,0); |
EpicG10 | 9:56aed8c6779f | 173 | richtung1 = false; |
EpicG10 | 9:56aed8c6779f | 174 | richtung2 = true; |
EpicG10 | 9:56aed8c6779f | 175 | } |
EpicG10 | 7:15e6fc689368 | 176 | |
EpicG10 | 7:15e6fc689368 | 177 | // Store Old Values |
EpicG10 | 7:15e6fc689368 | 178 | F_RPMOld = F_RPM; |
EpicG10 | 7:15e6fc689368 | 179 | F_AccOld = F_Acc; |
EpicG10 | 7:15e6fc689368 | 180 | } |