BMT K9 groep 9 2015-2016 / Mbed 2 deprecated frdm_PIDControllertest

Goed werkende PID, vanaf nu dit script gebruiken.

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@2:d04df4be6cf7, 2015-10-19 (annotated)

Committer:
roosalyn
Date:
Mon Oct 19 09:18:40 2015 +0000
Revision:
2:d04df4be6cf7
Parent:
1:d9cfdc904b10 
teller met knoppen eruitgehaald, krijgen nog steeds geen goed EMG-signaal

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roosalyn 0:50d492ea0fd0 1 #include "mbed.h"
roosalyn 0:50d492ea0fd0 2 #include "encoder.h"
roosalyn 0:50d492ea0fd0 3 #include "MODSERIAL.h"
roosalyn 0:50d492ea0fd0 4 #include "math.h"
roosalyn 0:50d492ea0fd0 5 #include "HIDScope.h"
roosalyn 1:d9cfdc904b10 6 #include "biquadFilter.h"
roosalyn 1:d9cfdc904b10 7 #include "iostream"
roosalyn 0:50d492ea0fd0 8 #define M_PI 3.14159265358979323846
roosalyn 0:50d492ea0fd0 9
roosalyn 0:50d492ea0fd0 10 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
roosalyn 0:50d492ea0fd0 11 PwmOut motor2speed(D5);
roosalyn 0:50d492ea0fd0 12 DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
roosalyn 0:50d492ea0fd0 13 PwmOut motor1speed(D6);
roosalyn 0:50d492ea0fd0 14 AnalogIn potmeter1(A3); //blauw draaiknopje, nr1
roosalyn 0:50d492ea0fd0 15 AnalogIn potmeter2(A2); //nr 2
roosalyn 0:50d492ea0fd0 16 Encoder encoder2(D13,D12,true);
roosalyn 0:50d492ea0fd0 17 Encoder encoder1(D11,D10,true);
roosalyn 0:50d492ea0fd0 18 MODSERIAL pc(USBTX,USBRX);
roosalyn 0:50d492ea0fd0 19
roosalyn 0:50d492ea0fd0 20 Ticker myControllerTicker1, myControllerTicker2, countStepTickerX, countStepTickerY, buttonStateCheckX, buttonStateCheckY;
roosalyn 0:50d492ea0fd0 21 bool motor1_PID_Controller_go = false;
roosalyn 0:50d492ea0fd0 22 bool countStepX_go = false;
roosalyn 0:50d492ea0fd0 23 bool countStepY_go = false;
roosalyn 0:50d492ea0fd0 24 bool checkButtonStateX_go = false;
roosalyn 0:50d492ea0fd0 25 bool checkButtonStateY_go = false;
roosalyn 1:d9cfdc904b10 26 bool Filter_go = false;
roosalyn 0:50d492ea0fd0 27
roosalyn 2:d04df4be6cf7 28 //DigitalIn telknopX(SW2);
roosalyn 2:d04df4be6cf7 29 //DigitalIn telknopY(SW3);
roosalyn 0:50d492ea0fd0 30 DigitalOut ledX(D2);
roosalyn 0:50d492ea0fd0 31 DigitalOut ledY(D3);
roosalyn 0:50d492ea0fd0 32
roosalyn 1:d9cfdc904b10 33 double m1_Kp = 0.0003, m1_Ki = 0.0, m1_Kd = 0.0;
roosalyn 0:50d492ea0fd0 34 const double m1_Ts = 0.01;
roosalyn 0:50d492ea0fd0 35 double m1_err_int = 0, m1_prev_err = 0;
roosalyn 0:50d492ea0fd0 36 double m1_u_prev = 0;
roosalyn 0:50d492ea0fd0 37
roosalyn 0:50d492ea0fd0 38 /*
roosalyn 0:50d492ea0fd0 39 Gain = 0.002356
roosalyn 0:50d492ea0fd0 40 B = [ 1.00000000000, 2.00000000000, 1.00000000000]
roosalyn 0:50d492ea0fd0 41 A = [ 1.00000000000, -1.85808607200, 0.86750940236]
roosalyn 0:50d492ea0fd0 42 */
roosalyn 0:50d492ea0fd0 43
roosalyn 0:50d492ea0fd0 44 const double m1_f_a1 = -1.94042975320, m1_f_a2 = 0.94215346226, m1_f_b0 = 1.0*0.000431, m1_f_b1 = 2.0*0.000431, m1_f_b2 = 1.0*0.000431;
roosalyn 0:50d492ea0fd0 45 //dit zijn de constanten die bij de filter horen (de a0 t/m b2) a0 is altijd 1
roosalyn 0:50d492ea0fd0 46 double m1_f_v1 = 0, m1_f_v2 = 0; //de variabelen van de filter
roosalyn 0:50d492ea0fd0 47
roosalyn 0:50d492ea0fd0 48 int reference1;
roosalyn 0:50d492ea0fd0 49 int reference2;
roosalyn 0:50d492ea0fd0 50 int difference1;
roosalyn 0:50d492ea0fd0 51 int difference2;
roosalyn 0:50d492ea0fd0 52 int buttonStateX = 1;
roosalyn 0:50d492ea0fd0 53 int buttonStateY = 1;
roosalyn 0:50d492ea0fd0 54 int motorDirectionX = 1;
roosalyn 0:50d492ea0fd0 55 int motorDirectionY = 1;
roosalyn 0:50d492ea0fd0 56 int n;
roosalyn 0:50d492ea0fd0 57 int countsX=0;
roosalyn 0:50d492ea0fd0 58 int countsY=0;
roosalyn 0:50d492ea0fd0 59 int countStateX = 1;
roosalyn 0:50d492ea0fd0 60 int countStateY = 1;
roosalyn 0:50d492ea0fd0 61
roosalyn 0:50d492ea0fd0 62 float x = 4.0; // voorbeeld, moet eigenlijk bepaald worden door EMG signaal
roosalyn 0:50d492ea0fd0 63 float y = 4.0; // is het 4de vakje van rechtsonderin de hoek geteld
roosalyn 0:50d492ea0fd0 64 float xdist_from_board = 4.0; //er moet 2 van afgetrokken worden. Als de as 6cm van het bord staat moet er 4 worden ingevuld. Telt namelijk
roosalyn 0:50d492ea0fd0 65 float ydist_from_board = 4.0; //vanaf het midden van een vakje.
roosalyn 0:50d492ea0fd0 66 double constante1;
roosalyn 0:50d492ea0fd0 67 double constante2;
roosalyn 0:50d492ea0fd0 68 double constante3;
roosalyn 0:50d492ea0fd0 69
roosalyn 1:d9cfdc904b10 70 // --- EMG declaraties --- //
roosalyn 1:d9cfdc904b10 71 AnalogIn emg1(A0);
roosalyn 1:d9cfdc904b10 72 AnalogIn emg2(A1);
roosalyn 1:d9cfdc904b10 73 DigitalIn button(SW2);
roosalyn 1:d9cfdc904b10 74 DigitalOut led_green(LED_GREEN);
roosalyn 1:d9cfdc904b10 75 DigitalOut led_red(LED_RED);
roosalyn 1:d9cfdc904b10 76 DigitalOut led_blue(LED_BLUE);
roosalyn 1:d9cfdc904b10 77
roosalyn 1:d9cfdc904b10 78 double filtered_notch1;
roosalyn 1:d9cfdc904b10 79 double filtered_notch2;
roosalyn 1:d9cfdc904b10 80 double signal1;
roosalyn 1:d9cfdc904b10 81 double signal2;
roosalyn 1:d9cfdc904b10 82 bool start = false;
roosalyn 1:d9cfdc904b10 83
roosalyn 1:d9cfdc904b10 84 HIDScope scope(2); //4 channels
roosalyn 1:d9cfdc904b10 85 Ticker scopeTimer;
roosalyn 1:d9cfdc904b10 86 Ticker Amplitude_height;
roosalyn 1:d9cfdc904b10 87 int counter = 0;
roosalyn 1:d9cfdc904b10 88 using namespace std;
roosalyn 1:d9cfdc904b10 89
roosalyn 1:d9cfdc904b10 90 double high[2] = {};
roosalyn 1:d9cfdc904b10 91 double low[2] = {};
roosalyn 1:d9cfdc904b10 92 double notch[2] = {};
roosalyn 1:d9cfdc904b10 93 double max1 =0.01;
roosalyn 1:d9cfdc904b10 94 double max2=0.01;
roosalyn 1:d9cfdc904b10 95
roosalyn 1:d9cfdc904b10 96 // ----- Filters ----- //
roosalyn 1:d9cfdc904b10 97
roosalyn 1:d9cfdc904b10 98 //tweede highpass filter, poging met latere cutoff frequency: werkt niet. Nu alleen nog bewegingsartefacten te zien, geen kracht!
roosalyn 1:d9cfdc904b10 99 //9 oktober: nu werkt het wel. Een beetje beweging te zien, maar aanspannen geeft hogere amplitude
roosalyn 1:d9cfdc904b10 100 biquadFilter High_1_Filter1( -0.69832496500, 0.00000000000, 0.849163, -0.849163, 0.00000000000);
roosalyn 1:d9cfdc904b10 101 biquadFilter High_2_Filter1( -1.87472749640, 0.90756583051, 0.945573, 1.891146, 0.945573);
roosalyn 1:d9cfdc904b10 102 biquadFilter High_1_Filter2( -0.69832496500, 0.00000000000, 0.849163, -0.849163, 0.00000000000);
roosalyn 1:d9cfdc904b10 103 biquadFilter High_2_Filter2( -1.87472749640, 0.90756583051, 0.945573, 1.891146, 0.945573);
roosalyn 1:d9cfdc904b10 104 //Notch filter voor 50 Hz. Butter
roosalyn 1:d9cfdc904b10 105 biquadFilter Notch_Filter1(-1.65238019196, 0.73741535193, 0.86870768, -1.652380199639, 0.86870768);
roosalyn 1:d9cfdc904b10 106 biquadFilter Notch_Filter2(-1.65238019196, 0.73741535193, 0.86870768, -1.652380199639, 0.86870768);
roosalyn 1:d9cfdc904b10 107 //Lowpass cheby II. bestaat uit twee biquads. De B coefficienten moeten maal de gain gedaan worden.
roosalyn 1:d9cfdc904b10 108 biquadFilter Low_1_Filter1( -0.96240334975 , 0.00000000000 , 0.018798 , 0.018798 , 0.000000 ); // a1, a2, b0, b1, b2.
roosalyn 1:d9cfdc904b10 109 biquadFilter Low_2_Filter1( -1.96214371581 , 0.96354072388 , 0.030252 , -0.0591069922, 0.030252 );
roosalyn 1:d9cfdc904b10 110 biquadFilter Low_1_Filter2( -0.96240334975 , 0.00000000000 , 0.018798 , 0.018798 , 0.000000 ); // a1, a2, b0, b1, b2.
roosalyn 1:d9cfdc904b10 111 biquadFilter Low_2_Filter2( -1.96214371581 , 0.96354072388 , 0.030252 , -0.0591069922, 0.030252 );
roosalyn 1:d9cfdc904b10 112
roosalyn 1:d9cfdc904b10 113 double* LowFilter(double n3_emg1, double n3_emg2)
roosalyn 1:d9cfdc904b10 114 {
roosalyn 1:d9cfdc904b10 115 double l1_emg1 = Low_1_Filter1.step(n3_emg1);
roosalyn 1:d9cfdc904b10 116 double l1_emg2 = Low_1_Filter2.step(n3_emg2);
roosalyn 1:d9cfdc904b10 117 low[0] = Low_2_Filter1.step(l1_emg1);
roosalyn 1:d9cfdc904b10 118 low[1] = Low_2_Filter2.step(l1_emg2);
roosalyn 1:d9cfdc904b10 119 return low;
roosalyn 1:d9cfdc904b10 120 }
roosalyn 1:d9cfdc904b10 121
roosalyn 1:d9cfdc904b10 122 double* HighFilter(void)
roosalyn 1:d9cfdc904b10 123 {
roosalyn 1:d9cfdc904b10 124 double reademg1 = emg1.read();
roosalyn 1:d9cfdc904b10 125 double reademg2 = emg2.read();
roosalyn 2:d04df4be6cf7 126 scope.set(0, reademg1);
roosalyn 2:d04df4be6cf7 127 scope.set(1, reademg2);
roosalyn 2:d04df4be6cf7 128 scope.send();
roosalyn 1:d9cfdc904b10 129 double h1_emg1 = High_1_Filter1.step(reademg1);
roosalyn 1:d9cfdc904b10 130 high[0] = High_2_Filter1.step(h1_emg1);
roosalyn 1:d9cfdc904b10 131 double h1_emg2 = High_1_Filter2.step(reademg2);
roosalyn 1:d9cfdc904b10 132 high[1] = High_2_Filter2.step(h1_emg2);
roosalyn 1:d9cfdc904b10 133 return high;
roosalyn 1:d9cfdc904b10 134 }
roosalyn 1:d9cfdc904b10 135
roosalyn 1:d9cfdc904b10 136 double* NotchFilter(double n_emg1, double n_emg2 )
roosalyn 1:d9cfdc904b10 137 {
roosalyn 1:d9cfdc904b10 138 notch[0] = Notch_Filter1.step(n_emg1);
roosalyn 1:d9cfdc904b10 139 notch[1] = Notch_Filter2.step(n_emg2);
roosalyn 1:d9cfdc904b10 140 return notch;
roosalyn 1:d9cfdc904b10 141 }
roosalyn 1:d9cfdc904b10 142
roosalyn 1:d9cfdc904b10 143 void Filter(void)
roosalyn 1:d9cfdc904b10 144 {
roosalyn 1:d9cfdc904b10 145 HighFilter();
roosalyn 1:d9cfdc904b10 146 double h2_emg1_abs = fabs(high[0]);
roosalyn 1:d9cfdc904b10 147 double h2_emg2_abs = fabs(high[1]);
roosalyn 1:d9cfdc904b10 148 LowFilter(h2_emg1_abs, h2_emg2_abs);
roosalyn 1:d9cfdc904b10 149 double filtered_emg1 = low[0];
roosalyn 1:d9cfdc904b10 150 double filtered_emg2 = low[1];
roosalyn 1:d9cfdc904b10 151 NotchFilter(filtered_emg1, filtered_emg2);
roosalyn 1:d9cfdc904b10 152 filtered_notch1 = notch[0];
roosalyn 1:d9cfdc904b10 153 filtered_notch2 = notch[1];
roosalyn 1:d9cfdc904b10 154 signal1 = filtered_notch1/max1;
roosalyn 1:d9cfdc904b10 155 signal2 = filtered_notch2/max2;
roosalyn 1:d9cfdc904b10 156 //scope.set(2, filtered_notch1);
roosalyn 1:d9cfdc904b10 157 //scope.set(3, filtered_notch2);
roosalyn 2:d04df4be6cf7 158 //scope.set(0, reademg1);
roosalyn 2:d04df4be6cf7 159 //scope.set(1, reademg2);
roosalyn 1:d9cfdc904b10 160 scope.send();
roosalyn 1:d9cfdc904b10 161 }
roosalyn 1:d9cfdc904b10 162
roosalyn 1:d9cfdc904b10 163
roosalyn 1:d9cfdc904b10 164
roosalyn 1:d9cfdc904b10 165 // ------ OVERIGE FUNCTIES ------ //
roosalyn 1:d9cfdc904b10 166
roosalyn 0:50d492ea0fd0 167 bool buttonReleasedX(DigitalIn& button)
roosalyn 0:50d492ea0fd0 168 {
roosalyn 0:50d492ea0fd0 169 if (button.read() == 1 && buttonStateX == 0) {
roosalyn 0:50d492ea0fd0 170 buttonStateX = 1;
roosalyn 0:50d492ea0fd0 171 return true;
roosalyn 0:50d492ea0fd0 172 }
roosalyn 0:50d492ea0fd0 173 buttonStateX = button.read();
roosalyn 0:50d492ea0fd0 174 return false;
roosalyn 0:50d492ea0fd0 175 }
roosalyn 0:50d492ea0fd0 176
roosalyn 0:50d492ea0fd0 177 bool buttonReleasedY(DigitalIn& button)
roosalyn 0:50d492ea0fd0 178 {
roosalyn 0:50d492ea0fd0 179 if (button.read() == 1 && buttonStateY == 0) {
roosalyn 0:50d492ea0fd0 180 buttonStateY = 1;
roosalyn 0:50d492ea0fd0 181 return true;
roosalyn 0:50d492ea0fd0 182 }
roosalyn 0:50d492ea0fd0 183 buttonStateY = button.read();
roosalyn 0:50d492ea0fd0 184 return false;
roosalyn 0:50d492ea0fd0 185 }
roosalyn 0:50d492ea0fd0 186
roosalyn 0:50d492ea0fd0 187 bool buttonHold(DigitalIn& button)
roosalyn 0:50d492ea0fd0 188 {
roosalyn 0:50d492ea0fd0 189 return (button.read() == 0);
roosalyn 0:50d492ea0fd0 190 }
roosalyn 0:50d492ea0fd0 191
roosalyn 0:50d492ea0fd0 192 float bereken_hoek1(float x, float y)
roosalyn 0:50d492ea0fd0 193 {
roosalyn 0:50d492ea0fd0 194 float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
roosalyn 1:d9cfdc904b10 195 float alpha = acos((langste_zijde/2)/35); //4200=1 rondje
roosalyn 0:50d492ea0fd0 196 return (atan((4*y)/(4*x)) + alpha)*4200/(2*M_PI); // omrekenen van radialen naar counts = *4200/(2*M_PI), dit moet nog omgerekend worden met de gear ratio van de extra tandwielen.
roosalyn 0:50d492ea0fd0 197 }
roosalyn 0:50d492ea0fd0 198
roosalyn 0:50d492ea0fd0 199 float bereken_hoek2(float x, float y)
roosalyn 0:50d492ea0fd0 200 {
roosalyn 0:50d492ea0fd0 201 float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
roosalyn 1:d9cfdc904b10 202 float alpha = acos((langste_zijde/2)/35);
roosalyn 0:50d492ea0fd0 203 float beta = 0.5*M_PI - alpha;
roosalyn 0:50d492ea0fd0 204 return (2*beta)*4200/(2*M_PI); // x wordt hoek voor eerste arm t.o.v. x-as (CW), y hoek voor tweede arm t.o.v. eerste arm (CW)
roosalyn 0:50d492ea0fd0 205 }
roosalyn 0:50d492ea0fd0 206
roosalyn 1:d9cfdc904b10 207 void Amplitude() //true is aangespanen, false is niet aangespannen. met 2 thresholds. Dit moet nog in procent worden omgezet!
roosalyn 1:d9cfdc904b10 208 {
roosalyn 2:d04df4be6cf7 209 if(start == false && signal1< 0.6) { //moeten waarden 70 en 20 worden als drempelwaarden
roosalyn 1:d9cfdc904b10 210 start = true;
roosalyn 1:d9cfdc904b10 211 //printf("start 0 \n\r");
roosalyn 1:d9cfdc904b10 212 }
roosalyn 2:d04df4be6cf7 213 if (start == true && signal1<0.6) {
roosalyn 1:d9cfdc904b10 214 start = true;
roosalyn 1:d9cfdc904b10 215 //printf("start 1\n\r");
roosalyn 1:d9cfdc904b10 216 }
roosalyn 2:d04df4be6cf7 217 if (start == true && signal1< 0.2) {
roosalyn 1:d9cfdc904b10 218 start = false;
roosalyn 1:d9cfdc904b10 219 //printf("start 1\n\r");
roosalyn 1:d9cfdc904b10 220 }
roosalyn 2:d04df4be6cf7 221 if (start == false && signal1 <0.2) {
roosalyn 1:d9cfdc904b10 222 start = false;
roosalyn 1:d9cfdc904b10 223 //printf("start 0\n\r");
roosalyn 1:d9cfdc904b10 224 }
roosalyn 2:d04df4be6cf7 225 if (start == true && signal1 <0.6 && signal1>0.2) {
roosalyn 1:d9cfdc904b10 226 start = true;
roosalyn 1:d9cfdc904b10 227 //printf("start 1\n\r");
roosalyn 1:d9cfdc904b10 228 }
roosalyn 2:d04df4be6cf7 229 if (start == false && signal1 <0.6 && signal1>0.2) {
roosalyn 1:d9cfdc904b10 230 start = false;
roosalyn 1:d9cfdc904b10 231 //printf("start 0\n\r");
roosalyn 1:d9cfdc904b10 232 }
roosalyn 1:d9cfdc904b10 233 }
roosalyn 1:d9cfdc904b10 234
roosalyn 1:d9cfdc904b10 235 // ----- KALIBRATIE ------ //
roosalyn 1:d9cfdc904b10 236
roosalyn 1:d9cfdc904b10 237 void MaxValue()
roosalyn 1:d9cfdc904b10 238 {
roosalyn 1:d9cfdc904b10 239 int length = 500;
roosalyn 1:d9cfdc904b10 240 double meetwaarden1 [500] = {}; //lege array maken voor emg1
roosalyn 1:d9cfdc904b10 241 double meetwaarden2 [500] = {}; //lege array maken voor emg2
roosalyn 1:d9cfdc904b10 242 for(int i = 0; i<500; i++) { //array vullen met meetwaarden van emg1
roosalyn 1:d9cfdc904b10 243 meetwaarden1[i] = filtered_notch1;
roosalyn 1:d9cfdc904b10 244 wait(0.005); // zonder wait zit er maar een dezelfde waarde in de array! eventueel een ticker er van maken
roosalyn 1:d9cfdc904b10 245 }
roosalyn 1:d9cfdc904b10 246 for(int i = 0; i<500; i++) { //array vullen met meetwaarden van emg2
roosalyn 1:d9cfdc904b10 247 meetwaarden2[i] = filtered_notch2;
roosalyn 1:d9cfdc904b10 248 wait(0.005);
roosalyn 1:d9cfdc904b10 249 }
roosalyn 1:d9cfdc904b10 250 //printarray(meetwaarden, 500); //was als controle
roosalyn 1:d9cfdc904b10 251 for(int i = 0; i<length; i++) { //maximale waarde van de array berekenen
roosalyn 1:d9cfdc904b10 252 if(meetwaarden1[i] > max1) {
roosalyn 1:d9cfdc904b10 253 max1= meetwaarden1[i];
roosalyn 1:d9cfdc904b10 254 }
roosalyn 1:d9cfdc904b10 255 }
roosalyn 1:d9cfdc904b10 256 for(int i = 0; i<length; i++) { //maximale waarde van de array berekenen
roosalyn 1:d9cfdc904b10 257 if(meetwaarden2[i] > max2) {
roosalyn 1:d9cfdc904b10 258 max2 = meetwaarden2[i];
roosalyn 1:d9cfdc904b10 259 }
roosalyn 1:d9cfdc904b10 260 }
roosalyn 1:d9cfdc904b10 261 printf("max value of emg1 is %f, and of emg 2 is %f \n\r", max1, max2);
roosalyn 1:d9cfdc904b10 262 }
roosalyn 1:d9cfdc904b10 263
roosalyn 1:d9cfdc904b10 264 void Calibration()
roosalyn 1:d9cfdc904b10 265 {
roosalyn 1:d9cfdc904b10 266 switch(button.read()) {
roosalyn 1:d9cfdc904b10 267 case 0: //knop wel indrukken
roosalyn 1:d9cfdc904b10 268 switch(counter) {
roosalyn 1:d9cfdc904b10 269 case 0: //eerste keer knop indrukken
roosalyn 1:d9cfdc904b10 270 led_red.write(0); //rode led aan
roosalyn 1:d9cfdc904b10 271 MaxValue();
roosalyn 1:d9cfdc904b10 272 led_green.write(0); //groene led aan + rode = geel
roosalyn 1:d9cfdc904b10 273 wait(1); //even wachten, zodat er tijd is om de knop weer los te laten
roosalyn 1:d9cfdc904b10 274 counter++;
roosalyn 1:d9cfdc904b10 275 break;
roosalyn 1:d9cfdc904b10 276 case 1: //tweede keer knop indrukken
roosalyn 1:d9cfdc904b10 277 led_red.write(1); //uit
roosalyn 1:d9cfdc904b10 278 led_green.write(1); //uit
roosalyn 1:d9cfdc904b10 279 led_blue.write(0); //blauw ledje aan
roosalyn 1:d9cfdc904b10 280
roosalyn 1:d9cfdc904b10 281 // move arm to 0 position: motoren aansturen.
roosalyn 1:d9cfdc904b10 282 //emg_signal1 = filtered_notch/max*100 //schalen
roosalyn 1:d9cfdc904b10 283 wait(1);
roosalyn 1:d9cfdc904b10 284 counter++;
roosalyn 1:d9cfdc904b10 285 break;
roosalyn 1:d9cfdc904b10 286 case 2:
roosalyn 1:d9cfdc904b10 287 //encoder....setPosition(0);
roosalyn 1:d9cfdc904b10 288 led_blue.write(1); //uit
roosalyn 1:d9cfdc904b10 289 led_green.write(0); // ledje groen aan
roosalyn 1:d9cfdc904b10 290 wait(1);
roosalyn 1:d9cfdc904b10 291 counter++;
roosalyn 1:d9cfdc904b10 292 break;
roosalyn 1:d9cfdc904b10 293 }
roosalyn 1:d9cfdc904b10 294 break;
roosalyn 1:d9cfdc904b10 295 }
roosalyn 1:d9cfdc904b10 296 }
roosalyn 1:d9cfdc904b10 297
roosalyn 1:d9cfdc904b10 298 //voor de motoren: als counter =0: motorspeed =0
roosalyn 1:d9cfdc904b10 299 //als counter = 1: niet in vakjes rekenen
roosalyn 1:d9cfdc904b10 300 // als counter >1: wel in vakjes rekenen
roosalyn 1:d9cfdc904b10 301
roosalyn 1:d9cfdc904b10 302
roosalyn 0:50d492ea0fd0 303 //-------------------------- MOTORCONTROLLERS --------------------------//
roosalyn 0:50d492ea0fd0 304
roosalyn 0:50d492ea0fd0 305 void constanteBepalen()
roosalyn 0:50d492ea0fd0 306 {
roosalyn 0:50d492ea0fd0 307 constante1 = potmeter1.read();
roosalyn 0:50d492ea0fd0 308 constante2 = potmeter2.read();
roosalyn 0:50d492ea0fd0 309 m1_Kd = constante2*0.001; //loopt nu van 0 tot 1
roosalyn 0:50d492ea0fd0 310 m1_Ki = constante1*0.001;
roosalyn 0:50d492ea0fd0 311 }
roosalyn 0:50d492ea0fd0 312
roosalyn 0:50d492ea0fd0 313 double biquad(double u, double &v1, double &v2, const double a1, const double a2,
roosalyn 0:50d492ea0fd0 314 const double b0, const double b1, const double b2){
roosalyn 0:50d492ea0fd0 315 double v = u-a1*v1-a2*v2; //formule uit slides van biquad filter
roosalyn 0:50d492ea0fd0 316 double y = b0*v+b1*v1+b2*v2; //zie ook slides
roosalyn 0:50d492ea0fd0 317 v2 = v1; v1 =v;
roosalyn 0:50d492ea0fd0 318 return y;
roosalyn 0:50d492ea0fd0 319 }
roosalyn 0:50d492ea0fd0 320
roosalyn 0:50d492ea0fd0 321 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,
roosalyn 0:50d492ea0fd0 322 double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,
roosalyn 0:50d492ea0fd0 323 const double f_a2, const double f_b0, const double f_b1, const double f_b2){ //In de slides staat f_b3, maar volgens mij moet het f_b2 zijn
roosalyn 0:50d492ea0fd0 324 //Bereken eerst afgeleide van de error
roosalyn 0:50d492ea0fd0 325 double e_der = (e-e_prev)/Ts;
roosalyn 0:50d492ea0fd0 326 double e_der2 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); //de y die de biquad returnt is de gefilterde e_der
roosalyn 0:50d492ea0fd0 327 e_prev = e;
roosalyn 0:50d492ea0fd0 328 //Bereken nu de geintegreerde versie van de error
roosalyn 0:50d492ea0fd0 329 e_int = e_int + Ts*e;
roosalyn 0:50d492ea0fd0 330 //PID
roosalyn 0:50d492ea0fd0 331
roosalyn 0:50d492ea0fd0 332 return Kp*e+Ki*e_int+Kd*e_der2; //formule uit slides
roosalyn 0:50d492ea0fd0 333 }
roosalyn 0:50d492ea0fd0 334
roosalyn 1:d9cfdc904b10 335 // ---- PID Controller ----- //
roosalyn 0:50d492ea0fd0 336
roosalyn 0:50d492ea0fd0 337 void motor1_PID_Controller()
roosalyn 0:50d492ea0fd0 338 {
roosalyn 0:50d492ea0fd0 339 //reference1 = potmeter1.read()*4200; //draaiknop uitlezen, tussen 0 en 1
roosalyn 0:50d492ea0fd0 340 float hoek1 = bereken_hoek1(countsX,countsY);
roosalyn 0:50d492ea0fd0 341 reference1=hoek1;
roosalyn 0:50d492ea0fd0 342 double position =(encoder1.getPosition()); // waarde tussen 0 en 4200
roosalyn 0:50d492ea0fd0 343
roosalyn 0:50d492ea0fd0 344 scope.set(0,reference1);
roosalyn 0:50d492ea0fd0 345 scope.set(1,position);
roosalyn 0:50d492ea0fd0 346 scope.send();
roosalyn 0:50d492ea0fd0 347
roosalyn 0:50d492ea0fd0 348 difference1 = reference1 - position;
roosalyn 0:50d492ea0fd0 349 constanteBepalen();
roosalyn 0:50d492ea0fd0 350 double u = PID( reference1 - position, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int,m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
roosalyn 0:50d492ea0fd0 351 if (u < 0) {
roosalyn 0:50d492ea0fd0 352 motor1direction = 1; //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
roosalyn 0:50d492ea0fd0 353 } else if (u>=0) {
roosalyn 0:50d492ea0fd0 354 motor1direction = 0;
roosalyn 0:50d492ea0fd0 355 }
roosalyn 1:d9cfdc904b10 356 if(counter != 0) {
roosalyn 1:d9cfdc904b10 357 motor1speed = fabs(u);
roosalyn 1:d9cfdc904b10 358 }
roosalyn 0:50d492ea0fd0 359 pc.printf("u = %.2f \r\n", u);
roosalyn 0:50d492ea0fd0 360
roosalyn 0:50d492ea0fd0 361 }
roosalyn 0:50d492ea0fd0 362
roosalyn 0:50d492ea0fd0 363 //-------------------------- POSITIEBEPALING --------------------------//
roosalyn 2:d04df4be6cf7 364 /*
roosalyn 0:50d492ea0fd0 365 void checkButtonStateX()
roosalyn 0:50d492ea0fd0 366 {
roosalyn 0:50d492ea0fd0 367 if (buttonReleasedX(telknopX)) {
roosalyn 0:50d492ea0fd0 368 countStateX = -countStateX;
roosalyn 0:50d492ea0fd0 369 }
roosalyn 0:50d492ea0fd0 370 }
roosalyn 0:50d492ea0fd0 371
roosalyn 0:50d492ea0fd0 372 void checkButtonStateY()
roosalyn 0:50d492ea0fd0 373 {
roosalyn 0:50d492ea0fd0 374 if (buttonReleasedY(telknopY)) {
roosalyn 0:50d492ea0fd0 375 countStateY = -countStateY;
roosalyn 0:50d492ea0fd0 376 }
roosalyn 0:50d492ea0fd0 377 }
roosalyn 2:d04df4be6cf7 378 */
roosalyn 0:50d492ea0fd0 379 void countStepX()
roosalyn 0:50d492ea0fd0 380 {
roosalyn 1:d9cfdc904b10 381 if (start == true) {
roosalyn 0:50d492ea0fd0 382 countsX+= countStateX; //hier moet de richting dus veranderen
roosalyn 0:50d492ea0fd0 383 if(countsX >= 10) {
roosalyn 0:50d492ea0fd0 384 countsX = 10;
roosalyn 0:50d492ea0fd0 385 } else if(countsX<=0) {
roosalyn 0:50d492ea0fd0 386 countsX = 0;
roosalyn 0:50d492ea0fd0 387 }
roosalyn 0:50d492ea0fd0 388 }
roosalyn 0:50d492ea0fd0 389 }
roosalyn 0:50d492ea0fd0 390
roosalyn 0:50d492ea0fd0 391 void countStepY()
roosalyn 0:50d492ea0fd0 392 {
roosalyn 1:d9cfdc904b10 393 if (start == true) { //X en Y kunnen niet tegelijk optellen, alleen als de ander niet ingedrukt is // start moet nog start1 en start2 worden
roosalyn 0:50d492ea0fd0 394 countsY+= countStateY;
roosalyn 0:50d492ea0fd0 395 if(countsY >= 10) {
roosalyn 0:50d492ea0fd0 396 countsY = 10;
roosalyn 0:50d492ea0fd0 397 } else if(countsY<=0) {
roosalyn 0:50d492ea0fd0 398 countsY = 0;
roosalyn 0:50d492ea0fd0 399 }
roosalyn 0:50d492ea0fd0 400 }
roosalyn 0:50d492ea0fd0 401 }
roosalyn 0:50d492ea0fd0 402
roosalyn 1:d9cfdc904b10 403
roosalyn 0:50d492ea0fd0 404 void motor1_PID_Controller_activate(){
roosalyn 0:50d492ea0fd0 405 motor1_PID_Controller_go = true;
roosalyn 0:50d492ea0fd0 406 }
roosalyn 0:50d492ea0fd0 407
roosalyn 2:d04df4be6cf7 408 /*void countStepX_activate(){
roosalyn 0:50d492ea0fd0 409 countStepX_go = true;
roosalyn 0:50d492ea0fd0 410 }
roosalyn 2:d04df4be6cf7 411 */
roosalyn 2:d04df4be6cf7 412 void checkButtonState_activate(){
roosalyn 0:50d492ea0fd0 413 checkButtonStateX_go = true;
roosalyn 2:d04df4be6cf7 414 checkButtonStateY_go = true;
roosalyn 0:50d492ea0fd0 415 }
roosalyn 2:d04df4be6cf7 416 /*
roosalyn 0:50d492ea0fd0 417 void countStepY_activate(){
roosalyn 0:50d492ea0fd0 418 countStepY_go = true;
roosalyn 0:50d492ea0fd0 419 }
roosalyn 0:50d492ea0fd0 420
roosalyn 2:d04df4be6cf7 421
roosalyn 0:50d492ea0fd0 422 void checkButtonStateY_activate(){
roosalyn 0:50d492ea0fd0 423 checkButtonStateY_go = true;
roosalyn 0:50d492ea0fd0 424 }
roosalyn 2:d04df4be6cf7 425 */
roosalyn 1:d9cfdc904b10 426 void Filter_activate(){
roosalyn 1:d9cfdc904b10 427 Filter_go = true;
roosalyn 1:d9cfdc904b10 428 }
roosalyn 1:d9cfdc904b10 429
roosalyn 0:50d492ea0fd0 430 int main()
roosalyn 0:50d492ea0fd0 431 {
roosalyn 0:50d492ea0fd0 432 pc.baud(9600);
roosalyn 0:50d492ea0fd0 433 encoder1.setPosition(0);
roosalyn 0:50d492ea0fd0 434 encoder2.setPosition(0);
roosalyn 2:d04df4be6cf7 435
roosalyn 0:50d492ea0fd0 436 myControllerTicker1.attach( &motor1_PID_Controller_activate, 0.01f); // Function@ hz
roosalyn 2:d04df4be6cf7 437 //countStepTickerX.attach( &countStepX_activate, 1.0f); // Function@ hz
roosalyn 2:d04df4be6cf7 438 //countStepTickerY.attach( &countStepY_activate, 1.04f); // Function@ hz
roosalyn 2:d04df4be6cf7 439 //buttonStateCheckX.attach( &checkButtonStateX_activate, 0.001f); // Function@ hz
roosalyn 2:d04df4be6cf7 440 //buttonStateCheckY.attach( &checkButtonStateY_activate, 0.001f); // Function@ hz
roosalyn 2:d04df4be6cf7 441 //buttonStateCheck.attach( &checkButtonState_activate,0.001f);
roosalyn 0:50d492ea0fd0 442
roosalyn 1:d9cfdc904b10 443 //EMG-gedeelte
roosalyn 1:d9cfdc904b10 444 led_green.write(1);
roosalyn 1:d9cfdc904b10 445 led_red.write(1);
roosalyn 1:d9cfdc904b10 446 led_blue.write(1);
roosalyn 1:d9cfdc904b10 447 scopeTimer.attach(Filter_activate, 0.001);
roosalyn 1:d9cfdc904b10 448 //scopeTimer.attach(Filter, 0.001);
roosalyn 1:d9cfdc904b10 449 //Amplitude_height.attach(Amplitude, 0.1); //elke 0.05 seconde start is 0 of 1 printen
roosalyn 1:d9cfdc904b10 450
roosalyn 1:d9cfdc904b10 451
roosalyn 0:50d492ea0fd0 452 while (true) {
roosalyn 0:50d492ea0fd0 453
roosalyn 1:d9cfdc904b10 454 pc.printf(" 1: pos: %d, ref: %d, dif: %d, Ki: %.5f, Kd: %.5f, countsX: %d, countsY: %d \r\n", encoder1.getPosition(), reference1,difference1,m1_Ki,m1_Kd,countsX,countsY);
roosalyn 0:50d492ea0fd0 455 //pc.printf(" 2: pos: %d, ref: %d, dif: %d \r\n", encoder2.getPosition(), reference2,difference2);
roosalyn 0:50d492ea0fd0 456
roosalyn 1:d9cfdc904b10 457 Calibration();
roosalyn 1:d9cfdc904b10 458
roosalyn 0:50d492ea0fd0 459 if(motor1_PID_Controller_go) {
roosalyn 0:50d492ea0fd0 460 motor1_PID_Controller_go = false;
roosalyn 0:50d492ea0fd0 461 motor1_PID_Controller();
roosalyn 0:50d492ea0fd0 462 }
roosalyn 2:d04df4be6cf7 463 /*
roosalyn 0:50d492ea0fd0 464 if(countStepX_go) {
roosalyn 0:50d492ea0fd0 465 countStepX_go = false;
roosalyn 0:50d492ea0fd0 466 countStepX();
roosalyn 0:50d492ea0fd0 467 }
roosalyn 0:50d492ea0fd0 468 if(countStepY_go) {
roosalyn 0:50d492ea0fd0 469 countStepY_go = false;
roosalyn 0:50d492ea0fd0 470 countStepY();
roosalyn 0:50d492ea0fd0 471 }
roosalyn 2:d04df4be6cf7 472
roosalyn 0:50d492ea0fd0 473 if(checkButtonStateX_go) {
roosalyn 0:50d492ea0fd0 474 checkButtonStateX_go = false;
roosalyn 0:50d492ea0fd0 475 checkButtonStateX();
roosalyn 0:50d492ea0fd0 476 }
roosalyn 0:50d492ea0fd0 477 if(checkButtonStateY_go) {
roosalyn 0:50d492ea0fd0 478 checkButtonStateY_go = false;
roosalyn 0:50d492ea0fd0 479 checkButtonStateY();
roosalyn 0:50d492ea0fd0 480 }
roosalyn 2:d04df4be6cf7 481 */
roosalyn 1:d9cfdc904b10 482 if(Filter_go) {
roosalyn 1:d9cfdc904b10 483 Filter_go = false;
roosalyn 1:d9cfdc904b10 484 Filter();
roosalyn 1:d9cfdc904b10 485 }
roosalyn 0:50d492ea0fd0 486 }
roosalyn 0:50d492ea0fd0 487 }