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@0:46b6258b2b00, 2016-05-30 (annotated)

Committer:
s1503189
Date:
Mon May 30 08:50:13 2016 +0000
Revision:
0:46b6258b2b00
Child:
1:f63d8a73460c
Final principle werkend, slechte Controllerwaarden

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