Arun Raveenthiran / Mbed 2 deprecated VerslagCode

Opgeschoonde code voor verslag

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of TotalCodegr13V2 by Rianne Bulthuis

main.cpp@16:bf76ed65da88, 2015-11-02 (annotated)

Committer:
arunr
Date:
Mon Nov 02 14:06:12 2015 +0000
Revision:
16:bf76ed65da88
Parent:
15:ece330b45d93
Child:
17:416876824d8c
PI controller uitgesloopt, home pos arm1 werkt, 2 niet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arunr 0:65ab9f79a4cc 1 #include "mbed.h"
arunr 0:65ab9f79a4cc 2 #include "encoder.h"
arunr 0:65ab9f79a4cc 3 #include "HIDScope.h"
riannebulthuis 1:7d5e6bc2b314 4 #include "MODSERIAL.h"
arunr 0:65ab9f79a4cc 5
riannebulthuis 8:b219ca30967f 6
riannebulthuis 8:b219ca30967f 7 // pins
arunr 0:65ab9f79a4cc 8 DigitalOut motor1_direction(D4);
arunr 0:65ab9f79a4cc 9 PwmOut motor1_speed(D5);
arunr 10:34ccb2fed2ef 10 DigitalOut motor2_direction(D7);
arunr 10:34ccb2fed2ef 11 PwmOut motor2_speed(D6);
arunr 0:65ab9f79a4cc 12 DigitalIn button_1(PTC6); //counterclockwise
arunr 0:65ab9f79a4cc 13 DigitalIn button_2(PTA4); //clockwise
riannebulthuis 8:b219ca30967f 14 AnalogIn PotMeter1(A4);
arunr 10:34ccb2fed2ef 15 AnalogIn PotMeter2(A5);
arunr 10:34ccb2fed2ef 16 AnalogIn EMG_bicepsright(A0);
arunr 10:34ccb2fed2ef 17 AnalogIn EMG_bicepsleft(A1);
RichellBooyink 11:b2dec8f3e75c 18 AnalogIn EMG_legright(A2);
RichellBooyink 11:b2dec8f3e75c 19 AnalogIn EMG_legleft(A3);
arunr 5:b9d5d7311dac 20 Ticker controller;
arunr 5:b9d5d7311dac 21 Ticker ticker_regelaar;
arunr 6:1597888c9a56 22 Ticker EMG_Filter;
arunr 6:1597888c9a56 23 Ticker Scope;
arunr 0:65ab9f79a4cc 24 Encoder motor1(D12,D13);
arunr 10:34ccb2fed2ef 25 Encoder motor2(D10,D11);
arunr 10:34ccb2fed2ef 26 HIDScope scope(6);
riannebulthuis 8:b219ca30967f 27 MODSERIAL pc(USBTX, USBRX);
arunr 0:65ab9f79a4cc 28
riannebulthuis 8:b219ca30967f 29
riannebulthuis 8:b219ca30967f 30
riannebulthuis 8:b219ca30967f 31 // Regelaar homeposition
arunr 10:34ccb2fed2ef 32 #define SAMPLETIME_REGELAAR 0.005
riannebulthuis 2:866a8a9f2b93 33 volatile bool regelaar_ticker_flag;
arunr 10:34ccb2fed2ef 34 void setregelaar_ticker_flag()
arunr 10:34ccb2fed2ef 35 {
riannebulthuis 2:866a8a9f2b93 36 regelaar_ticker_flag = true;
riannebulthuis 2:866a8a9f2b93 37 }
riannebulthuis 2:866a8a9f2b93 38
riannebulthuis 2:866a8a9f2b93 39 //define states
RichellBooyink 12:146ba6f95fe0 40 enum state {HOME, MOVE_MOTORS, BACKTOHOMEPOSITION, STOP};
arunr 4:b4530fb376dd 41 uint8_t state = HOME;
arunr 0:65ab9f79a4cc 42
riannebulthuis 8:b219ca30967f 43 // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering (PI-controller)
riannebulthuis 3:5f59cbe53d7d 44 const double g = 360; // Aantal graden 1 rotatie
riannebulthuis 3:5f59cbe53d7d 45 const double c = 4200; // Aantal counts 1 rotatie
arunr 15:ece330b45d93 46 const double q = g/c;
riannebulthuis 3:5f59cbe53d7d 47
riannebulthuis 3:5f59cbe53d7d 48
arunr 5:b9d5d7311dac 49
arunr 5:b9d5d7311dac 50 // Filter1 = High pass filter tot 20 Hz
arunr 10:34ccb2fed2ef 51 double ah1_v1=0, ah1_v2=0, ah2_v1=0, ah2_v2=0;
arunr 10:34ccb2fed2ef 52 double bh1_v1=0, bh1_v2=0, bh2_v1=0, bh2_v2=0;
arunr 10:34ccb2fed2ef 53 double ch1_v1=0, ch1_v2=0, ch2_v1=0, ch2_v2=0;
arunr 10:34ccb2fed2ef 54 double dh1_v1=0, dh1_v2=0, dh2_v1=0, dh2_v2=0;
arunr 10:34ccb2fed2ef 55 const double fh1_a1=-0.50701081158, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
arunr 10:34ccb2fed2ef 56 const double fh2_a1=-1.24532140600, fh2_a2=0.54379713891, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
arunr 5:b9d5d7311dac 57 // Filter2 = Low pass filter na 60 Hz
arunr 10:34ccb2fed2ef 58 double al1_v1=0, al1_v2=0, al2_v1=0, al2_v2=0;
arunr 10:34ccb2fed2ef 59 double bl1_v1=0, bl1_v2=0, bl2_v1=0, bl2_v2=0;
arunr 10:34ccb2fed2ef 60 double cl1_v1=0, cl1_v2=0, cl2_v1=0, cl2_v2=0;
arunr 10:34ccb2fed2ef 61 double dl1_v1=0, dl1_v2=0, dl2_v1=0, dl2_v2=0;
arunr 10:34ccb2fed2ef 62 const double fl1_a1=0.15970686439, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
arunr 10:34ccb2fed2ef 63 const double fl2_a1=0.42229458338, fl2_a2=0.35581444792, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
arunr 5:b9d5d7311dac 64 // Filter3 = Notch filter at 50 Hz
arunr 10:34ccb2fed2ef 65 double ano1_v1=0, ano1_v2=0, ano2_v1=0, ano2_v2=0, ano3_v1=0, ano3_v2=0;
arunr 10:34ccb2fed2ef 66 double bno1_v1=0, bno1_v2=0, bno2_v1=0, bno2_v2=0, bno3_v1=0, bno3_v2=0;
arunr 10:34ccb2fed2ef 67 double cno1_v1=0, cno1_v2=0, cno2_v1=0, cno2_v2=0, cno3_v1=0, cno3_v2=0;
arunr 10:34ccb2fed2ef 68 double dno1_v1=0, dno1_v2=0, dno2_v1=0, dno2_v2=0, dno3_v1=0, dno3_v2=0;
arunr 10:34ccb2fed2ef 69 const double fno1_a1 = -0.03255814954, fno1_a2= 0.92336486961, fno1_b0= 1, fno1_b1= -0.03385540628, fno1_b2= 1;
arunr 10:34ccb2fed2ef 70 const double fno2_a1 = 0.03447359684, fno2_a2= 0.96095720701, fno2_b0= 1, fno2_b1= -0.03385540628, fno2_b2= 1;
arunr 10:34ccb2fed2ef 71 const double fno3_a1 = -0.10078591122, fno3_a2= 0.96100189080, fno3_b0= 1, fno3_b1= -0.03385540628, fno3_b2= 1;
arunr 5:b9d5d7311dac 72 // Filter4 = Lowpass filter at 5 Hz
arunr 10:34ccb2fed2ef 73 double alp1_v1=0, alp1_v2=0, alp2_v1=0, alp2_v2=0;
arunr 10:34ccb2fed2ef 74 double blp1_v1=0, blp1_v2=0, blp2_v1=0, blp2_v2=0;
arunr 10:34ccb2fed2ef 75 double clp1_v1=0, clp1_v2=0, clp2_v1=0, clp2_v2=0;
arunr 10:34ccb2fed2ef 76 double dlp1_v1=0, dlp1_v2=0, dlp2_v1=0, dlp2_v2=0;
arunr 10:34ccb2fed2ef 77 const double flp1_a1=-0.86962685441, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
arunr 10:34ccb2fed2ef 78 const double flp2_a1=-1.85211666729, flp2_a2=0.87021679713, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
arunr 10:34ccb2fed2ef 79 double y1, y2, y3, y4, y5, y6, y7, y8, y9, y10, y11, y12, y13, y14, y15, y16, y17, y18, y19, y20, y21, y22, y23, y24, y25, y26, y27, y28, y29, y30, y31, y32, y33, y34, y35, y36;
arunr 10:34ccb2fed2ef 80 double A, B, C, D;
arunr 10:34ccb2fed2ef 81 double final_filter1, final_filter2, final_filter3, final_filter4;
arunr 5:b9d5d7311dac 82
arunr 5:b9d5d7311dac 83 // Standaard formule voor het biquad filter
arunr 10:34ccb2fed2ef 84 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
arunr 10:34ccb2fed2ef 85 {
arunr 5:b9d5d7311dac 86 double v = u - a1*v1 - a2*v2;
arunr 5:b9d5d7311dac 87 double y = b0*v + b1*v1 + b2*v2;
arunr 5:b9d5d7311dac 88 v2=v1;
arunr 5:b9d5d7311dac 89 v1=v;
arunr 5:b9d5d7311dac 90 return y;
arunr 5:b9d5d7311dac 91 }
arunr 5:b9d5d7311dac 92
riannebulthuis 8:b219ca30967f 93 // script voor filters
arunr 10:34ccb2fed2ef 94 void Filters()
arunr 10:34ccb2fed2ef 95 {
RichellBooyink 12:146ba6f95fe0 96 // Biceps right
arunr 10:34ccb2fed2ef 97 A = EMG_bicepsright.read();
arunr 5:b9d5d7311dac 98 //Highpass
arunr 10:34ccb2fed2ef 99 y1 = biquad (A, ah1_v1, ah1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
arunr 10:34ccb2fed2ef 100 y2 = biquad (y1, ah2_v1, ah2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
arunr 5:b9d5d7311dac 101 //Lowpass
arunr 10:34ccb2fed2ef 102 y3 = biquad (y2, al1_v1, al1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
arunr 10:34ccb2fed2ef 103 y4 = biquad (y3, al2_v1, al2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
arunr 10:34ccb2fed2ef 104 // Notch
arunr 10:34ccb2fed2ef 105 y5 = biquad (y4, ano1_v1, ano1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
arunr 10:34ccb2fed2ef 106 y6 = biquad (y5, ano2_v1, ano2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
arunr 10:34ccb2fed2ef 107 y7 = biquad (y6, ano3_v1, ano3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 108 // Rectify sample
arunr 10:34ccb2fed2ef 109 y8 = fabs(y7);
arunr 10:34ccb2fed2ef 110 // Make it smooth
arunr 10:34ccb2fed2ef 111 y9 = biquad (y8, alp1_v1, alp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1* 0.065187, flp1_b2* 0.065187);
arunr 10:34ccb2fed2ef 112 final_filter1 = biquad(y9, alp2_v1, alp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
arunr 10:34ccb2fed2ef 113
RichellBooyink 11:b2dec8f3e75c 114 //Biceps left
arunr 10:34ccb2fed2ef 115 B = EMG_bicepsleft.read();
arunr 10:34ccb2fed2ef 116 //Highpass
arunr 10:34ccb2fed2ef 117 y10 = biquad (B, bh1_v1, bh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
arunr 10:34ccb2fed2ef 118 y11 = biquad (y10, bh2_v1, bh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
arunr 10:34ccb2fed2ef 119 //Lowpass
arunr 10:34ccb2fed2ef 120 y12 = biquad (y11, bl1_v1, bl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
arunr 10:34ccb2fed2ef 121 y13 = biquad (y12, bl2_v1, bl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
arunr 5:b9d5d7311dac 122 // Notch
arunr 10:34ccb2fed2ef 123 y14 = biquad (y13, bno1_v1, bno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
arunr 10:34ccb2fed2ef 124 y15 = biquad (y14, bno2_v1, bno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
arunr 10:34ccb2fed2ef 125 y16 = biquad (y15, bno3_v1, bno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 126 // Rectify sample
arunr 10:34ccb2fed2ef 127 y17 = fabs(y16);
arunr 10:34ccb2fed2ef 128 // Make it smooth
arunr 10:34ccb2fed2ef 129 y18 = biquad (y17, blp1_v1, blp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
arunr 10:34ccb2fed2ef 130 final_filter2 = biquad(y18, blp2_v1, blp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
arunr 10:34ccb2fed2ef 131
RichellBooyink 11:b2dec8f3e75c 132 /// EMG Filter right leg
RichellBooyink 11:b2dec8f3e75c 133 C = EMG_legright.read();
arunr 10:34ccb2fed2ef 134 //Highpass
arunr 10:34ccb2fed2ef 135 y19 = biquad (C, ch1_v1, ch1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
arunr 10:34ccb2fed2ef 136 y20 = biquad (y19, ch2_v1, ch2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
arunr 10:34ccb2fed2ef 137 //Lowpass
arunr 10:34ccb2fed2ef 138 y21 = biquad (y20, cl1_v1, cl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
arunr 10:34ccb2fed2ef 139 y22 = biquad (y21, cl2_v1, cl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
arunr 10:34ccb2fed2ef 140 // Notch
arunr 10:34ccb2fed2ef 141 y23 = biquad (y22, cno1_v1, cno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
arunr 10:34ccb2fed2ef 142 y24 = biquad (y23, cno2_v1, cno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
arunr 10:34ccb2fed2ef 143 y25 = biquad (y24, cno3_v1, cno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 144 // Rectify sample
arunr 10:34ccb2fed2ef 145 y26 = fabs(y25);
arunr 10:34ccb2fed2ef 146 // Make it smooth
arunr 10:34ccb2fed2ef 147 y27 = biquad (y26, clp1_v1, clp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
arunr 10:34ccb2fed2ef 148 final_filter3 = biquad(y27, clp2_v1, clp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
arunr 10:34ccb2fed2ef 149
RichellBooyink 11:b2dec8f3e75c 150 // EMG filter left leg
RichellBooyink 11:b2dec8f3e75c 151 D = EMG_legleft.read();
arunr 10:34ccb2fed2ef 152 //Highpass
arunr 10:34ccb2fed2ef 153 y28 = biquad (D, dh1_v1, dh1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
arunr 10:34ccb2fed2ef 154 y29 = biquad (y28, dh2_v1, dh2_v2, fh2_a1, fh2_a2, fh2_b0*0.697278, fh2_b1*0.697278, fh2_b2*0.697278);
arunr 10:34ccb2fed2ef 155 //Lowpass
arunr 10:34ccb2fed2ef 156 y30 = biquad (y29, dl1_v1, dl1_v2, fl1_a1, fl1_a2, fl1_b0*0.579853, fl1_b1*0.579853, fl1_b2*0.579853);
arunr 10:34ccb2fed2ef 157 y31 = biquad (y30, dl2_v1, dl2_v2, fl2_a1, fl2_a2, fl2_b0*0.444527, fl2_b1*0.444527, fl2_b2*0.444527);
arunr 10:34ccb2fed2ef 158 // Notch
arunr 10:34ccb2fed2ef 159 y32 = biquad (y31, dno1_v1, dno1_v2, fno1_a1, fno1_a2, fno1_b0*0.968276, fno1_b1*0.968276, fno1_b2*0.968276);
arunr 10:34ccb2fed2ef 160 y33 = biquad (y32, dno2_v1, dno2_v2, fno2_a1, fno2_a2, fno2_b0*0.953678, fno2_b1*0.953678, fno2_b2*0.953678);
arunr 10:34ccb2fed2ef 161 y34 = biquad (y33, dno3_v1, dno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
arunr 10:34ccb2fed2ef 162 // Rectify sample
arunr 10:34ccb2fed2ef 163 y35 = fabs(y34);
arunr 10:34ccb2fed2ef 164 // Make it smooth
arunr 10:34ccb2fed2ef 165 y36 = biquad (y35, dlp1_v1, dlp1_v2, flp1_a1, flp1_a2, flp1_b0*0.065187, flp1_b1*0.065187, flp1_b2*0.065187);
arunr 10:34ccb2fed2ef 166 final_filter4 = biquad(y36, dlp2_v1, dlp2_v2, flp2_a1, flp2_a2, flp2_b0*0.004525, flp2_b1*0.004525, flp2_b2*0.004525);
riannebulthuis 9:888ed3c72795 167 }
arunr 5:b9d5d7311dac 168
riannebulthuis 3:5f59cbe53d7d 169
arunr 0:65ab9f79a4cc 170 const int pressed = 0;
arunr 0:65ab9f79a4cc 171
riannebulthuis 8:b219ca30967f 172 // constantes voor berekening homepositie
arunr 10:34ccb2fed2ef 173 double H1;
arunr 10:34ccb2fed2ef 174 double H2;
arunr 10:34ccb2fed2ef 175 double P1;
arunr 10:34ccb2fed2ef 176 double P2;
RichellBooyink 13:4f4cc1a5a79a 177 // Safety stop. Motoren mogen niet verder dan 90 graden bewegen.
RichellBooyink 13:4f4cc1a5a79a 178 volatile bool safety_stop;
arunr 0:65ab9f79a4cc 179
riannebulthuis 8:b219ca30967f 180 void move_motor1()
riannebulthuis 8:b219ca30967f 181 {
arunr 15:ece330b45d93 182 if (button_1 == pressed || (final_filter1 > 0.05 && final_filter2 < 0.02 && final_filter3 < 0.04 && final_filter4 < 0.04)) {
arunr 10:34ccb2fed2ef 183 pc.printf("motor1 cw \n\r");
riannebulthuis 14:25edcf2935f6 184 motor1_direction = 1; //clockwise
RichellBooyink 13:4f4cc1a5a79a 185 motor1_speed = 0.1;
riannebulthuis 14:25edcf2935f6 186 } else if (safety_stop == true && (final_filter2 > 0.05 && final_filter1 < 0.02 && final_filter3 < 0.04 && final_filter4 < 0.04)) {
arunr 10:34ccb2fed2ef 187 pc.printf("motor1 ccw \n\r");
riannebulthuis 14:25edcf2935f6 188 motor1_direction = 0; //counterclockwise
RichellBooyink 13:4f4cc1a5a79a 189 motor1_speed = 0.1 ;
arunr 10:34ccb2fed2ef 190 } else {
RichellBooyink 13:4f4cc1a5a79a 191 pc.printf("Not moving1 \n\r");
arunr 10:34ccb2fed2ef 192 motor1_speed = 0;
arunr 10:34ccb2fed2ef 193 }
arunr 0:65ab9f79a4cc 194 }
arunr 0:65ab9f79a4cc 195
arunr 10:34ccb2fed2ef 196 void move_motor2()
arunr 10:34ccb2fed2ef 197 {
riannebulthuis 14:25edcf2935f6 198 if (safety_stop == true && (final_filter3 > 0.04 && final_filter1 < 0.02 && final_filter2 < 0.02 && final_filter4 < 0.02)) {
arunr 10:34ccb2fed2ef 199 pc.printf("motor2 cw \n\r");
arunr 10:34ccb2fed2ef 200 motor2_direction = 1; //clockwise
arunr 10:34ccb2fed2ef 201 motor2_speed = 0.4;
arunr 15:ece330b45d93 202 } else if (button_2 == pressed || (final_filter4 > 0.03 && final_filter1 < 0.02 && final_filter2 < 0.02 && final_filter3 < 0.02)) {
RichellBooyink 11:b2dec8f3e75c 203 pc.printf("motor2 ccw \n\r");
arunr 10:34ccb2fed2ef 204 motor2_direction = 0; //counterclockwise
arunr 10:34ccb2fed2ef 205 motor2_speed = 0.4;
RichellBooyink 13:4f4cc1a5a79a 206 }
RichellBooyink 13:4f4cc1a5a79a 207 else if (P2 > 500) {
RichellBooyink 13:4f4cc1a5a79a 208 safety_stop = false;
RichellBooyink 13:4f4cc1a5a79a 209 pc.printf("Stop! /n/r");
RichellBooyink 13:4f4cc1a5a79a 210 motor2_direction = 1;
RichellBooyink 13:4f4cc1a5a79a 211 motor2_speed = 0.1;
RichellBooyink 13:4f4cc1a5a79a 212 wait(0.2);
RichellBooyink 13:4f4cc1a5a79a 213 safety_stop = true;
RichellBooyink 13:4f4cc1a5a79a 214 pc.printf("En door /n/r");
RichellBooyink 13:4f4cc1a5a79a 215 } else if (P2 < -500) {
RichellBooyink 13:4f4cc1a5a79a 216 safety_stop = false;
RichellBooyink 13:4f4cc1a5a79a 217 pc.printf("Stop! /n/r");
RichellBooyink 13:4f4cc1a5a79a 218 motor2_direction = 0;
RichellBooyink 13:4f4cc1a5a79a 219 motor2_speed = 0.1;
RichellBooyink 13:4f4cc1a5a79a 220 wait(0.2);
RichellBooyink 13:4f4cc1a5a79a 221 safety_stop = true;
RichellBooyink 13:4f4cc1a5a79a 222 pc.printf("En gaan /n/r");
arunr 10:34ccb2fed2ef 223 } else {
RichellBooyink 13:4f4cc1a5a79a 224 pc.printf("Not moving2 \n\r");
arunr 10:34ccb2fed2ef 225 motor2_speed = 0;
arunr 10:34ccb2fed2ef 226 }
RichellBooyink 13:4f4cc1a5a79a 227
arunr 10:34ccb2fed2ef 228 }
arunr 10:34ccb2fed2ef 229
arunr 10:34ccb2fed2ef 230 void movetohome()
arunr 10:34ccb2fed2ef 231 {
arunr 10:34ccb2fed2ef 232 P1 = motor1.getPosition();
arunr 10:34ccb2fed2ef 233 P2 = motor2.getPosition();
arunr 10:34ccb2fed2ef 234
arunr 10:34ccb2fed2ef 235 if ((P1 >= -28 && P1 <= 28) || (P2 >= -28 && P2 <= 28)) {
arunr 0:65ab9f79a4cc 236 motor1_speed = 0;
arunr 10:34ccb2fed2ef 237 } else if (P1 > 24) {
riannebulthuis 3:5f59cbe53d7d 238 motor1_direction = 1;
riannebulthuis 3:5f59cbe53d7d 239 motor1_speed = 0.1;
arunr 10:34ccb2fed2ef 240 } else if (P1 < -24) {
riannebulthuis 3:5f59cbe53d7d 241 motor1_direction = 0;
riannebulthuis 3:5f59cbe53d7d 242 motor1_speed = 0.1;
arunr 10:34ccb2fed2ef 243 } else if (P2 > 24) {
arunr 10:34ccb2fed2ef 244 motor2_direction = 1;
arunr 10:34ccb2fed2ef 245 motor2_speed = 0.1;
arunr 10:34ccb2fed2ef 246 } else if (P2 < -24) {
arunr 10:34ccb2fed2ef 247 motor2_direction = 0;
arunr 10:34ccb2fed2ef 248 motor2_speed = 0.1;
arunr 0:65ab9f79a4cc 249 }
arunr 0:65ab9f79a4cc 250 }
arunr 0:65ab9f79a4cc 251
arunr 10:34ccb2fed2ef 252 void HIDScope ()
arunr 10:34ccb2fed2ef 253 {
arunr 10:34ccb2fed2ef 254 scope.set (0, final_filter1);
arunr 10:34ccb2fed2ef 255 scope.set (1, final_filter2);
arunr 10:34ccb2fed2ef 256 scope.set (2, final_filter3);
arunr 10:34ccb2fed2ef 257 scope.set (3, final_filter4);
arunr 10:34ccb2fed2ef 258 scope.set (4, motor1.getPosition());
arunr 10:34ccb2fed2ef 259 scope.set (5, motor2.getPosition());
arunr 6:1597888c9a56 260 scope.send ();
arunr 10:34ccb2fed2ef 261 }
arunr 10:34ccb2fed2ef 262
arunr 0:65ab9f79a4cc 263 int main()
arunr 10:34ccb2fed2ef 264 {
RichellBooyink 13:4f4cc1a5a79a 265 safety_stop = true;
arunr 0:65ab9f79a4cc 266 while (true) {
riannebulthuis 8:b219ca30967f 267 pc.baud(9600); //pc baud rate van de computer
riannebulthuis 14:25edcf2935f6 268 EMG_Filter.attach_us(Filters, 5e3); //filters uitvoeren
riannebulthuis 14:25edcf2935f6 269 Scope.attach_us(HIDScope, 5e3); //EMG en encoder signaal naar de hidscope sturen
arunr 10:34ccb2fed2ef 270
arunr 10:34ccb2fed2ef 271 switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
arunr 10:34ccb2fed2ef 272
arunr 10:34ccb2fed2ef 273 case HOME: { //positie op 0 zetten voor arm 1
arunr 10:34ccb2fed2ef 274 pc.printf("home\n\r");
arunr 10:34ccb2fed2ef 275 H1 = motor1.getPosition();
arunr 10:34ccb2fed2ef 276 H2 = motor2.getPosition();
arunr 10:34ccb2fed2ef 277 pc.printf("Home-position is %f\n\r", H1);
arunr 10:34ccb2fed2ef 278 pc.printf("Home-pso is %f\n\r", H2);
RichellBooyink 12:146ba6f95fe0 279 state = MOVE_MOTORS;
arunr 10:34ccb2fed2ef 280 wait(2);
arunr 10:34ccb2fed2ef 281 break;
riannebulthuis 2:866a8a9f2b93 282 }
arunr 10:34ccb2fed2ef 283
RichellBooyink 12:146ba6f95fe0 284 case MOVE_MOTORS: { //motor kan cw en ccw bewegen a.d.h.v. buttons
arunr 10:34ccb2fed2ef 285 pc.printf("move_motor\n\r");
RichellBooyink 12:146ba6f95fe0 286 while (state == MOVE_MOTORS) {
arunr 10:34ccb2fed2ef 287 move_motor1();
RichellBooyink 12:146ba6f95fe0 288 move_motor2();
arunr 10:34ccb2fed2ef 289 if (button_1 == pressed && button_2 == pressed) {
arunr 10:34ccb2fed2ef 290 motor1_speed = 0;
arunr 10:34ccb2fed2ef 291 motor2_speed = 0;
arunr 10:34ccb2fed2ef 292 state = BACKTOHOMEPOSITION;
arunr 10:34ccb2fed2ef 293 }
arunr 10:34ccb2fed2ef 294 }
arunr 10:34ccb2fed2ef 295 wait (1);
riannebulthuis 3:5f59cbe53d7d 296 break;
RichellBooyink 12:146ba6f95fe0 297 }
arunr 10:34ccb2fed2ef 298
arunr 10:34ccb2fed2ef 299
RichellBooyink 12:146ba6f95fe0 300 case BACKTOHOMEPOSITION: { //motor gaat terug naar homeposition
RichellBooyink 12:146ba6f95fe0 301 pc.printf("backhomeposition\n\r");
RichellBooyink 12:146ba6f95fe0 302 wait (1);
arunr 10:34ccb2fed2ef 303
RichellBooyink 12:146ba6f95fe0 304 ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
RichellBooyink 12:146ba6f95fe0 305 while(state == BACKTOHOMEPOSITION) {
RichellBooyink 12:146ba6f95fe0 306 movetohome();
RichellBooyink 12:146ba6f95fe0 307 while(regelaar_ticker_flag != true);
RichellBooyink 12:146ba6f95fe0 308 regelaar_ticker_flag = false;
arunr 10:34ccb2fed2ef 309
RichellBooyink 12:146ba6f95fe0 310 pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 311 pc.printf("referentie %f, %f \n\r", H1, H2);
RichellBooyink 12:146ba6f95fe0 312
RichellBooyink 12:146ba6f95fe0 313 if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
RichellBooyink 12:146ba6f95fe0 314 pc.printf("motor1 pos %d", motor1.getPosition());
RichellBooyink 12:146ba6f95fe0 315 pc.printf("motor2 pos %d", motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 316 pc.printf("referentie %f %f\n\r", H1, H2);
RichellBooyink 12:146ba6f95fe0 317 state = STOP;
RichellBooyink 12:146ba6f95fe0 318 pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 319 break;
arunr 10:34ccb2fed2ef 320 }
arunr 10:34ccb2fed2ef 321 }
RichellBooyink 12:146ba6f95fe0 322 }
RichellBooyink 12:146ba6f95fe0 323 case STOP: {
RichellBooyink 12:146ba6f95fe0 324 static int c;
RichellBooyink 12:146ba6f95fe0 325 while(state == STOP) {
RichellBooyink 12:146ba6f95fe0 326 motor1_speed = 0;
RichellBooyink 12:146ba6f95fe0 327 motor2_speed = 0;
RichellBooyink 12:146ba6f95fe0 328 if (c++ == 0) {
RichellBooyink 12:146ba6f95fe0 329 pc.printf("einde\n\r");
arunr 10:34ccb2fed2ef 330 }
riannebulthuis 2:866a8a9f2b93 331 }
RichellBooyink 12:146ba6f95fe0 332 break;
riannebulthuis 3:5f59cbe53d7d 333 }
riannebulthuis 3:5f59cbe53d7d 334 }
arunr 10:34ccb2fed2ef 335 }
RichellBooyink 12:146ba6f95fe0 336 }