Dit is het actieve stuurprogramma, behorend bij de bacheloropdracht van Menno Sytsma over de integratie van een Twisted string actuator in een handorthese
Dependencies: EMG FastPWM HIDScope mbed-src
main.cpp
- Committer:
- s1503189
- Date:
- 2016-06-10
- Revision:
- 3:5b5b8ebdc46c
- Parent:
- 2:f5bb7fcfc7a2
File content as of revision 3:5b5b8ebdc46c:
#include "mbed.h" #include "HIDScope.h" #include "biquadFilter.h" #include "FastPWM.h" // Define the HIDScope and Ticker object HIDScope scope(5); Ticker scopeTimer; volatile bool LoopTimerFlag; Ticker ForceTimer; volatile bool LoopTimerFlag2; // Read the analog input AnalogIn a_in2(A3); //OUT- AnalogIn a_in3(A4); //OUT+ Ticker Loopticker; // Deze ticker wordt gebruikt om de looptimerflag aan te roepen. AnalogIn Referentie(A0); // De schuifpotmeter naast de geleiding van de draad is de referentie. AnalogIn Boardpotmeter2(A2); DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield) FastPWM motor1speed(D6); DigitalOut Led(LED_RED); DigitalOut Roodisplus(D5); DigitalOut Versterker(D4); float Error; float Referentie2;//=1.0; float Output; float Input; double Error_prev = 0; double Error_int = 0; double Error_der; double Ref_der = 0; double Ref_prev = 0; double Ref_prev2 = 0; double Ref2; double Input3; double Plaats_der; double Force_nieuw=0; double Ref_der_prev; double Plaats_der1; double Plaats_der2; //LowPass filter 2 Hz const double a1_LP = -1.940778135263835, a2_LP = 0.942482022027066; const double b0_LP = 0.000425971690807714, b1_LP = 0.000851943381615428, b2_LP = 0.000425971690807714; const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125; const double b0_HP = 0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663; const double a1_LP5 = -1.822694925196308, a2_LP5 = 0.837181651256023 ; const double b0_LP5 = 0.003621681514929 , b1_LP5 = 0.007243363029857, b2_LP5 = 0.003621681514929; const double a1_LP2 = -1.561018075800718, a2_LP2 = 0.641351538057563; const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211; biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); //(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); // Lowpassfilter voor signaal van krachtsensor, 300 Hz, fk = 2 biquadFilter Filter2(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); //(a1_HP, a2_HP, b0_HP, b1_HP, b2_HP); // Highpassfilter voor signaal van krachtsensor, 300 Hz, fk = 10 biquadFilter Filter3(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2); // Lowpassfilter voor signaal positie-potmeter, 300 Hz biquadFilter Filter4(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); // Lowpassfilter voor geschaalde signaal positie-potmeter, 300 Hz biquadFilter Filter6(a1_LP5, a2_LP5, b0_LP5, b1_LP5, b2_LP5); // Laagdoorlaatfilter 100 Hz, fk is 2 Hz /* Aanpasmogelijkheden die succes kunnen geven: -Positiesignaal ook door highpassfilter gooien, filters voor krachtsensor en positie gelijkstellen. -Lowpassfilter van geschaalde signaal weghalen. - Signalen bekijken van referentie, ref_der en de tweede afgeleide. Wellicht ook derde afgeleide bekijken. - */ double Y_filt; double Y_filt2; double Y_filtprev; double Y_filtder; int A; float time_; const float time_increment = 0.005; // The data read and send function void scopeSend() { scope.set(0,Force_nieuw); //Y_filt); scope.set(1,Y_filt2); scope.set(2,Plaats_der1); scope.set(3,Input);//Referentie2);// scope.set(4,A);//Y_filtder);// scope.send(); } void tickerfunctie() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden. { LoopTimerFlag = 1; } void tickerfunctie2() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden. { LoopTimerFlag2 = 1; } 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 = Filter4.step(Y); if (Referentie2<=2.5 or Referentie2>=9.2) { Led = 0; Referentie2 = B; } return Referentie2; } double Inputberekening(double B) { double Inp = 10*B; // 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. Input = Filter3.step(Inp); 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 Force_prev; double Input_prev3; double Inputberekening3(double B) { if (Force_nieuw>=2.5) { if (A == 1) { Input3 = Input_prev3-0.01; } if (A == 0 && Force_prev>2.5) { A = 0; } if (A <= 0 && Force_prev<=2.5) { A++; Input3 = Input_prev3+0.01; } } else if (Force_nieuw<=-2.5) { if (A == -1) { Input3 = Input_prev3+0.01; } if (A == 0 && Force_prev<-2.5) { A = 0; } if (A >= 0 && Force_prev>=-2.5) { A--; Input3 = Input_prev3-0.01; } } else { Input3 = Input_prev3 - A*0.01; } Force_prev= Force_nieuw; if (Input3 >= 9) { Input3 =9; } if (Input3 <= 3.5) { Input3 =3.5; } Input_prev3 = Input3; return Input3; } double Errorberekening(double Ref,double Input) { 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. 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*Ts*Error_int+KD*Ts*Error_der; } void Afgeleide_Force() { Y_filtder = (Force_nieuw-Y_filtprev)/0.01; Y_filtprev = Force_nieuw; } void Afgeleide_Plaats() { LoopTimerFlag2 = 0; Plaats_der = Ref_der/0.1; Plaats_der1 = (Filter6.step(Plaats_der)/(Boardpotmeter2.read()*5))-1; Plaats_der2 = (Plaats_der1-Ref_der_prev); if (Plaats_der1>=-1) { Plaats_der1 = Plaats_der1; } else if (Plaats_der1<-1) { Plaats_der1 = (Filter6.step(Plaats_der)/(Boardpotmeter2.read()*5))*0.5-1; } Ref_der_prev= Plaats_der1; } void Motor_controller() { if(Output>=0) { motor1direction.write(1); motor1speed = 0.2+fabs(Output); } else if (Output<0) { motor1direction.write(0); motor1speed = 0.05*fabs(Output); } } int main() { Roodisplus.write(1); Versterker.write(1); // Attach the data read and send function at 100 Hz scopeTimer.attach_us(&tickerfunctie, 1/0.0003);// 1e4); ForceTimer.attach_us(&tickerfunctie2, 1e4); while(1) { 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 Led = 1; Y_filt = 5*Filter1.step(a_in3.read())+1; Y_filt2 = Filter2.step(Y_filt); Input = Inputberekening3(Force_nieuw); time_ += time_increment; 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. Error = Errorberekening(Ref2, Input); Ref_der = Referentie2-Ref_prev; Ref_prev = Referentie2; Output = PID_controller(Error,3,2,1,0.01, Error_prev, Error_int); Motor_controller(); if (LoopTimerFlag2==1) { Afgeleide_Force(); Afgeleide_Plaats(); Force_nieuw = Y_filtder-Plaats_der2; } scopeSend(); } }