Menno Sytsma
Dit is het passieve stuurprogramma van de handorthese behorend bij mijn bacheloropdracht. Groet, Menno Sytsma
Dependencies: EMG FastPWM HIDScope mbed-src
Revision 0:46b6258b2b00, committed 2016-05-30
- Comitter:
- s1503189
- Date:
- Mon May 30 08:50:13 2016 +0000
- Child:
- 1:f63d8a73460c
- Commit message:
- Final principle werkend, slechte Controllerwaarden
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EMG.lib Mon May 30 08:50:13 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/Numero-Uno/code/EMG/#2eb227a21a93
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FastPWM.lib Mon May 30 08:50:13 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/Sissors/code/FastPWM/#87e38b846651
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HIDScope.lib Mon May 30 08:50:13 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/tomlankhorst/code/HIDScope/#5020a2c0934b
--- /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");
+ }
+
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-src.lib Mon May 30 08:50:13 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-src/#a11c0372f0ba