Arun Raveenthiran / Mbed 2 deprecated VerslagCode

Opgeschoonde code voor verslag

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of TotalCodegr13V2 by Rianne Bulthuis

main.cpp@18:60251896da8d, 2015-11-02 (annotated)

Committer:
arunr
Date:
Mon Nov 02 14:54:54 2015 +0000
Revision:
18:60251896da8d
Parent:
17:416876824d8c
Child:
19:aa7e88a631ad
homepos werkt niet meer;

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