Menno Sytsma / Passief_stuurprogramma

Dit is het passieve stuurprogramma van de handorthese behorend bij mijn bacheloropdracht. Groet, Menno Sytsma

Dependencies:   EMG FastPWM HIDScope mbed-src

main.cpp@7:9ea55ce667be, 2016-06-21 (annotated)

Committer:
s1503189
Date:
Tue Jun 21 15:02:23 2016 +0000
Revision:
7:9ea55ce667be
Parent:
6:e206abd0b2ca 
Versie zonder pc.printf en comments;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1503189 0:46b6258b2b00 1 #include "mbed.h"
s1503189 0:46b6258b2b00 2 #include "FastPWM.h"
s1503189 0:46b6258b2b00 3 #include <iostream>
s1503189 0:46b6258b2b00 4 #include "HIDScope.h"
s1503189 0:46b6258b2b00 5 #include "math.h"
s1503189 0:46b6258b2b00 6 #include "biquadFilter.h"
s1503189 0:46b6258b2b00 7
s1503189 0:46b6258b2b00 8 DigitalOut Rood(LED_RED);
s1503189 0:46b6258b2b00 9 DigitalOut Groen(LED_GREEN);
s1503189 0:46b6258b2b00 10 DigitalOut Blauw(LED_BLUE);
s1503189 0:46b6258b2b00 11 DigitalIn Button1pressed(SW2);
s1503189 0:46b6258b2b00 12 DigitalIn Button2pressed(SW3);
s1503189 0:46b6258b2b00 13 volatile bool LoopTimerFlag = 0;
s1503189 0:46b6258b2b00 14 Serial pc(USBTX, USBRX);
s1503189 0:46b6258b2b00 15
s1503189 0:46b6258b2b00 16 int N;
s1503189 0:46b6258b2b00 17 bool Start(0);
s1503189 2:1dd9e630a7b5 18 double Setting(0);
s1503189 0:46b6258b2b00 19 bool Dotask(0);
s1503189 0:46b6258b2b00 20 int i;
s1503189 0:46b6258b2b00 21
s1503189 0:46b6258b2b00 22 Ticker Finitestatemachine;
s1503189 0:46b6258b2b00 23
s1503189 0:46b6258b2b00 24 // Verklaren van de in en outputs
s1503189 0:46b6258b2b00 25 Ticker Loopticker; // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
s1503189 0:46b6258b2b00 26 volatile bool LoopTimerFlag2 = 0; // Volatile, omdat deze heel vaak verandert van waar naar onwaar.
s1503189 0:46b6258b2b00 27 AnalogIn Referentie(A0); // De schuifpotmeter naast de geleiding van de draad is de referentie.
s1503189 2:1dd9e630a7b5 28 AnalogIn Boardpotmeter(A2); // POT1 op het board wordt gebruikt als input van de controlloop.
s1503189 2:1dd9e630a7b5 29 AnalogIn Boardpotmeter2(A1);
s1503189 0:46b6258b2b00 30 DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
s1503189 0:46b6258b2b00 31 FastPWM motor1speed(D6);
s1503189 2:1dd9e630a7b5 32
s1503189 0:46b6258b2b00 33 float potmeter;
s1503189 0:46b6258b2b00 34 float Error;
s1503189 4:4ad3fc99c356 35 float Referentie2;//=0.0;
s1503189 0:46b6258b2b00 36 float Input;
s1503189 0:46b6258b2b00 37 float Output;
s1503189 2:1dd9e630a7b5 38 float POT;
s1503189 3:9f9ef68a25a2 39 float Foutje;
s1503189 3:9f9ef68a25a2 40 float Potmetertje;
s1503189 0:46b6258b2b00 41 double Error_prev = 0;
s1503189 0:46b6258b2b00 42 double Error_int = 0;
s1503189 0:46b6258b2b00 43 double Error_der;
s1503189 1:f63d8a73460c 44 double Ref_der = 0;
s1503189 1:f63d8a73460c 45 double Ref_prev = 0;
s1503189 4:4ad3fc99c356 46 float Amplitude;
s1503189 4:4ad3fc99c356 47 float Baseline;
s1503189 0:46b6258b2b00 48 // Define the HIDScope and Ticker object
s1503189 0:46b6258b2b00 49 HIDScope scope(4);
s1503189 0:46b6258b2b00 50 Ticker Hidscope;
s1503189 0:46b6258b2b00 51 // Lowpassfilter
s1503189 0:46b6258b2b00 52 const double a1_LP = -1.561018075800718, a2_LP = 0.641351538057563;
s1503189 0:46b6258b2b00 53 const double b0_LP = 0.020083365564211, b1_LP = 0.040166731128423, b2_LP = 0.020083365564211;
s1503189 0:46b6258b2b00 54 const double a1_LP2 = -1.561018075800718, a2_LP2 = 0.641351538057563;
s1503189 0:46b6258b2b00 55 const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211;
s1503189 0:46b6258b2b00 56
s1503189 0:46b6258b2b00 57 float time_;
s1503189 0:46b6258b2b00 58 const float time_increment = 0.01;
s1503189 0:46b6258b2b00 59
s1503189 0:46b6258b2b00 60 biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);
s1503189 0:46b6258b2b00 61 biquadFilter Filter2(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);
s1503189 0:46b6258b2b00 62
s1503189 0:46b6258b2b00 63 double Referentieschaling(double A, double B)
s1503189 0:46b6258b2b00 64 {
s1503189 0:46b6258b2b00 65 double Y= 787.3008916207206*pow(A,4) -565.1143141517078*pow(A,3) + 122.8516837382677*pow(A,2) + 0.0556616744031*A + 0.0912411880277;
s1503189 0:46b6258b2b00 66 Referentie2 = Filter1.step(Y);
s1503189 0:46b6258b2b00 67 if (Referentie2<=2.5 or Referentie2>=9.2) {
s1503189 4:4ad3fc99c356 68 Rood = 0;
s1503189 0:46b6258b2b00 69 Referentie2 = B;
s1503189 0:46b6258b2b00 70 }
s1503189 0:46b6258b2b00 71 return Referentie2;
s1503189 0:46b6258b2b00 72 }
s1503189 0:46b6258b2b00 73
s1503189 0:46b6258b2b00 74 double Inputberekening(double B)
s1503189 0:46b6258b2b00 75 {
s1503189 7:9ea55ce667be 76 Input = Baseline+(Amplitude*-cos(1.5*time_));
s1503189 0:46b6258b2b00 77 if (Input>=9) { // De Input moet binnnen een aantal grenzen blijven. Groter dan 7 is mechanisch niet mogelijk.
s1503189 0:46b6258b2b00 78 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.
s1503189 0:46b6258b2b00 79 } else if (Input<=3.0) {
s1503189 0:46b6258b2b00 80 Input = 3.0;
s1503189 0:46b6258b2b00 81 }
s1503189 0:46b6258b2b00 82 return Input;
s1503189 0:46b6258b2b00 83 }
s1503189 0:46b6258b2b00 84
s1503189 0:46b6258b2b00 85 double Errorberekening(double Input,double Ref)
s1503189 0:46b6258b2b00 86 {
s1503189 7:9ea55ce667be 87 Error = Input-Ref;
s1503189 0:46b6258b2b00 88 return Error;
s1503189 0:46b6258b2b00 89 }
s1503189 0:46b6258b2b00 90 double PID_controller(double Error, double KP, double KI, double KD, double Ts, double &Error_int, double &e_prev)
s1503189 0:46b6258b2b00 91 {
s1503189 0:46b6258b2b00 92 Error_der = (Error-Error_prev)/Ts;
s1503189 0:46b6258b2b00 93 Error_prev = Error;
s1503189 0:46b6258b2b00 94 Error_int = Error_int + Ts*Error;
s1503189 0:46b6258b2b00 95
s1503189 0:46b6258b2b00 96 return KP*Error+KI*Error_int+KD*Error_der;
s1503189 0:46b6258b2b00 97 }
s1503189 0:46b6258b2b00 98
s1503189 0:46b6258b2b00 99 // The data read and send function
s1503189 0:46b6258b2b00 100 void scopeSend()
s1503189 0:46b6258b2b00 101 {
s1503189 0:46b6258b2b00 102 scope.set(0,Referentie2); // Kanaal 1 van HIDscope geeft: De positie van de schuifpotmeter in cm.
s1503189 0:46b6258b2b00 103 scope.set(1,Input); // Kanaal 2 van HIDscope geeft: De gewenste positie in cm door de potmeter op het board ingesteld.
s1503189 0:46b6258b2b00 104 scope.set(2,Error); // Kanaal 3 van HIDscope geeft: De waarde van de Error die de P_controller in gaat.
s1503189 0:46b6258b2b00 105 scope.set(3,motor1speed.read());// Kanaal 1 van HIDscope geeft: De snelheid van de motor, is een absolute waarde, richting wordt elders gegeven.
s1503189 0:46b6258b2b00 106 scope.send();
s1503189 0:46b6258b2b00 107
s1503189 0:46b6258b2b00 108 }
s1503189 0:46b6258b2b00 109
s1503189 0:46b6258b2b00 110 void tickerfunctie2() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
s1503189 0:46b6258b2b00 111 {
s1503189 0:46b6258b2b00 112 LoopTimerFlag2 = 1;
s1503189 0:46b6258b2b00 113 }
s1503189 0:46b6258b2b00 114
s1503189 3:9f9ef68a25a2 115 void Motor_controller(float Output) // De P_controller, door de ticker elke honderdste seconde uitgevoerd.
s1503189 0:46b6258b2b00 116 {
s1503189 1:f63d8a73460c 117 if(Ref_der < 0) {
s1503189 1:f63d8a73460c 118 if(Output>=0) {
s1503189 1:f63d8a73460c 119 motor1direction.write(1);
s1503189 7:9ea55ce667be 120 motor1speed = 0.1*fabs(Output);
s1503189 1:f63d8a73460c 121 } else if (Output<0) {
s1503189 1:f63d8a73460c 122 motor1direction.write(0);
s1503189 7:9ea55ce667be 123 motor1speed = 0.25*fabs(Output);
s1503189 0:46b6258b2b00 124 }
s1503189 0:46b6258b2b00 125 }
s1503189 3:9f9ef68a25a2 126 if(Ref_der > 0) {
s1503189 1:f63d8a73460c 127 if(Output>=0) {
s1503189 1:f63d8a73460c 128 motor1direction.write(1);
s1503189 7:9ea55ce667be 129 motor1speed = (fabs(Output));
s1503189 1:f63d8a73460c 130 } else if (Output<0) {
s1503189 1:f63d8a73460c 131 motor1direction.write(0);
s1503189 1:f63d8a73460c 132 motor1speed = 0.01*fabs(Output);
s1503189 0:46b6258b2b00 133 }
s1503189 0:46b6258b2b00 134 }
s1503189 0:46b6258b2b00 135 }
s1503189 3:9f9ef68a25a2 136
s1503189 3:9f9ef68a25a2 137 void Motor_controller2(float Foutje)
s1503189 3:9f9ef68a25a2 138 {
s1503189 3:9f9ef68a25a2 139 if(Foutje>=0.01){
s1503189 7:9ea55ce667be 140 motor1direction.write(0);
s1503189 7:9ea55ce667be 141 motor1speed.write(0.02*Foutje);
s1503189 3:9f9ef68a25a2 142 }
s1503189 3:9f9ef68a25a2 143 else if(Foutje<=-0.01){
s1503189 7:9ea55ce667be 144 motor1direction.write(1);
s1503189 3:9f9ef68a25a2 145 motor1speed.write(-Foutje);
s1503189 3:9f9ef68a25a2 146 }
s1503189 3:9f9ef68a25a2 147 else {
s1503189 3:9f9ef68a25a2 148 motor1speed.write(0);
s1503189 3:9f9ef68a25a2 149 }
s1503189 3:9f9ef68a25a2 150 }
s1503189 3:9f9ef68a25a2 151
s1503189 2:1dd9e630a7b5 152 void Extendfinger()
s1503189 2:1dd9e630a7b5 153 {
s1503189 2:1dd9e630a7b5 154 Error = 0;
s1503189 2:1dd9e630a7b5 155 int J = 0;
s1503189 2:1dd9e630a7b5 156 time_ = 0; // Voordat het hele programma begint, staat de Error op 0, zodat de motor niet spastisch gaat draaien om dit te compenseren.
s1503189 7:9ea55ce667be 157 while(1 && J<=480) {
s1503189 2:1dd9e630a7b5 158 while(LoopTimerFlag2 !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 2:1dd9e630a7b5 159 LoopTimerFlag2 = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
s1503189 2:1dd9e630a7b5 160 J= J+1;
s1503189 2:1dd9e630a7b5 161 //pc.printf(" J is %i \n",J);
s1503189 4:4ad3fc99c356 162 Rood = 1;
s1503189 2:1dd9e630a7b5 163 double Input = Inputberekening(Boardpotmeter.read());
s1503189 2:1dd9e630a7b5 164 time_ += time_increment;
s1503189 2:1dd9e630a7b5 165 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.
s1503189 2:1dd9e630a7b5 166 Ref_der = Referentie2-Ref_prev;
s1503189 2:1dd9e630a7b5 167 Ref_prev = Referentie2;
s1503189 2:1dd9e630a7b5 168 Error = Errorberekening(Input, Referentie2);
s1503189 7:9ea55ce667be 169 Output = PID_controller(Error,0.5,0.1,0.01,0.01, Error_prev, Error_int); //,5,1,0.1,
s1503189 3:9f9ef68a25a2 170 Motor_controller(Output);
s1503189 1:f63d8a73460c 171 }
s1503189 3:9f9ef68a25a2 172 //}
s1503189 2:1dd9e630a7b5 173 motor1speed.write(0);
s1503189 2:1dd9e630a7b5 174 }
s1503189 1:f63d8a73460c 175
s1503189 2:1dd9e630a7b5 176 void tickerfunctie()
s1503189 2:1dd9e630a7b5 177 {
s1503189 2:1dd9e630a7b5 178 LoopTimerFlag = 1;
s1503189 2:1dd9e630a7b5 179 }
s1503189 2:1dd9e630a7b5 180
s1503189 3:9f9ef68a25a2 181 void P_controller(double Max, bool Keuze)
s1503189 2:1dd9e630a7b5 182 {
s1503189 3:9f9ef68a25a2 183 while (Button1pressed.read()==1) {
s1503189 3:9f9ef68a25a2 184 while(LoopTimerFlag2 !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 2:1dd9e630a7b5 185 LoopTimerFlag2 = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
s1503189 2:1dd9e630a7b5 186 if (bool (0)) {
s1503189 2:1dd9e630a7b5 187 POT = Boardpotmeter.read();
s1503189 2:1dd9e630a7b5 188 } else if (bool (1)) {
s1503189 2:1dd9e630a7b5 189 POT = Boardpotmeter2.read();
s1503189 1:f63d8a73460c 190 }
s1503189 7:9ea55ce667be 191 Input = 10*POT; // 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.
s1503189 3:9f9ef68a25a2 192 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.
s1503189 4:4ad3fc99c356 193 Rood = 1;
s1503189 4:4ad3fc99c356 194 Error = Referentie2-Input;
s1503189 3:9f9ef68a25a2 195 Motor_controller2(Error);
s1503189 2:1dd9e630a7b5 196
s1503189 1:f63d8a73460c 197 }
s1503189 4:4ad3fc99c356 198
s1503189 2:1dd9e630a7b5 199 Groen = 0;
s1503189 2:1dd9e630a7b5 200 wait (0.5);
s1503189 2:1dd9e630a7b5 201 }
s1503189 2:1dd9e630a7b5 202
s1503189 2:1dd9e630a7b5 203 void Determinetask()
s1503189 2:1dd9e630a7b5 204 {
s1503189 2:1dd9e630a7b5 205 if (Button1pressed == 0) {
s1503189 2:1dd9e630a7b5 206
s1503189 2:1dd9e630a7b5 207 Groen = 0;
s1503189 2:1dd9e630a7b5 208 N++;
s1503189 1:f63d8a73460c 209 wait (0.5);
s1503189 2:1dd9e630a7b5 210 Groen = 1;
s1503189 2:1dd9e630a7b5 211 }
s1503189 2:1dd9e630a7b5 212 }
s1503189 2:1dd9e630a7b5 213
s1503189 2:1dd9e630a7b5 214 int main()
s1503189 2:1dd9e630a7b5 215 {
s1503189 2:1dd9e630a7b5 216 Rood = 1;
s1503189 2:1dd9e630a7b5 217 Blauw = 1;
s1503189 2:1dd9e630a7b5 218 Groen = 1;
s1503189 2:1dd9e630a7b5 219 int i = 0;
s1503189 2:1dd9e630a7b5 220 Finitestatemachine.attach(tickerfunctie,0.1);
s1503189 3:9f9ef68a25a2 221 Loopticker.attach(tickerfunctie2,0.01);
s1503189 3:9f9ef68a25a2 222 Hidscope.attach(scopeSend,0.01); // Verzenden naar HIDscope
s1503189 2:1dd9e630a7b5 223
s1503189 2:1dd9e630a7b5 224 while (true) {
s1503189 2:1dd9e630a7b5 225 while(LoopTimerFlag !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 2:1dd9e630a7b5 226 LoopTimerFlag = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
s1503189 2:1dd9e630a7b5 227 if (Start==0 && Setting==0 && Dotask==0) { // In eerste instantie gaan de leds even uit
s1503189 2:1dd9e630a7b5 228 Start = 1;
s1503189 2:1dd9e630a7b5 229 Groen = 0;
s1503189 2:1dd9e630a7b5 230 }
s1503189 2:1dd9e630a7b5 231 if (Start==1 && Setting==0 && Dotask==0) { // State 2 heeft Geel als kleur
s1503189 2:1dd9e630a7b5 232 Groen = 0;
s1503189 2:1dd9e630a7b5 233 Rood = 0;
s1503189 2:1dd9e630a7b5 234 if (Button1pressed.read()==1 && Button2pressed.read()==0) {
s1503189 6:e206abd0b2ca 235 if (Amplitude != 0){
s1503189 6:e206abd0b2ca 236 Setting = 2;
s1503189 6:e206abd0b2ca 237 Start = false;
s1503189 6:e206abd0b2ca 238 Groen = 1;
s1503189 6:e206abd0b2ca 239 Rood = 1;
s1503189 6:e206abd0b2ca 240 N = 0;
s1503189 6:e206abd0b2ca 241 wait(0.5);}
s1503189 6:e206abd0b2ca 242 else {
s1503189 6:e206abd0b2ca 243 Setting = 1;
s1503189 2:1dd9e630a7b5 244 Start = false;
s1503189 2:1dd9e630a7b5 245 Groen = 1;
s1503189 2:1dd9e630a7b5 246 Rood = 1;
s1503189 6:e206abd0b2ca 247 wait(0.5);}
s1503189 1:f63d8a73460c 248
s1503189 2:1dd9e630a7b5 249 }
s1503189 2:1dd9e630a7b5 250 if (Button1pressed.read()==0 && Button2pressed.read()==1 && N>0) {
s1503189 2:1dd9e630a7b5 251 Groen = 1;
s1503189 2:1dd9e630a7b5 252 Rood = 1;
s1503189 2:1dd9e630a7b5 253 Dotask = 1;
s1503189 2:1dd9e630a7b5 254 Start=0;
s1503189 2:1dd9e630a7b5 255 wait(0.5);
s1503189 2:1dd9e630a7b5 256 }
s1503189 2:1dd9e630a7b5 257 }
s1503189 3:9f9ef68a25a2 258 if (Start==0 && Setting==1 && Dotask==0) { // Leds zijn uit, elke bepaling gaat het groene lampje even branden.
s1503189 3:9f9ef68a25a2 259 P_controller(0,0);
s1503189 3:9f9ef68a25a2 260 double Maximum = 10*POT;
s1503189 2:1dd9e630a7b5 261 Groen = 1;
s1503189 3:9f9ef68a25a2 262 P_controller(0,1);
s1503189 3:9f9ef68a25a2 263 double Minimum = 10*POT;
s1503189 2:1dd9e630a7b5 264 Groen = 1;
s1503189 4:4ad3fc99c356 265 Amplitude = (Minimum+Maximum)/2;
s1503189 4:4ad3fc99c356 266 Baseline = (Maximum-Minimum)/2;
s1503189 4:4ad3fc99c356 267 Rood = 0;
s1503189 4:4ad3fc99c356 268 Blauw = 0;
s1503189 4:4ad3fc99c356 269 wait (2);
s1503189 4:4ad3fc99c356 270 Rood = 1;
s1503189 4:4ad3fc99c356 271 Blauw = 1;
s1503189 2:1dd9e630a7b5 272 Setting = 2;
s1503189 2:1dd9e630a7b5 273 }
s1503189 1:f63d8a73460c 274
s1503189 2:1dd9e630a7b5 275 if (Start==0 && Setting==2 && Dotask==0) { // Leds zijn uit, elke count gaat het groene lampje even branden.
s1503189 2:1dd9e630a7b5 276 Determinetask(); // Aantal gewenste herhalingen van Dotask instellen
s1503189 2:1dd9e630a7b5 277 if (Button1pressed.read()== 1 && Button2pressed.read()== 0) {
s1503189 2:1dd9e630a7b5 278 Setting = 0;
s1503189 2:1dd9e630a7b5 279 Dotask = 1;
s1503189 2:1dd9e630a7b5 280 }
s1503189 2:1dd9e630a7b5 281 }
s1503189 1:f63d8a73460c 282
s1503189 2:1dd9e630a7b5 283 if (Start==0 && Setting==0 && Dotask==1) {
s1503189 2:1dd9e630a7b5 284 while(i < N) {
s1503189 2:1dd9e630a7b5 285 Extendfinger();
s1503189 2:1dd9e630a7b5 286 i++;
s1503189 2:1dd9e630a7b5 287 }
s1503189 2:1dd9e630a7b5 288 if (i==N) {
s1503189 2:1dd9e630a7b5 289 Dotask = 0;
s1503189 1:f63d8a73460c 290 Start = 1;
s1503189 2:1dd9e630a7b5 291 i=0;
s1503189 1:f63d8a73460c 292 }
s1503189 1:f63d8a73460c 293
s1503189 1:f63d8a73460c 294 }
s1503189 1:f63d8a73460c 295 }
s1503189 2:1dd9e630a7b5 296 }