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@1:d9cfdc904b10, 2015-10-16 (annotated)

Committer:
roosalyn
Date:
Fri Oct 16 13:41:47 2015 +0000
Revision:
1:d9cfdc904b10
Parent:
0:50d492ea0fd0
Child:
2:d04df4be6cf7
EMG aangesloten op motoren, werkt nog niet: raar signaal in hidscope

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 0:50d492ea0fd0 28 DigitalIn telknopX(SW2);
roosalyn 0:50d492ea0fd0 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 1:d9cfdc904b10 126 double h1_emg1 = High_1_Filter1.step(reademg1);
roosalyn 1:d9cfdc904b10 127 high[0] = High_2_Filter1.step(h1_emg1);
roosalyn 1:d9cfdc904b10 128 double h1_emg2 = High_1_Filter2.step(reademg2);
roosalyn 1:d9cfdc904b10 129 high[1] = High_2_Filter2.step(h1_emg2);
roosalyn 1:d9cfdc904b10 130 return high;
roosalyn 1:d9cfdc904b10 131 }
roosalyn 1:d9cfdc904b10 132
roosalyn 1:d9cfdc904b10 133 double* NotchFilter(double n_emg1, double n_emg2 )
roosalyn 1:d9cfdc904b10 134 {
roosalyn 1:d9cfdc904b10 135 notch[0] = Notch_Filter1.step(n_emg1);
roosalyn 1:d9cfdc904b10 136 notch[1] = Notch_Filter2.step(n_emg2);
roosalyn 1:d9cfdc904b10 137 return notch;
roosalyn 1:d9cfdc904b10 138 }
roosalyn 1:d9cfdc904b10 139
roosalyn 1:d9cfdc904b10 140 void Filter(void)
roosalyn 1:d9cfdc904b10 141 {
roosalyn 1:d9cfdc904b10 142 HighFilter();
roosalyn 1:d9cfdc904b10 143 double h2_emg1_abs = fabs(high[0]);
roosalyn 1:d9cfdc904b10 144 double h2_emg2_abs = fabs(high[1]);
roosalyn 1:d9cfdc904b10 145 LowFilter(h2_emg1_abs, h2_emg2_abs);
roosalyn 1:d9cfdc904b10 146 double filtered_emg1 = low[0];
roosalyn 1:d9cfdc904b10 147 double filtered_emg2 = low[1];
roosalyn 1:d9cfdc904b10 148 NotchFilter(filtered_emg1, filtered_emg2);
roosalyn 1:d9cfdc904b10 149 filtered_notch1 = notch[0];
roosalyn 1:d9cfdc904b10 150 filtered_notch2 = notch[1];
roosalyn 1:d9cfdc904b10 151 signal1 = filtered_notch1/max1;
roosalyn 1:d9cfdc904b10 152 signal2 = filtered_notch2/max2;
roosalyn 1:d9cfdc904b10 153 //scope.set(2, filtered_notch1);
roosalyn 1:d9cfdc904b10 154 //scope.set(3, filtered_notch2);
roosalyn 1:d9cfdc904b10 155 scope.set(0, signal1);
roosalyn 1:d9cfdc904b10 156 scope.set(1, signal2);
roosalyn 1:d9cfdc904b10 157 scope.send();
roosalyn 1:d9cfdc904b10 158 }
roosalyn 1:d9cfdc904b10 159
roosalyn 1:d9cfdc904b10 160
roosalyn 1:d9cfdc904b10 161
roosalyn 1:d9cfdc904b10 162 // ------ OVERIGE FUNCTIES ------ //
roosalyn 1:d9cfdc904b10 163
roosalyn 0:50d492ea0fd0 164 bool buttonReleasedX(DigitalIn& button)
roosalyn 0:50d492ea0fd0 165 {
roosalyn 0:50d492ea0fd0 166 if (button.read() == 1 && buttonStateX == 0) {
roosalyn 0:50d492ea0fd0 167 buttonStateX = 1;
roosalyn 0:50d492ea0fd0 168 return true;
roosalyn 0:50d492ea0fd0 169 }
roosalyn 0:50d492ea0fd0 170 buttonStateX = button.read();
roosalyn 0:50d492ea0fd0 171 return false;
roosalyn 0:50d492ea0fd0 172 }
roosalyn 0:50d492ea0fd0 173
roosalyn 0:50d492ea0fd0 174 bool buttonReleasedY(DigitalIn& button)
roosalyn 0:50d492ea0fd0 175 {
roosalyn 0:50d492ea0fd0 176 if (button.read() == 1 && buttonStateY == 0) {
roosalyn 0:50d492ea0fd0 177 buttonStateY = 1;
roosalyn 0:50d492ea0fd0 178 return true;
roosalyn 0:50d492ea0fd0 179 }
roosalyn 0:50d492ea0fd0 180 buttonStateY = button.read();
roosalyn 0:50d492ea0fd0 181 return false;
roosalyn 0:50d492ea0fd0 182 }
roosalyn 0:50d492ea0fd0 183
roosalyn 0:50d492ea0fd0 184 bool buttonHold(DigitalIn& button)
roosalyn 0:50d492ea0fd0 185 {
roosalyn 0:50d492ea0fd0 186 return (button.read() == 0);
roosalyn 0:50d492ea0fd0 187 }
roosalyn 0:50d492ea0fd0 188
roosalyn 0:50d492ea0fd0 189 float bereken_hoek1(float x, float y)
roosalyn 0:50d492ea0fd0 190 {
roosalyn 0:50d492ea0fd0 191 float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
roosalyn 1:d9cfdc904b10 192 float alpha = acos((langste_zijde/2)/35); //4200=1 rondje
roosalyn 0:50d492ea0fd0 193 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 194 }
roosalyn 0:50d492ea0fd0 195
roosalyn 0:50d492ea0fd0 196 float bereken_hoek2(float x, float y)
roosalyn 0:50d492ea0fd0 197 {
roosalyn 0:50d492ea0fd0 198 float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
roosalyn 1:d9cfdc904b10 199 float alpha = acos((langste_zijde/2)/35);
roosalyn 0:50d492ea0fd0 200 float beta = 0.5*M_PI - alpha;
roosalyn 0:50d492ea0fd0 201 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 202 }
roosalyn 0:50d492ea0fd0 203
roosalyn 1:d9cfdc904b10 204 void Amplitude() //true is aangespanen, false is niet aangespannen. met 2 thresholds. Dit moet nog in procent worden omgezet!
roosalyn 1:d9cfdc904b10 205 {
roosalyn 1:d9cfdc904b10 206 if(start == false && filtered_notch1< 0.6) { //moeten waarden 70 en 20 worden als drempelwaarden
roosalyn 1:d9cfdc904b10 207 start = true;
roosalyn 1:d9cfdc904b10 208 //printf("start 0 \n\r");
roosalyn 1:d9cfdc904b10 209 }
roosalyn 1:d9cfdc904b10 210 if (start == true && filtered_notch1<0.6) {
roosalyn 1:d9cfdc904b10 211 start = true;
roosalyn 1:d9cfdc904b10 212 //printf("start 1\n\r");
roosalyn 1:d9cfdc904b10 213 }
roosalyn 1:d9cfdc904b10 214 if (start == true && filtered_notch1< 0.2) {
roosalyn 1:d9cfdc904b10 215 start = false;
roosalyn 1:d9cfdc904b10 216 //printf("start 1\n\r");
roosalyn 1:d9cfdc904b10 217 }
roosalyn 1:d9cfdc904b10 218 if (start == false && filtered_notch1 <0.2) {
roosalyn 1:d9cfdc904b10 219 start = false;
roosalyn 1:d9cfdc904b10 220 //printf("start 0\n\r");
roosalyn 1:d9cfdc904b10 221 }
roosalyn 1:d9cfdc904b10 222 if (start == true && filtered_notch1 <0.6 && filtered_notch1>0.2) {
roosalyn 1:d9cfdc904b10 223 start = true;
roosalyn 1:d9cfdc904b10 224 //printf("start 1\n\r");
roosalyn 1:d9cfdc904b10 225 }
roosalyn 1:d9cfdc904b10 226 if (start == false && filtered_notch1 <0.6 && filtered_notch1>0.2) {
roosalyn 1:d9cfdc904b10 227 start = false;
roosalyn 1:d9cfdc904b10 228 //printf("start 0\n\r");
roosalyn 1:d9cfdc904b10 229 }
roosalyn 1:d9cfdc904b10 230 }
roosalyn 1:d9cfdc904b10 231
roosalyn 1:d9cfdc904b10 232 // ----- KALIBRATIE ------ //
roosalyn 1:d9cfdc904b10 233
roosalyn 1:d9cfdc904b10 234 void MaxValue()
roosalyn 1:d9cfdc904b10 235 {
roosalyn 1:d9cfdc904b10 236 int length = 500;
roosalyn 1:d9cfdc904b10 237 double meetwaarden1 [500] = {}; //lege array maken voor emg1
roosalyn 1:d9cfdc904b10 238 double meetwaarden2 [500] = {}; //lege array maken voor emg2
roosalyn 1:d9cfdc904b10 239 for(int i = 0; i<500; i++) { //array vullen met meetwaarden van emg1
roosalyn 1:d9cfdc904b10 240 meetwaarden1[i] = filtered_notch1;
roosalyn 1:d9cfdc904b10 241 wait(0.005); // zonder wait zit er maar een dezelfde waarde in de array! eventueel een ticker er van maken
roosalyn 1:d9cfdc904b10 242 }
roosalyn 1:d9cfdc904b10 243 for(int i = 0; i<500; i++) { //array vullen met meetwaarden van emg2
roosalyn 1:d9cfdc904b10 244 meetwaarden2[i] = filtered_notch2;
roosalyn 1:d9cfdc904b10 245 wait(0.005);
roosalyn 1:d9cfdc904b10 246 }
roosalyn 1:d9cfdc904b10 247 //printarray(meetwaarden, 500); //was als controle
roosalyn 1:d9cfdc904b10 248 for(int i = 0; i<length; i++) { //maximale waarde van de array berekenen
roosalyn 1:d9cfdc904b10 249 if(meetwaarden1[i] > max1) {
roosalyn 1:d9cfdc904b10 250 max1= meetwaarden1[i];
roosalyn 1:d9cfdc904b10 251 }
roosalyn 1:d9cfdc904b10 252 }
roosalyn 1:d9cfdc904b10 253 for(int i = 0; i<length; i++) { //maximale waarde van de array berekenen
roosalyn 1:d9cfdc904b10 254 if(meetwaarden2[i] > max2) {
roosalyn 1:d9cfdc904b10 255 max2 = meetwaarden2[i];
roosalyn 1:d9cfdc904b10 256 }
roosalyn 1:d9cfdc904b10 257 }
roosalyn 1:d9cfdc904b10 258 printf("max value of emg1 is %f, and of emg 2 is %f \n\r", max1, max2);
roosalyn 1:d9cfdc904b10 259 }
roosalyn 1:d9cfdc904b10 260
roosalyn 1:d9cfdc904b10 261 void Calibration()
roosalyn 1:d9cfdc904b10 262 {
roosalyn 1:d9cfdc904b10 263 switch(button.read()) {
roosalyn 1:d9cfdc904b10 264 case 0: //knop wel indrukken
roosalyn 1:d9cfdc904b10 265 switch(counter) {
roosalyn 1:d9cfdc904b10 266 case 0: //eerste keer knop indrukken
roosalyn 1:d9cfdc904b10 267 led_red.write(0); //rode led aan
roosalyn 1:d9cfdc904b10 268 MaxValue();
roosalyn 1:d9cfdc904b10 269 led_green.write(0); //groene led aan + rode = geel
roosalyn 1:d9cfdc904b10 270 wait(1); //even wachten, zodat er tijd is om de knop weer los te laten
roosalyn 1:d9cfdc904b10 271 counter++;
roosalyn 1:d9cfdc904b10 272 break;
roosalyn 1:d9cfdc904b10 273 case 1: //tweede keer knop indrukken
roosalyn 1:d9cfdc904b10 274 led_red.write(1); //uit
roosalyn 1:d9cfdc904b10 275 led_green.write(1); //uit
roosalyn 1:d9cfdc904b10 276 led_blue.write(0); //blauw ledje aan
roosalyn 1:d9cfdc904b10 277
roosalyn 1:d9cfdc904b10 278 // move arm to 0 position: motoren aansturen.
roosalyn 1:d9cfdc904b10 279 //emg_signal1 = filtered_notch/max*100 //schalen
roosalyn 1:d9cfdc904b10 280 wait(1);
roosalyn 1:d9cfdc904b10 281 counter++;
roosalyn 1:d9cfdc904b10 282 break;
roosalyn 1:d9cfdc904b10 283 case 2:
roosalyn 1:d9cfdc904b10 284 //encoder....setPosition(0);
roosalyn 1:d9cfdc904b10 285 led_blue.write(1); //uit
roosalyn 1:d9cfdc904b10 286 led_green.write(0); // ledje groen aan
roosalyn 1:d9cfdc904b10 287 wait(1);
roosalyn 1:d9cfdc904b10 288 counter++;
roosalyn 1:d9cfdc904b10 289 break;
roosalyn 1:d9cfdc904b10 290 }
roosalyn 1:d9cfdc904b10 291 break;
roosalyn 1:d9cfdc904b10 292 }
roosalyn 1:d9cfdc904b10 293 }
roosalyn 1:d9cfdc904b10 294
roosalyn 1:d9cfdc904b10 295 //voor de motoren: als counter =0: motorspeed =0
roosalyn 1:d9cfdc904b10 296 //als counter = 1: niet in vakjes rekenen
roosalyn 1:d9cfdc904b10 297 // als counter >1: wel in vakjes rekenen
roosalyn 1:d9cfdc904b10 298
roosalyn 1:d9cfdc904b10 299
roosalyn 0:50d492ea0fd0 300 //-------------------------- MOTORCONTROLLERS --------------------------//
roosalyn 0:50d492ea0fd0 301
roosalyn 0:50d492ea0fd0 302 void constanteBepalen()
roosalyn 0:50d492ea0fd0 303 {
roosalyn 0:50d492ea0fd0 304 constante1 = potmeter1.read();
roosalyn 0:50d492ea0fd0 305 constante2 = potmeter2.read();
roosalyn 0:50d492ea0fd0 306 m1_Kd = constante2*0.001; //loopt nu van 0 tot 1
roosalyn 0:50d492ea0fd0 307 m1_Ki = constante1*0.001;
roosalyn 0:50d492ea0fd0 308 }
roosalyn 0:50d492ea0fd0 309
roosalyn 0:50d492ea0fd0 310 double biquad(double u, double &v1, double &v2, const double a1, const double a2,
roosalyn 0:50d492ea0fd0 311 const double b0, const double b1, const double b2){
roosalyn 0:50d492ea0fd0 312 double v = u-a1*v1-a2*v2; //formule uit slides van biquad filter
roosalyn 0:50d492ea0fd0 313 double y = b0*v+b1*v1+b2*v2; //zie ook slides
roosalyn 0:50d492ea0fd0 314 v2 = v1; v1 =v;
roosalyn 0:50d492ea0fd0 315 return y;
roosalyn 0:50d492ea0fd0 316 }
roosalyn 0:50d492ea0fd0 317
roosalyn 0:50d492ea0fd0 318 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,
roosalyn 0:50d492ea0fd0 319 double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,
roosalyn 0:50d492ea0fd0 320 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 321 //Bereken eerst afgeleide van de error
roosalyn 0:50d492ea0fd0 322 double e_der = (e-e_prev)/Ts;
roosalyn 0:50d492ea0fd0 323 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 324 e_prev = e;
roosalyn 0:50d492ea0fd0 325 //Bereken nu de geintegreerde versie van de error
roosalyn 0:50d492ea0fd0 326 e_int = e_int + Ts*e;
roosalyn 0:50d492ea0fd0 327 //PID
roosalyn 0:50d492ea0fd0 328
roosalyn 0:50d492ea0fd0 329 return Kp*e+Ki*e_int+Kd*e_der2; //formule uit slides
roosalyn 0:50d492ea0fd0 330 }
roosalyn 0:50d492ea0fd0 331
roosalyn 1:d9cfdc904b10 332 // ---- PID Controller ----- //
roosalyn 0:50d492ea0fd0 333
roosalyn 0:50d492ea0fd0 334 void motor1_PID_Controller()
roosalyn 0:50d492ea0fd0 335 {
roosalyn 0:50d492ea0fd0 336 //reference1 = potmeter1.read()*4200; //draaiknop uitlezen, tussen 0 en 1
roosalyn 0:50d492ea0fd0 337 float hoek1 = bereken_hoek1(countsX,countsY);
roosalyn 0:50d492ea0fd0 338 reference1=hoek1;
roosalyn 0:50d492ea0fd0 339 double position =(encoder1.getPosition()); // waarde tussen 0 en 4200
roosalyn 0:50d492ea0fd0 340
roosalyn 0:50d492ea0fd0 341 scope.set(0,reference1);
roosalyn 0:50d492ea0fd0 342 scope.set(1,position);
roosalyn 0:50d492ea0fd0 343 scope.send();
roosalyn 0:50d492ea0fd0 344
roosalyn 0:50d492ea0fd0 345 difference1 = reference1 - position;
roosalyn 0:50d492ea0fd0 346 constanteBepalen();
roosalyn 0:50d492ea0fd0 347 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 348 if (u < 0) {
roosalyn 0:50d492ea0fd0 349 motor1direction = 1; //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
roosalyn 0:50d492ea0fd0 350 } else if (u>=0) {
roosalyn 0:50d492ea0fd0 351 motor1direction = 0;
roosalyn 0:50d492ea0fd0 352 }
roosalyn 1:d9cfdc904b10 353 if(counter != 0) {
roosalyn 1:d9cfdc904b10 354 motor1speed = fabs(u);
roosalyn 1:d9cfdc904b10 355 }
roosalyn 0:50d492ea0fd0 356 pc.printf("u = %.2f \r\n", u);
roosalyn 0:50d492ea0fd0 357
roosalyn 0:50d492ea0fd0 358 }
roosalyn 0:50d492ea0fd0 359
roosalyn 0:50d492ea0fd0 360 //-------------------------- POSITIEBEPALING --------------------------//
roosalyn 0:50d492ea0fd0 361
roosalyn 0:50d492ea0fd0 362 void checkButtonStateX()
roosalyn 0:50d492ea0fd0 363 {
roosalyn 0:50d492ea0fd0 364 if (buttonReleasedX(telknopX)) {
roosalyn 0:50d492ea0fd0 365 countStateX = -countStateX;
roosalyn 0:50d492ea0fd0 366 }
roosalyn 0:50d492ea0fd0 367 }
roosalyn 0:50d492ea0fd0 368
roosalyn 0:50d492ea0fd0 369 void checkButtonStateY()
roosalyn 0:50d492ea0fd0 370 {
roosalyn 0:50d492ea0fd0 371 if (buttonReleasedY(telknopY)) {
roosalyn 0:50d492ea0fd0 372 countStateY = -countStateY;
roosalyn 0:50d492ea0fd0 373 }
roosalyn 0:50d492ea0fd0 374 }
roosalyn 0:50d492ea0fd0 375
roosalyn 0:50d492ea0fd0 376 void countStepX()
roosalyn 0:50d492ea0fd0 377 {
roosalyn 1:d9cfdc904b10 378 if (start == true) {
roosalyn 0:50d492ea0fd0 379 countsX+= countStateX; //hier moet de richting dus veranderen
roosalyn 0:50d492ea0fd0 380 if(countsX >= 10) {
roosalyn 0:50d492ea0fd0 381 countsX = 10;
roosalyn 0:50d492ea0fd0 382 } else if(countsX<=0) {
roosalyn 0:50d492ea0fd0 383 countsX = 0;
roosalyn 0:50d492ea0fd0 384 }
roosalyn 0:50d492ea0fd0 385 }
roosalyn 0:50d492ea0fd0 386 }
roosalyn 0:50d492ea0fd0 387
roosalyn 0:50d492ea0fd0 388 void countStepY()
roosalyn 0:50d492ea0fd0 389 {
roosalyn 1:d9cfdc904b10 390 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 391 countsY+= countStateY;
roosalyn 0:50d492ea0fd0 392 if(countsY >= 10) {
roosalyn 0:50d492ea0fd0 393 countsY = 10;
roosalyn 0:50d492ea0fd0 394 } else if(countsY<=0) {
roosalyn 0:50d492ea0fd0 395 countsY = 0;
roosalyn 0:50d492ea0fd0 396 }
roosalyn 0:50d492ea0fd0 397 }
roosalyn 0:50d492ea0fd0 398 }
roosalyn 0:50d492ea0fd0 399
roosalyn 1:d9cfdc904b10 400
roosalyn 0:50d492ea0fd0 401 void motor1_PID_Controller_activate(){
roosalyn 0:50d492ea0fd0 402 motor1_PID_Controller_go = true;
roosalyn 0:50d492ea0fd0 403 }
roosalyn 0:50d492ea0fd0 404
roosalyn 0:50d492ea0fd0 405 void countStepX_activate(){
roosalyn 0:50d492ea0fd0 406 countStepX_go = true;
roosalyn 0:50d492ea0fd0 407 }
roosalyn 0:50d492ea0fd0 408
roosalyn 0:50d492ea0fd0 409 void checkButtonStateX_activate(){
roosalyn 0:50d492ea0fd0 410 checkButtonStateX_go = true;
roosalyn 0:50d492ea0fd0 411 }
roosalyn 0:50d492ea0fd0 412
roosalyn 0:50d492ea0fd0 413 void countStepY_activate(){
roosalyn 0:50d492ea0fd0 414 countStepY_go = true;
roosalyn 0:50d492ea0fd0 415 }
roosalyn 0:50d492ea0fd0 416
roosalyn 0:50d492ea0fd0 417 void checkButtonStateY_activate(){
roosalyn 0:50d492ea0fd0 418 checkButtonStateY_go = true;
roosalyn 0:50d492ea0fd0 419 }
roosalyn 0:50d492ea0fd0 420
roosalyn 1:d9cfdc904b10 421 void Filter_activate(){
roosalyn 1:d9cfdc904b10 422 Filter_go = true;
roosalyn 1:d9cfdc904b10 423 }
roosalyn 1:d9cfdc904b10 424
roosalyn 0:50d492ea0fd0 425 int main()
roosalyn 0:50d492ea0fd0 426 {
roosalyn 0:50d492ea0fd0 427 pc.baud(9600);
roosalyn 0:50d492ea0fd0 428 encoder1.setPosition(0);
roosalyn 0:50d492ea0fd0 429 encoder2.setPosition(0);
roosalyn 0:50d492ea0fd0 430 /*myControllerTicker1.attach( &motor1_PID_Controller, 0.005f ); //Ticker roept elke 0.01 s de motor2 controller aan. = 100Hz
roosalyn 0:50d492ea0fd0 431 myControllerTicker2.attach( &motor2_PI_Controller, 0.01f);
roosalyn 0:50d492ea0fd0 432 countStepTickerX.attach( &countStepX,1.0f );
roosalyn 0:50d492ea0fd0 433 countStepTickerY.attach( &countStepY,1.0f);
roosalyn 0:50d492ea0fd0 434 buttonStateCheckX.attach( &checkButtonStateX,0.001f);
roosalyn 0:50d492ea0fd0 435 buttonStateCheckY.attach( &checkButtonStateY,0.001f);
roosalyn 0:50d492ea0fd0 436 */
roosalyn 0:50d492ea0fd0 437 myControllerTicker1.attach( &motor1_PID_Controller_activate, 0.01f); // Function@ hz
roosalyn 0:50d492ea0fd0 438 countStepTickerX.attach( &countStepX_activate, 1.0f); // Function@ hz
roosalyn 0:50d492ea0fd0 439 countStepTickerY.attach( &countStepY_activate, 1.04f); // Function@ hz
roosalyn 0:50d492ea0fd0 440 buttonStateCheckX.attach( &checkButtonStateX_activate, 0.001f); // Function@ hz
roosalyn 0:50d492ea0fd0 441 buttonStateCheckY.attach( &checkButtonStateY_activate, 0.001f); // Function@ hz
roosalyn 0:50d492ea0fd0 442
roosalyn 0:50d492ea0fd0 443
roosalyn 1:d9cfdc904b10 444 //EMG-gedeelte
roosalyn 1:d9cfdc904b10 445 led_green.write(1);
roosalyn 1:d9cfdc904b10 446 led_red.write(1);
roosalyn 1:d9cfdc904b10 447 led_blue.write(1);
roosalyn 1:d9cfdc904b10 448 scopeTimer.attach(Filter_activate, 0.001);
roosalyn 1:d9cfdc904b10 449 //scopeTimer.attach(Filter, 0.001);
roosalyn 1:d9cfdc904b10 450 //Amplitude_height.attach(Amplitude, 0.1); //elke 0.05 seconde start is 0 of 1 printen
roosalyn 1:d9cfdc904b10 451
roosalyn 1:d9cfdc904b10 452
roosalyn 0:50d492ea0fd0 453 while (true) {
roosalyn 0:50d492ea0fd0 454
roosalyn 1:d9cfdc904b10 455 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 456 //pc.printf(" 2: pos: %d, ref: %d, dif: %d \r\n", encoder2.getPosition(), reference2,difference2);
roosalyn 0:50d492ea0fd0 457
roosalyn 1:d9cfdc904b10 458 Calibration();
roosalyn 1:d9cfdc904b10 459
roosalyn 0:50d492ea0fd0 460 if(motor1_PID_Controller_go) {
roosalyn 0:50d492ea0fd0 461 motor1_PID_Controller_go = false;
roosalyn 0:50d492ea0fd0 462 motor1_PID_Controller();
roosalyn 0:50d492ea0fd0 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 0:50d492ea0fd0 472 if(checkButtonStateX_go) {
roosalyn 0:50d492ea0fd0 473 checkButtonStateX_go = false;
roosalyn 0:50d492ea0fd0 474 checkButtonStateX();
roosalyn 0:50d492ea0fd0 475 }
roosalyn 0:50d492ea0fd0 476 if(checkButtonStateY_go) {
roosalyn 0:50d492ea0fd0 477 checkButtonStateY_go = false;
roosalyn 0:50d492ea0fd0 478 checkButtonStateY();
roosalyn 0:50d492ea0fd0 479 }
roosalyn 1:d9cfdc904b10 480 if(Filter_go) {
roosalyn 1:d9cfdc904b10 481 Filter_go = false;
roosalyn 1:d9cfdc904b10 482 Filter();
roosalyn 1:d9cfdc904b10 483 }
roosalyn 0:50d492ea0fd0 484 }
roosalyn 0:50d492ea0fd0 485 }