Revision 9:56aed8c6779f, committed 2019-05-29

Comitter:

EpicG10

Date:

Wed May 29 17:05:34 2019 +0000

Parent:

8:1655d27152e6

Commit message:

pedelec;

Changed in this revision

--- a/Peripherien/Daumenbetaetigung.cpp	Fri May 17 14:35:54 2019 +0000
+++ b/Peripherien/Daumenbetaetigung.cpp	Wed May 29 17:05:34 2019 +0000
@@ -1,6 +1,6 @@
 #include "Daumenbetaetigung.h"
 
-Daumenbetaetigung::Daumenbetaetigung(): poti(PC_3){
+Daumenbetaetigung::Daumenbetaetigung(): poti(POTIPIN){ //POTIPIN = PC_3
     this->Error     = false;
 }
 

--- a/Peripherien/Daumenbetaetigung.h	Fri May 17 14:35:54 2019 +0000
+++ b/Peripherien/Daumenbetaetigung.h	Wed May 29 17:05:34 2019 +0000
@@ -16,10 +16,10 @@
     AnalogIn poti;
 
     //Hoechster Wert, der der Poti zurückgibt
-    static const uint16_t OBERGRENZE = 77;
+    static const uint16_t OBERGRENZE = 75;
 
     //Tiefster Wert, der der Poti zurückgibt
-    static const uint16_t UNTERGRENZE = 27;
+    static const uint16_t UNTERGRENZE = 31;
 
     //Pin auf dem die Daumenbetätigung ist
     static const PinName POTIPIN = PC_3;

--- a/Peripherien/Encoder.cpp	Fri May 17 14:35:54 2019 +0000
+++ b/Peripherien/Encoder.cpp	Wed May 29 17:05:34 2019 +0000
@@ -49,7 +49,7 @@
         HallSensor.fall(callback(this, &Encoder::ResetInterrupt));
         this->resetOn = 0;
         
-        // Ticker for the calculation of the frequency wiht dt = 5ms
+        // Ticker for the calculation of the frequency with dt = 5ms
         this->ticker.attach(callback(this, &Encoder::calculateFrequency),dt);
            
 }

--- a/Peripherien/Phaserunner.cpp	Fri May 17 14:35:54 2019 +0000
+++ b/Peripherien/Phaserunner.cpp	Wed May 29 17:05:34 2019 +0000
@@ -2,7 +2,7 @@
 
 Phaserunner::Phaserunner(RawSerial& connection): connection(connection), led(LED2){
     //this->connection = connection;
-    //this->connection.attach(callback(this, &Phaserunner::Rx_interrupt), Serial::RxIrq);
+    this->connection.attach(callback(this, &Phaserunner::Rx_interrupt), Serial::RxIrq);
 
     //this->thread.start(callback(this, &Phaserunner::writeToPhaserunner));
     //this->ticker.attach(callback(this, &Phaserunner::writeToPhaserunner), 0.5f);

--- a/Peripherien/Phaserunner.h	Fri May 17 14:35:54 2019 +0000
+++ b/Peripherien/Phaserunner.h	Wed May 29 17:05:34 2019 +0000
@@ -89,7 +89,7 @@
      * @param adress:   Adresse des Registers aus dem gelesen werden soll.
      * @return:         Anzahl gelesener Bytes
      */
-    int readBuffer(uint16_t adress);
+    //int readBuffer(uint16_t adress);
 
     /**
      * @brief:  Schreibt ein Drehmoment auf den Phaserunner.
@@ -131,6 +131,7 @@
     static const uint8_t MIN_RECUPERATION   = 10;   //Schwellwert für die Aktivierung des Daumengriffbetätigers
     static const uint8_t MIN_HANDGRIFF      = 5;    //Schwellwert für die Aktivierung des Handgriffbetätigers
     int sendBuffer(unsigned short adress, unsigned short value);
+    int readBuffer(uint16_t adress);
     /**
      * @brief Initialisiert ein Phaserunner-Objekt
      * @param connection: Serielle Schnittstelle zum Phaserunner

--- a/Peripherien/Regler.cpp	Fri May 17 14:35:54 2019 +0000
+++ b/Peripherien/Regler.cpp	Wed May 29 17:05:34 2019 +0000
@@ -1,10 +1,16 @@
 #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){
+    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
@@ -12,14 +18,14 @@
     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  
+    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);
+    //this->ticker.attach(this,&Regler::ReglerCalculate,0.005);
 }
 
 void Regler::setSpeed(uint8_t speed){
@@ -93,6 +99,24 @@
     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
@@ -104,7 +128,8 @@
     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
+    float ReglerWert;                               // Ausgangswert des Reglers
+    static bool  richtung1= false, richtung2 = false;
     // Set local value
     a=this->a_;
     b=this->b_;
@@ -129,10 +154,25 @@
     pid.setTunings(this->Kp_,this->Ki_,0);
     pid.setSetPoint(this->setPoint_);
     pid.setProcessValue(F_RPM);             // Neue Ist-Wert für den Regler
-    ReglerWert = pid.compute();             // Der Regler berechnet den neuen Wert
+    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;

--- a/Peripherien/Regler.h	Fri May 17 14:35:54 2019 +0000
+++ b/Peripherien/Regler.h	Wed May 29 17:05:34 2019 +0000
@@ -5,10 +5,16 @@
 #ifndef PA_REGLER_H
 #define PA_REGLER_H
 
+#include "mbed.h"
 #include "Phaserunner.h"
 #include "Encoder.h"
 #include "PID.h"
 
+
+#define ERGO 0
+#define MOFA 1
+#define PEDELEC 2
+
 class Regler{
 private:
     //Alle Phaserunner
@@ -17,8 +23,12 @@
     Phaserunner& linkspedal;
     Phaserunner& rechtspedal;
     
+    // Analog Outputs für Generatoren am Pedalen
+    AnalogOut gegenmomentLinks;
+    AnalogOut gegenmomentRechts;
+    
     // PID Regler
-    PID          pid;
+    PID&          pid;
     
     //Encoder
     Encoder&     encoder;
@@ -41,11 +51,18 @@
     // Regler Parameters Pedale
     float Kp_,Ki_,Ts_,setPoint_;
     
-    // Ergometer Stufe (von 1 bis 10)
+    // Betriebsmodus (0->Ergo, 1->Mofa, 2->Pedelec) 
+    uint8_t Modus_;
+    
+    // Ergometer Stufe "(von 1 bis 10)"
     float ErgoStufe_;
     
     // Pedelec Values
-    float PedStufe_, PedFactor_; 
+    float PedStufe_, PedFactor_;
+    
+    // AnalogOut Pins für gegenmomentsteuerung
+    static const PinName GEG_LINKS_PIN = PA_4; 
+    static const PinName GEG_RECHTS_PIN = PA_5; 
     
     const static float PI = 3.14159265;
 
@@ -92,6 +109,12 @@
     void setTorquePedals(uint8_t torque);
 
     /**
+     * Setzt das Nullpunkt der Pedalen.
+     * 
+     */
+    void setZero();
+    
+    /**
      * Setzt die Rekuperation der Motoren
      * @param recuperation
      */
@@ -142,6 +165,12 @@
      * @param PedFactor
      */
     void setPedFactor(float PedFactor);
+    
+    /**
+     * Setzt die Modus
+     * @param Modus
+     */
+    void setModus(float Modus);
 };
 
 #endif //PA_REGLER_H

--- a/main.cpp	Fri May 17 14:35:54 2019 +0000
+++ b/main.cpp	Wed May 29 17:05:34 2019 +0000
@@ -9,12 +9,15 @@
 
 
 
-
 int main(){
 
-     
+    RawSerial motorV(PA_0, PA_1, 115200);
+    RawSerial motorH(PC_6, PC_7,  115200);
     RawSerial pedalL(PC_10, PC_11, 115200);
     RawSerial pedalR(PC_12, PD_2, 115200);
+    
+    Phaserunner vorderrad(motorV);
+    Phaserunner hinterrad(motorH);
     Phaserunner linkspedal(pedalL);
     Phaserunner rechtspedal(pedalR);
     
@@ -35,18 +38,23 @@
     Encoder encoder(Hallsensor); 
     
     Serial pc(USBTX,USBRX);
+    pc.baud(19200);
     DigitalOut led(LED2);
     DigitalOut ON_OFF(PH_1);
     
     DigitalOut Abvorne(PB_7);
     DigitalOut Abhinten(PC_13);
- 
+    
+    DigitalIn Bremsen(PA_14);
+    Bremsen.mode(PullDown);
+    AnalogIn daumen1(A5); //qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqq
+    
     ON_OFF = true;
     uint16_t rot,rotOld,daumenValue;
     double angle_rad, angleOld=0;
     float a=0.0f,b=0.0f,beta=0.0f; // Ellipse Parameters
     float R=0.0f, phi=0.0f, Rold=0.0f;
-    float strom, spannung, leistung;
+    float strom, spannung, leistung1, leistung2; //1:Elektrische Lesitung Batterie, 2:Mechanische (M*omega), 3: Leistung aus den Phaserunner
     float leistungsOffset = 15.0f; // Leerlauf Leistung 
     float RPM, Acc; // Value form Encoder
     float MessungP[4001], MessungFreq[4001],MessungAngle[4001], MessungPID[4001];
@@ -70,28 +78,34 @@
     pc.printf("Siamo accesi\n\r");
     wait(1);
     
-    linkspedal.sendBuffer(156,5000);
-    rechtspedal.sendBuffer(156,5000);
-    wait(0.05);
-    linkspedal.sendBuffer(154,int(80*40.96));
-    rechtspedal.sendBuffer(154,int(80*40.96));
-    
     linkspedal.sendBuffer(495,0);
     rechtspedal.sendBuffer(495,0);
+    vorderrad.sendBuffer(495,0);
+    hinterrad.sendBuffer(495,0);
     wait(0.2);
     
+    // Pedelec Values
+    float torque, torqueOld=0.0f; // Torque an den Räder
+    float pedFaktor; // Faktor zwischen PedaleLeistung und Moment an der Rädern
+
+    
     // Lowpass Filter Leistung
     float T_ab = 0.005; // Abtastzeit: 5ms
     float F_ab = 1/T_ab; // Abtastfrequenz
-    float Omega_c = 2*PI*F_ab/750; // 750 Mal kleiner als die Abtastfrequenz
+    float Omega_c = 2*PI*F_ab/1000; // 1000 Mal kleiner als die Abtastfrequenz
     float sf = (Omega_c*T_ab)/(1+Omega_c*T_ab);//smoothing factor
-    float F_Value, F_ValueOld = 0.0f; 
+    float F_Leistung1, F_Leistung1Old = 0.0f; 
     
     // Lowpass Filter RPM
     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
     float F_RPM, F_RPMOld = 0.0f;
     
+    // Lowpass Filter Mech.Leistung
+    float Omega_cPMech = 2*PI*F_ab/500; // 500 Mal kleiner als die Abtastfrequenz
+    float sfPMech = (Omega_cPMech*T_ab)/(1+Omega_cPMech*T_ab);//smoothing factor
+    float F_PMech, F_PMechOld = 0.0f;
+    
     // Lowpass Filter Acceleration
     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
@@ -102,7 +116,7 @@
     float PedaleFreq = 0.0f,PedaleFreqOld = 0.0f, PedaleFreqFil, PedaleFreqFilOld=0.0f; // in rad/s
     
     // PID instance
-    float Kp = 1.0, Ki = 30.0, Kd = 0.0;
+    float Kp = 1.2, Ki = 40, Kd = 0.0;
     PID pid(Kp, Ki, Kd, dt);
     pid.setInputLimits(0.0,200.0);
     pid.setOutputLimits(1.0,100.0);
@@ -110,20 +124,27 @@
     
     // Antretvariablen
     uint16_t verlauf=0;
-    float tau=0.9; // tau für exponentiel funktion in s : 0.9s;
+    float tau=0.8f; // tau für exponentiel funktion in s : 0.8s;
     
     bool init1= false, init = false;
+    bool richtung1= false, richtung2 = false;
+    bool writeNull = false; // write 0 to phaseerunner one time
+    int8_t write=0, writeArr[4001];
+    bool BremsenOld = false,BremsenOld2 = false;
     
     while(!encoder.reset()){
         if(!init){
-           rechtspedal.sendBuffer(495,0.2*4096);
+           rechtspedal.sendBuffer(495,0.8*4096);
            init = true;
-           wait(0.005);
+           wait(0.04);
            rechtspedal.sendBuffer(495,0.03*4096);
         }
     }
-    rechtspedal.sendBuffer(495,0); 
+    rechtspedal.sendBuffer(495,0);
+    wait(0.1); 
     init = false;
+     float leistungsfaktor = 4.8; // Faktor zwischen Elektrischeleistung und mechanische Leistung %
+
 while(1){
        // while(i<4000){
 
@@ -134,10 +155,10 @@
             Acc = encoder.readAcceleration();
             strom = ((Strom.read()*3.3f)-2.5f)/0.025f; 
             spannung = (Spannung.read()*42.95f);
-            leistung = strom*spannung-leistungsOffset; 
+            leistung1 = strom*spannung-leistungsOffset; 
             
             // LowPass Filter leistung
-            F_Value = sf * leistung + (1-sf)*F_ValueOld;
+            F_Leistung1 = sf * leistung1 + (1-sf)*F_Leistung1Old;
             
             // LowPass Filter RPM
             F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld;
@@ -148,9 +169,9 @@
             // Regulator
             //if(F_RPM > 1.1 * pid.getSetPoint()) pid.setSetPoint(F_RPM);
             //else if (F_RPM < 0.9f * pid.getSetPoint()) pid.setSetPoint(F_RPM);
-            pid.setSetPoint(80.0);
+            pid.setSetPoint(70.0);
+            pid.setProcessValue(F_RPM);
             
-            pid.setProcessValue(F_RPM);
             
             
 
@@ -160,50 +181,149 @@
             beta = 0.52f; // 30°
             phi = angle_rad;  
             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
-            daumenValue = 8;
+            daumenValue = 10;
             if(daumenValue < 1) daumenValue = 1;
-            rec = ((float)daumenValue)+(R*pid.getCalculation());
+            rec = ((float)daumenValue + pid.getCalculation())*R;
             
             // Antret
-            if(F_RPM > 2.0 && !init1) init1 = true;
+            if(!Bremsen && !init1){
+                init1 = true;
+            }
+            else recAntret = 100.0;
             if(!init && init1){
-                recAntret = 80*exp(-(dt*verlauf)/ tau);
+                recAntret = 100*exp(-(dt*verlauf)/ tau);
                 verlauf++;
-                if(recAntret < 5) init = true; // Use ellipse when rec_start < 5%
+                if(recAntret < rec) init = true; // Use just the ellipse when rec_start < 10%
             }
-            if(!init) rec = R * recAntret;
+            if(!init) rec = recAntret;
             else{
                 // if(F_Acc > 5.0){
                     rec = rec ;
                  //}
             }
+            
+            // Mechanische Leistung in % berechnet
+            leistung2 = 4.0*PI*F_RPM/60.0*rec; // 2 * omega * Gegenmoment in %
+            leistung2 = leistung2/leistungsfaktor;
+            
+            // LowPass Filter Mech Leistung
+            F_PMech = sfPMech * leistung2 + (1-sfPMech)*F_PMechOld;
+
+            
             if(rec < 0.0) rec = 0.0;      
-            GegenMomRechts.write(0.02+(rec*0.008f)); // offset for 0.2V -> ~0.02
-            GegenMomLinks.write(0.02+(rec*0.008f));  // offset for 0.2V -> ~0.02
+            GegenMomRechts.write(0.02+(rec*0.01f)); // offset for 0.2V -> ~0.02
+            GegenMomLinks.write(0.02+(rec*0.01f));  // offset for 0.2V -> ~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;
+            }
+            
+            pedFaktor = 2.0f;
+            
+            static bool gebremst = false;
+            static bool recup = false;
+            
+            if(F_RPM > 1.0f && !Bremsen && !recup){
+                torque = (F_PMech / 300.0 * 100)*pedFaktor;
+                if(i%10 == 0 && (torque < (torqueOld*0.95) || torque > (torqueOld * 1.05))) {
+                   // write torque to phaserunner;
+               //vorderrad.sendBuffer(477,uint16_t((80+(torque))*40.96f));
+               vorderrad.sendBuffer(495,uint16_t((0.10+(torque/100.0f))*4096));
+                //hinterrad.sendBuffer(495,uint16_t((0.20+(torque/100.0f))*4096));
+               //vorderrad.sendBuffer(495,0);
+               hinterrad.sendBuffer(495,0);
+                write = 10;
+                torqueOld = torque;
+                printf("State: torque\n\r");
+                  
+                }
+                else write = 0;
+                writeNull = false;
 
-           /* 
-           MessungP[i] = F_Value;
-           MessungFreq[i] = F_RPM;
-           MessungPID[i] = rec;
-            */
+      
+            }
+            else if(!writeNull){
+               writeNull = true;
+               write = -10;
+               // write 0 to phaserunner
+               vorderrad.sendBuffer(495,0);
+               hinterrad.sendBuffer(495,0);
+               
+            }
+            else write = 0; 
+            
+            if((daumen.getValue() > Phaserunner::MIN_RECUPERATION)&& !recup){
+               vorderrad.sendBuffer(495,0);
+               hinterrad.sendBuffer(495,0);
+               wait_ms(1);
+               vorderrad.sendBuffer(497,uint16_t((float)daumen.getValue()/100.0f*4096));
+               hinterrad.sendBuffer(497,uint16_t((float)daumen.getValue()/100.0f*4096));
+               printf("State: recup\n\r");
+               recup = true;
+            }
+            else if(daumen.getValue() < Phaserunner::MIN_RECUPERATION) recup = false;
+         
+            if(Bremsen && !gebremst){
+               write = -5;
+               // write 0 to phaserunner
+               vorderrad.sendBuffer(495,0);
+               hinterrad.sendBuffer(495,0);
+               wait_ms(1);
+               vorderrad.sendBuffer(497,uint16_t(0.10f*4096));
+               hinterrad.sendBuffer(497,uint16_t(0.10f*4096));
+               printf("State: bremsen\n\r");
+               gebremst = true; 
+            }
+            else if(!Bremsen) gebremst = false;  
+            
+            
+            
+            int wert1 = daumen;
+            float wert2 = daumen1.read();
+            float u;
+            if(i%200 == 0){
+                i = 0;
+                vorderrad.readBuffer(262);
+                u = vorderrad.getVoltage();
+                printf("P=%.3f, Torque=%.3f , RPM_Ped =%.3f, U=%.2f\n\r",F_PMech,torque,F_RPM,u);
+                //printf("Daumen1: %d, Daumen2: %.2f\n\r",wert1,wert2);
+            }
+          
+           
+           //MessungP[i] = F_PMech;
+           //MessungPID[i] = torque;
+           //MessungFreq[i] = F_RPM;
+           //writeArr[i] = write;
+            
             // Store Old Values
             angleOld = angle_rad;
-            F_ValueOld = F_Value;
+            F_Leistung1Old = F_Leistung1;
             F_RPMOld = F_RPM;
             F_AccOld = F_Acc;
+            F_PMechOld =  F_PMech;
+            BremsenOld = Bremsen;
+            BremsenOld2 = BremsenOld;
+
             i++;
-            wait(0.005);// Set to 5 ms
-      /* }
-       printf("[");
+            
+            wait(dt);// Set to 5 ms
+       /*}
+       pc.printf("[");
        for(i=0;i<3999;i++){
-            printf("%.3f,%.3f,%.3f;...\n\r",MessungP[i],MessungFreq[i],MessungPID[i]);
+            pc.printf("%.2f,%.2f,%.2f,%d;...\n\r",MessungP[i],MessungFreq[i],MessungPID[i],writeArr[i]);
        }
-       printf("%.3f,%.3f,%.3f];\n\r",MessungP[3999],MessungFreq[3999],MessungPID[3999]);
+       pc.printf("%.2f,%.2f,%.2f,%d];\n\r",MessungP[3999],MessungFreq[3999],MessungPID[3999],writeArr[3999]);
        i=0; 
-       */
+      */
+}
+  
 }
 
-
-
-   
-}
\ No newline at end of file