Dit is het passieve stuurprogramma van de handorthese behorend bij mijn bacheloropdracht. Groet, Menno Sytsma
Dependencies: EMG FastPWM HIDScope mbed-src
Diff: main.cpp
- Revision:
- 0:46b6258b2b00
- Child:
- 1:f63d8a73460c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon May 30 08:50:13 2016 +0000 @@ -0,0 +1,270 @@ +#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); +bool 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. +AnalogIn Referentie(A0); // De schuifpotmeter naast de geleiding van de draad is de referentie. +AnalogIn Boardpotmeter(A1); // POT1 op het board wordt gebruikt als input van de controlloop. +DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield) +FastPWM motor1speed(D6); +DigitalOut Led(LED_RED); +AnalogIn Safety(A5); + +float potmeter; +float Error; +float Referentie2=1.0; +float Input; +float Output; +double Error_prev = 0; +double Error_int = 0; +double Error_der; +// 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) { + 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 = Filter2.step(Inp); + Input = 6+(3*-cos(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() // De P_controller, door de ticker elke honderdste seconde uitgevoerd. +{ + if(Output>=0) { + motor1direction.write(1); + motor1speed = 0.3*fabs(Output); + } else if (Output<0) { + motor1direction.write(0); + motor1speed = fabs(Output); + } +} + +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<=640) { + Loopticker.attach(tickerfunctie2,0.01); + Hidscope.attach(scopeSend,0.01); // Verzenden naar HIDscope + while(1 && J<=640) { + 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); + Led = 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. + Error = Errorberekening(Referentie2, Input); + Output = PID_controller(Error,1,0.5,1,0.01, Error_prev, Error_int); + Motor_controller(); + } + } + motor1speed.write(0); + +} + +void tickerfunctie() +{ + LoopTimerFlag = 1; +} + +void Determinetask() +{ + if (Button1pressed == 0) { + + Groen = 0; + N++; + wait (0.5); + Groen = 1; + } +} + +/* void Extendfinger() +{ + //i = i+1; + wait (0.5); // Knipper 2 keer met Rood en 1 keer met paars + Rood = 0; + wait (0.5); + Rood = 1; + wait (0.5); + Rood = 0; + wait (0.5); + Rood = 1; + wait (0.5); + Rood = 0; + Blauw = 0; + wait (0.5); + Rood = 1; + Blauw = 1; + wait (0.5); +} */ + +int main() +{ + Rood = 1; + Blauw = 1; + Groen = 1; + int i = 0; + pc.printf("Hello World! %i \n", i); + Finitestatemachine.attach(tickerfunctie,0.1); + + 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) { + Setting = true; + 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 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"); + } + + } + } +}