Arun Raveenthiran / Mbed 2 deprecated VerslagCode

Opgeschoonde code voor verslag

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of TotalCodegr13V2 by Rianne Bulthuis

main.cpp@20:3424ef411538, 2015-11-02 (annotated)

Committer:
arunr
Date:
Mon Nov 02 15:45:52 2015 +0000
Revision:
20:3424ef411538
Parent:
19:aa7e88a631ad 
Toverstaf

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