Dit is het passieve stuurprogramma van de handorthese behorend bij mijn bacheloropdracht. Groet, Menno Sytsma
Dependencies: EMG FastPWM HIDScope mbed-src
main.cpp
- Committer:
- s1503189
- Date:
- 2016-06-01
- Revision:
- 6:e206abd0b2ca
- Parent:
- 5:f97bf30bec14
- Child:
- 7:9ea55ce667be
File content as of revision 6:e206abd0b2ca:
#include "mbed.h" #include "FastPWM.h" #include <iostream> #include "HIDScope.h" #include "math.h" #include "biquadFilter.h" DigitalOut Rood(LED_RED); DigitalOut Groen(LED_GREEN); DigitalOut Blauw(LED_BLUE); DigitalIn Button1pressed(SW2); DigitalIn Button2pressed(SW3); volatile bool LoopTimerFlag = 0; Serial pc(USBTX, USBRX); int N; bool Start(0); double Setting(0); bool Dotask(0); int i; Ticker Finitestatemachine; // Verklaren van de in en outputs Ticker Loopticker; // Deze ticker wordt gebruikt om de looptimerflag aan te roepen. volatile bool LoopTimerFlag2 = 0; // Volatile, omdat deze heel vaak verandert van waar naar onwaar. //Ticker Loopticker2; // Deze ticker wordt gebruikt om de looptimerflag aan te roepen. //volatile bool LoopTimerFlag3 = 0; AnalogIn Referentie(A0); // De schuifpotmeter naast de geleiding van de draad is de referentie. AnalogIn Boardpotmeter(A2); // POT1 op het board wordt gebruikt als input van de controlloop. AnalogIn Boardpotmeter2(A1); DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield) FastPWM motor1speed(D6); float potmeter; float Error; float Referentie2;//=0.0; float Input; float Output; float POT; float Foutje; float Potmetertje; double Error_prev = 0; double Error_int = 0; double Error_der; double Ref_der = 0; double Ref_prev = 0; float Amplitude; float Baseline; // Define the HIDScope and Ticker object HIDScope scope(4); Ticker Hidscope; // Lowpassfilter const double a1_LP = -1.561018075800718, a2_LP = 0.641351538057563; const double b0_LP = 0.020083365564211, b1_LP = 0.040166731128423, b2_LP = 0.020083365564211; const double a1_LP2 = -1.561018075800718, a2_LP2 = 0.641351538057563; const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211; //const double a1_LP2 = -1.866892279711715, a2_LP2 = 0.875214548253684; //const double b0_LP2 = 0.002080567135492, b1_LP2 = 0.004161134270985, b2_LP2 = 0.002080567135492; //const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125; //const double b0_HP = 0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663; float time_; const float time_increment = 0.01; biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); biquadFilter Filter2(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2); double Referentieschaling(double A, double B) { double Y= 787.3008916207206*pow(A,4) -565.1143141517078*pow(A,3) + 122.8516837382677*pow(A,2) + 0.0556616744031*A + 0.0912411880277; Referentie2 = Filter1.step(Y); if (Referentie2<=2.5 or Referentie2>=9.2) { Rood = 0; Referentie2 = B; } return Referentie2; } double Inputberekening(double B) { Input = Baseline+(Amplitude*-cos(2*time_)); if (Input>=9) { // De Input moet binnnen een aantal grenzen blijven. Groter dan 7 is mechanisch niet mogelijk. Input=9; // Bij een waarde kleiner dan 1,5 zijn de strings niet meer gewikkeld en werkt de controller averechts en is deze uiterst instabiel. } else if (Input<=3.0) { Input = 3.0; } return Input; } double Errorberekening(double Input,double Ref) { Error = Input-Ref; // Het Error-signaal wordt ook gebruikt voor de PWMOut, dus mag deze niet hoger worden dan 1, 1 is immers al full speed voor de motor. /*if (Error>=1) { Error=1; } else if (Error<=-1) { Error = -1; } else if (fabs(Error)<0.01) { Error = 0; } */ return Error; } double PID_controller(double Error, double KP, double KI, double KD, double Ts, double &Error_int, double &e_prev) { Error_der = (Error-Error_prev)/Ts; Error_prev = Error; Error_int = Error_int + Ts*Error; return KP*Error+KI*Error_int+KD*Error_der; } // The data read and send function void scopeSend() { scope.set(0,Referentie2); // Kanaal 1 van HIDscope geeft: De positie van de schuifpotmeter in cm. scope.set(1,Input); // Kanaal 2 van HIDscope geeft: De gewenste positie in cm door de potmeter op het board ingesteld. scope.set(2,Error); // Kanaal 3 van HIDscope geeft: De waarde van de Error die de P_controller in gaat. scope.set(3,motor1speed.read());// Kanaal 1 van HIDscope geeft: De snelheid van de motor, is een absolute waarde, richting wordt elders gegeven. scope.send(); } void tickerfunctie2() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden. { LoopTimerFlag2 = 1; } void Motor_controller(float Output) // De P_controller, door de ticker elke honderdste seconde uitgevoerd. { if(Ref_der < 0) { if(Output>=0) { motor1direction.write(1); motor1speed = 0.01*fabs(Output); } else if (Output<0) { motor1direction.write(0); motor1speed = 0.05*fabs(Output); } } if(Ref_der > 0) { if(Output>=0) { motor1direction.write(1); motor1speed = (20*fabs(Output)); } else if (Output<0) { motor1direction.write(0); motor1speed = 0.01*fabs(Output); } } } void Motor_controller2(float Foutje) { if(Foutje>=0.01){ motor1direction.write(1); motor1speed.write(0.05*Foutje); } else if(Foutje<=-0.01){ motor1direction.write(0); motor1speed.write(-Foutje); } else { motor1speed.write(0); } } void Extendfinger() { Error = 0; int J = 0; time_ = 0; // Voordat het hele programma begint, staat de Error op 0, zodat de motor niet spastisch gaat draaien om dit te compenseren. while(1 && J<=320) { while(LoopTimerFlag2 !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop. LoopTimerFlag2 = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma J= J+1; //pc.printf(" J is %i \n",J); Rood = 1; double Input = Inputberekening(Boardpotmeter.read()); time_ += time_increment; double Ref2 = Referentieschaling(Referentie.read()/2,Input); // De referentiewaarde is via deze functie (gevonden met metingen en polyfit) verbonden aan de afstand in centimeters voor waarden tussen 0 en 0.5. Ref_der = Referentie2-Ref_prev; Ref_prev = Referentie2; Error = Errorberekening(Input, Referentie2); Output = PID_controller(Error,5,1,0.1,0.01, Error_prev, Error_int); Motor_controller(Output); } //} motor1speed.write(0); } void tickerfunctie() { LoopTimerFlag = 1; } void P_controller(double Max, bool Keuze) { while (Button1pressed.read()==1) { while(LoopTimerFlag2 !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop. LoopTimerFlag2 = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma if (bool (0)) { POT = Boardpotmeter.read(); } else if (bool (1)) { POT = Boardpotmeter2.read(); } Input = 10*POT; //Inputberekening2(POT,0); Inputberekening(Boardpotmeter.read()); // De potmeter geeft ook waardes tussen 0 en 1, dit wordt met een factor 10 geschaald zodat deze als een positie in cm opgelegd kunnen worden. // time_ += time_increment; double Ref2 = Referentieschaling(Referentie.read()/2,Input); // De referentiewaarde is via deze functie (gevonden met metingen en polyfit) verbonden aan de afstand in centimeters voor waarden tussen 0 en 0.5. Rood = 1; //Ref_prev = Ref2; Error = Referentie2-Input; // pc.printf("Input = %f Referentie= %f Error= %f \n", Input, Referentie2, Error); Motor_controller2(Error); // pc.printf("output = %f \n", motor1speed.read()); } Groen = 0; wait (0.5); } void Determinetask() { if (Button1pressed == 0) { Groen = 0; N++; wait (0.5); Groen = 1; } } int main() { Rood = 1; Blauw = 1; Groen = 1; int i = 0; pc.printf("Hello World! %i \n", i); Finitestatemachine.attach(tickerfunctie,0.1); Loopticker.attach(tickerfunctie2,0.01); Hidscope.attach(scopeSend,0.01); // Verzenden naar HIDscope while (true) { while(LoopTimerFlag !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop. LoopTimerFlag = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma if (Start==0 && Setting==0 && Dotask==0) { // In eerste instantie gaan de leds even uit Start = 1; Groen = 0; pc.printf("State 1\n"); } if (Start==1 && Setting==0 && Dotask==0) { // State 2 heeft Geel als kleur Groen = 0; Rood = 0; pc.printf("State 2\n"); if (Button1pressed.read()==1 && Button2pressed.read()==0) { if (Amplitude != 0){ Setting = 2; Start = false; Groen = 1; Rood = 1; N = 0; pc.printf("State 2 D\n"); wait(0.5);} else { Setting = 1; Start = false; Groen = 1; Rood = 1; pc.printf("State 2 A\n"); wait(0.5);} } if (Button1pressed.read()==0 && Button2pressed.read()==1 && N>0) { Groen = 1; Rood = 1; Dotask = 1; Start=0; pc.printf("State 2 B\n"); wait(0.5); } } if (Start==0 && Setting==1 && Dotask==0) { // Leds zijn uit, elke bepaling gaat het groene lampje even branden. pc.printf("State 2 C\n"); P_controller(0,0); double Maximum = 10*POT; Groen = 1; pc.printf("Max = %f \n", Maximum); P_controller(0,1); double Minimum = 10*POT; Groen = 1; pc.printf("Min = %f \n", Minimum); Amplitude = (Minimum+Maximum)/2; Baseline = (Maximum-Minimum)/2; Rood = 0; Blauw = 0; wait (2); Rood = 1; Blauw = 1; Setting = 2; } if (Start==0 && Setting==2 && Dotask==0) { // Leds zijn uit, elke count gaat het groene lampje even branden. pc.printf("State 3\n"); Determinetask(); // Aantal gewenste herhalingen van Dotask instellen pc.printf("N = %i \n", N); if (Button1pressed.read()== 1 && Button2pressed.read()== 0) { Setting = 0; Dotask = 1; pc.printf("State 3 B\n"); } } if (Start==0 && Setting==0 && Dotask==1) { pc.printf("State 4\n"); while(i < N) { pc.printf("i = %i N = %i \n", i, N); Extendfinger(); //int J = 0; pc.printf("Extendfinger()afgerond\n", i, N); i++; } if (i==N) { Dotask = 0; Start = 1; i=0; pc.printf("Final state \n"); } } } }