main.cpp

00001 #include "mbed.h"
00002 #include "Encoder.h"
00003 #include "math.h"
00004 #include "Phaserunner.h"
00005 #include "Daumenbetaetigung.h"
00006 #include "Handgriffbetaetigung.h"
00007 #include "PID.h"
00008 #define PI 3.14159
00009 
00010 
00011 
00012 int main(){
00013 
00014     RawSerial motorV(PA_0, PA_1, 115200);
00015     RawSerial motorH(PC_6, PC_7,  115200);
00016     RawSerial pedalL(PC_10, PC_11, 115200);
00017     RawSerial pedalR(PC_12, PD_2, 115200);
00018     
00019     Phaserunner vorderrad(motorV);
00020     Phaserunner hinterrad(motorH);
00021     Phaserunner linkspedal(pedalL);
00022     Phaserunner rechtspedal(pedalR);
00023     
00024     Daumenbetaetigung daumen;
00025     Handgriffbetaetigung gasGlied;
00026     
00027     
00028     AnalogIn Strom(PB_0);
00029     AnalogIn Spannung(PC_1);
00030     AnalogOut GegenMomLinks(PA_4);
00031     AnalogOut GegenMomRechts(PA_5);
00032     
00033     DigitalOut PedalL_ON_OFF(PC_14);
00034     DigitalOut PedalR_ON_OFF(PC_15);
00035     DigitalIn Taste(PC_8);
00036     
00037     PinName Hallsensor(PA_13);  
00038     Encoder encoder(Hallsensor); 
00039     
00040     Serial pc(USBTX,USBRX);
00041     pc.baud(19200);
00042     DigitalOut led(LED2);
00043     DigitalOut ON_OFF(PH_1);
00044     
00045     DigitalOut Abvorne(PB_7);
00046     DigitalOut Abhinten(PC_13);
00047     
00048     DigitalIn Bremsen(PA_14);
00049     Bremsen.mode(PullDown);
00050     AnalogIn daumen1(A5); //qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqq
00051     
00052     ON_OFF = true;
00053     uint16_t rot,rotOld,daumenValue;
00054     double angle_rad, angleOld=0;
00055     float a=0.0f,b=0.0f,beta=0.0f; // Ellipse Parameters
00056     float R=0.0f, phi=0.0f, Rold=0.0f;
00057     float strom, spannung, leistung1, leistung2; //1:Elektrische Lesitung Batterie, 2:Mechanische (M*omega), 3: Leistung aus den Phaserunner
00058     float leistungsOffset = 15.0f; // Leerlauf Leistung 
00059     float RPM, Acc; // Value form Encoder
00060     float MessungP[4001], MessungFreq[4001],MessungAngle[4001], MessungPID[4001];
00061     float rec=0.0f; // recuperation 0..100%
00062     float recAntret;
00063     uint32_t i=0;
00064 
00065     led = false;
00066     pc.printf("Hello bike\n\r");
00067 
00068     
00069     PedalL_ON_OFF = true;
00070     PedalR_ON_OFF = true;
00071     wait(0.2);
00072     PedalL_ON_OFF = false;
00073     PedalR_ON_OFF = false;
00074     wait(0.2);
00075     PedalL_ON_OFF = true;
00076     PedalR_ON_OFF = true;
00077     float LeerlaufLeistung = 15.0f;
00078     pc.printf("Siamo accesi\n\r");
00079     wait(1);
00080     
00081     linkspedal.setTorque(0);
00082     rechtspedal.setTorque(0);
00083     vorderrad.setTorque(0);
00084     hinterrad.setTorque(0);
00085     wait(0.2);
00086     
00087     // Pedelec Values
00088     float torque, torqueOld=0.0f; // Torque an den Räder
00089     float pedFaktor; // Faktor zwischen PedaleLeistung und Moment an der Rädern
00090 
00091     
00092     // Lowpass Filter Leistung
00093     float T_ab = 0.005; // Abtastzeit: 5ms
00094     float F_ab = 1/T_ab; // Abtastfrequenz
00095     float Omega_c = 2*PI*F_ab/1000; // 1000 Mal kleiner als die Abtastfrequenz
00096     float sf = (Omega_c*T_ab)/(1+Omega_c*T_ab);//smoothing factor
00097     float F_Leistung1, F_Leistung1Old = 0.0f; 
00098     
00099     // Lowpass Filter RPM
00100     float Omega_cRPM = 2*PI*F_ab/150; // 150 Mal kleiner als die Abtastfrequenz
00101     float sfRPM = (Omega_cRPM*T_ab)/(1+Omega_cRPM*T_ab);//smoothing factor
00102     float F_RPM, F_RPMOld = 0.0f;
00103     
00104     // Lowpass Filter Mech.Leistung
00105     float Omega_cPMech = 2*PI*F_ab/500; // 500 Mal kleiner als die Abtastfrequenz
00106     float sfPMech = (Omega_cPMech*T_ab)/(1+Omega_cPMech*T_ab);//smoothing factor
00107     float F_PMech, F_PMechOld = 0.0f;
00108     
00109     // Lowpass Filter Acceleration
00110     float Omega_cAcc = 2*PI*F_ab/200; // 200 Mal kleiner als die Abtastfrequenz
00111     float sfAcc = (Omega_cAcc*T_ab)/(1+Omega_cAcc*T_ab);//smoothing factor
00112     float F_Acc, F_AccOld = 0.0f;
00113     
00114     //Diskrete Ableitung
00115     float dt = 0.005f; //5ms
00116     float PedaleFreq = 0.0f,PedaleFreqOld = 0.0f, PedaleFreqFil, PedaleFreqFilOld=0.0f; // in rad/s
00117     
00118     // PID instance
00119     float Kp = 1.2, Ki = 40, Kd = 0.0;
00120     PID pid(Kp, Ki, Kd, dt, true);
00121     pid.setInputLimits(0.0,200.0);
00122     pid.setOutputLimits(1.0,100.0);
00123     pid.setMode(1); //Regulator
00124     
00125     // Antretvariablen
00126     uint16_t verlauf=0;
00127     float tau=0.8f; // tau für exponentiel funktion in s : 0.8s;
00128     
00129     bool init1= false, init = false;
00130     bool richtung1= false, richtung2 = false;
00131     bool writeNull = false; // write 0 to phaseerunner one time
00132     int8_t write=0, writeArr[4001];
00133     bool BremsenOld = false,BremsenOld2 = false;
00134     
00135     while(!encoder.reset()){
00136         if(!init){
00137            rechtspedal.setTorque(80);
00138            init = true;
00139            wait(0.04);
00140            rechtspedal.setTorque(3);
00141         }
00142     }
00143     rechtspedal.setTorque(0);
00144     wait(0.1); 
00145     init = false;
00146      float leistungsfaktor = 4.8; // Faktor zwischen Elektrischeleistung und mechanische Leistung %
00147 
00148 while(1){
00149        // while(i<4000){
00150 
00151             
00152             // Measure data form sensors
00153             angle_rad = encoder.readAngle();
00154             RPM = encoder.readRPM();
00155             Acc = encoder.readAcceleration();
00156             strom = ((Strom.read()*3.3f)-2.5f)/0.025f; 
00157             spannung = (Spannung.read()*42.95f);
00158             leistung1 = strom*spannung-leistungsOffset; 
00159             
00160             // LowPass Filter leistung
00161             F_Leistung1 = sf * leistung1 + (1-sf)*F_Leistung1Old;
00162             
00163             // LowPass Filter RPM
00164             F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld;
00165             
00166             // LowPass Filter ACC
00167             F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld;
00168             
00169             // Regulator
00170             //if(F_RPM > 1.1 * pid.getSetPoint()) pid.setSetPoint(F_RPM);
00171             //else if (F_RPM < 0.9f * pid.getSetPoint()) pid.setSetPoint(F_RPM);
00172             pid.setSetPoint(70.0);
00173             pid.setProcessValue(F_RPM);
00174             
00175             
00176             
00177 
00178             // Ellipse
00179             a = 1.0f; // % of Torque Max 0..1
00180             b = 0.6f; // % of Torque Max 0..1
00181             beta = 0.52f; // 30°
00182             phi = angle_rad;  
00183             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
00184             daumenValue = 10;
00185             if(daumenValue < 1) daumenValue = 1;
00186             rec = ((float)daumenValue + pid.getCalculation())*R;
00187             
00188             // Antret
00189             if(!Bremsen && !init1){
00190                 init1 = true;
00191             }
00192             else recAntret = 100.0;
00193             if(!init && init1){
00194                 recAntret = 100*exp(-(dt*verlauf)/ tau);
00195                 verlauf++;
00196                 if(recAntret < rec) init = true; // Use just the ellipse when rec_start < 10%
00197             }
00198             if(!init) rec = recAntret;
00199             else{
00200                 // if(F_Acc > 5.0){
00201                     rec = rec ;
00202                  //}
00203             }
00204             
00205             // Mechanische Leistung in % berechnet
00206             leistung2 = 4.0*PI*F_RPM/60.0*rec; // 2 * omega * Gegenmoment in %
00207             leistung2 = leistung2/leistungsfaktor;
00208             
00209             // LowPass Filter Mech Leistung
00210             F_PMech = sfPMech * leistung2 + (1-sfPMech)*F_PMechOld;
00211 
00212             
00213             if(rec < 0.0) rec = 0.0;      
00214             GegenMomRechts.write(0.02+(rec*0.01f)); // offset for 0.2V -> ~0.02
00215             GegenMomLinks.write(0.02+(rec*0.01f));  // offset for 0.2V -> ~0.02
00216             
00217             // Hilfemoment für Rücktritt steuern
00218             if(F_RPM < -5.0f && !richtung1){
00219                 rechtspedal.setTorque(3);
00220                 richtung1 = true;
00221                 richtung2 = false;
00222             }
00223             else if(F_RPM > -3.0f && !richtung2){
00224                 rechtspedal.setTorque(0);
00225                 richtung1 = false;
00226                 richtung2 = true;
00227             }
00228             
00229             pedFaktor = 2.0f;
00230             
00231             static bool gebremst = false;
00232             static bool recup = false;
00233             
00234             if(F_RPM > 1.0f && !Bremsen && !recup){
00235                 torque = (F_PMech / 300.0 * 100)*pedFaktor;
00236                 if(i%10 == 0 && (torque < (torqueOld*0.95) || torque > (torqueOld * 1.05))) {
00237                     vorderrad.setRecuperation(0);
00238                     hinterrad.setRecuperation(0); 
00239                     // write torque to phaserunner;
00240                     //vorderrad.setRecuperation(uint16_t((80+(torque))*40.96f));
00241                     vorderrad.setTorque(uint16_t(10+torque));
00242                     //hinterrad.setTorque(uint16_t(20+torque));
00243                     //vorderrad.setTorque(0);
00244                     hinterrad.setTorque(0);
00245                     write = 10;
00246                 torqueOld = torque;
00247                 printf("State: torque\n\r");
00248                   
00249                 }
00250                 else write = 0;
00251                 writeNull = false;
00252 
00253       
00254             }
00255             else if(!writeNull){
00256                writeNull = true;
00257                write = -10;
00258                // write 0 to phaserunner
00259                vorderrad.setTorque(0);
00260                hinterrad.setTorque(0);
00261                
00262             }
00263             else write = 0; 
00264             
00265             if((daumen.getValue() > Phaserunner::MIN_RECUPERATION)&& !recup){
00266                vorderrad.setTorque(0);
00267                hinterrad.setTorque(0);
00268                wait_ms(5);
00269                vorderrad.setRecuperation(daumen);
00270                hinterrad.setRecuperation(daumen);
00271                printf("State: recup\n\r");
00272                recup = true;
00273             }
00274             else if(daumen.getValue() < Phaserunner::MIN_RECUPERATION){
00275                  recup = false;
00276             }
00277             if(Bremsen && !gebremst){
00278                write = -5;
00279                // write 0 to phaserunner
00280                vorderrad.setTorque(0);
00281                hinterrad.setTorque(0);
00282                wait_ms(5);
00283                vorderrad.setRecuperation(10);
00284                hinterrad.setRecuperation(10);
00285                printf("State: bremsen\n\r");
00286                gebremst = true; 
00287             }
00288             else if(!Bremsen){
00289                  gebremst = false; 
00290            }        
00291     
00292             
00293             
00294             int wert1 = daumen;
00295             float wert2 = daumen1.read();
00296             float u;
00297             if(i%200 == 0){
00298                 i = 0;
00299                 //vorderrad.readBuffer(262);
00300                 u = vorderrad.getVoltage();
00301                 printf("P=%.3f, Torque=%.3f , RPM_Ped =%.3f, U=%.2f\n\r",F_PMech,torque,F_RPM,u);
00302                 //printf("Daumen1: %d, Daumen2: %.2f\n\r",wert1,wert2);
00303             }
00304           
00305            
00306            //MessungP[i] = F_PMech;
00307            //MessungPID[i] = torque;
00308            //MessungFreq[i] = F_RPM;
00309            //writeArr[i] = write;
00310             
00311             // Store Old Values
00312             angleOld = angle_rad;
00313             F_Leistung1Old = F_Leistung1;
00314             F_RPMOld = F_RPM;
00315             F_AccOld = F_Acc;
00316             F_PMechOld =  F_PMech;
00317             BremsenOld = Bremsen;
00318             BremsenOld2 = BremsenOld;
00319 
00320             i++;
00321             
00322             wait(dt);// Set to 5 ms
00323        /*}
00324        pc.printf("[");
00325        for(i=0;i<3999;i++){
00326             pc.printf("%.2f,%.2f,%.2f,%d;...\n\r",MessungP[i],MessungFreq[i],MessungPID[i],writeArr[i]);
00327        }
00328        pc.printf("%.2f,%.2f,%.2f,%d];\n\r",MessungP[3999],MessungFreq[3999],MessungPID[3999],writeArr[3999]);
00329        i=0; 
00330       */
00331 }
00332   
00333 }
00334