Arun Raveenthiran / Mbed 2 deprecated VerslagCode

Opgeschoonde code voor verslag

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of TotalCodegr13V2 by Rianne Bulthuis

main.cpp@17:416876824d8c, 2015-11-02 (annotated)

Committer:
arunr
Date:
Mon Nov 02 14:24:46 2015 +0000
Revision:
17:416876824d8c
Parent:
16:bf76ed65da88
Child:
18:60251896da8d
Move to home opgesplitst in 2 functies voor m1 en m2. Werkt nu voor beide motoren.

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 17:416876824d8c 230 void movetohome1()
arunr 10:34ccb2fed2ef 231 {
arunr 10:34ccb2fed2ef 232 P1 = motor1.getPosition();
arunr 10:34ccb2fed2ef 233
arunr 17:416876824d8c 234 if ((P1 > -25 && P1 < 25)) {
arunr 0:65ab9f79a4cc 235 motor1_speed = 0;
arunr 10:34ccb2fed2ef 236 } else if (P1 > 24) {
riannebulthuis 3:5f59cbe53d7d 237 motor1_direction = 1;
riannebulthuis 3:5f59cbe53d7d 238 motor1_speed = 0.1;
arunr 10:34ccb2fed2ef 239 } else if (P1 < -24) {
riannebulthuis 3:5f59cbe53d7d 240 motor1_direction = 0;
riannebulthuis 3:5f59cbe53d7d 241 motor1_speed = 0.1;
arunr 17:416876824d8c 242 }
arunr 17:416876824d8c 243 }
arunr 17:416876824d8c 244
arunr 17:416876824d8c 245 void movetohome2()
arunr 17:416876824d8c 246 {
arunr 17:416876824d8c 247 P2 = motor2.getPosition();
arunr 17:416876824d8c 248 if (P2 > -25 && P2 < 25){
arunr 17:416876824d8c 249 motor2_speed = 0;
arunr 17:416876824d8c 250 }else if (P2 > 24) {
arunr 17:416876824d8c 251 motor2_direction = 0;
arunr 10:34ccb2fed2ef 252 motor2_speed = 0.1;
arunr 10:34ccb2fed2ef 253 } else if (P2 < -24) {
arunr 17:416876824d8c 254 motor2_direction = 1;
arunr 10:34ccb2fed2ef 255 motor2_speed = 0.1;
arunr 0:65ab9f79a4cc 256 }
arunr 0:65ab9f79a4cc 257 }
arunr 0:65ab9f79a4cc 258
arunr 10:34ccb2fed2ef 259 void HIDScope ()
arunr 10:34ccb2fed2ef 260 {
arunr 10:34ccb2fed2ef 261 scope.set (0, final_filter1);
arunr 10:34ccb2fed2ef 262 scope.set (1, final_filter2);
arunr 10:34ccb2fed2ef 263 scope.set (2, final_filter3);
arunr 10:34ccb2fed2ef 264 scope.set (3, final_filter4);
arunr 10:34ccb2fed2ef 265 scope.set (4, motor1.getPosition());
arunr 10:34ccb2fed2ef 266 scope.set (5, motor2.getPosition());
arunr 6:1597888c9a56 267 scope.send ();
arunr 10:34ccb2fed2ef 268 }
arunr 10:34ccb2fed2ef 269
arunr 0:65ab9f79a4cc 270 int main()
arunr 10:34ccb2fed2ef 271 {
RichellBooyink 13:4f4cc1a5a79a 272 safety_stop = true;
arunr 0:65ab9f79a4cc 273 while (true) {
riannebulthuis 8:b219ca30967f 274 pc.baud(9600); //pc baud rate van de computer
riannebulthuis 14:25edcf2935f6 275 EMG_Filter.attach_us(Filters, 5e3); //filters uitvoeren
riannebulthuis 14:25edcf2935f6 276 Scope.attach_us(HIDScope, 5e3); //EMG en encoder signaal naar de hidscope sturen
arunr 10:34ccb2fed2ef 277
arunr 10:34ccb2fed2ef 278 switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases
arunr 10:34ccb2fed2ef 279
arunr 10:34ccb2fed2ef 280 case HOME: { //positie op 0 zetten voor arm 1
arunr 10:34ccb2fed2ef 281 pc.printf("home\n\r");
arunr 10:34ccb2fed2ef 282 H1 = motor1.getPosition();
arunr 10:34ccb2fed2ef 283 H2 = motor2.getPosition();
arunr 10:34ccb2fed2ef 284 pc.printf("Home-position is %f\n\r", H1);
arunr 10:34ccb2fed2ef 285 pc.printf("Home-pso is %f\n\r", H2);
RichellBooyink 12:146ba6f95fe0 286 state = MOVE_MOTORS;
arunr 10:34ccb2fed2ef 287 wait(2);
arunr 10:34ccb2fed2ef 288 break;
riannebulthuis 2:866a8a9f2b93 289 }
arunr 10:34ccb2fed2ef 290
RichellBooyink 12:146ba6f95fe0 291 case MOVE_MOTORS: { //motor kan cw en ccw bewegen a.d.h.v. buttons
arunr 10:34ccb2fed2ef 292 pc.printf("move_motor\n\r");
RichellBooyink 12:146ba6f95fe0 293 while (state == MOVE_MOTORS) {
arunr 10:34ccb2fed2ef 294 move_motor1();
RichellBooyink 12:146ba6f95fe0 295 move_motor2();
arunr 10:34ccb2fed2ef 296 if (button_1 == pressed && button_2 == pressed) {
arunr 10:34ccb2fed2ef 297 motor1_speed = 0;
arunr 10:34ccb2fed2ef 298 motor2_speed = 0;
arunr 10:34ccb2fed2ef 299 state = BACKTOHOMEPOSITION;
arunr 10:34ccb2fed2ef 300 }
arunr 10:34ccb2fed2ef 301 }
arunr 10:34ccb2fed2ef 302 wait (1);
riannebulthuis 3:5f59cbe53d7d 303 break;
RichellBooyink 12:146ba6f95fe0 304 }
arunr 10:34ccb2fed2ef 305
arunr 10:34ccb2fed2ef 306
RichellBooyink 12:146ba6f95fe0 307 case BACKTOHOMEPOSITION: { //motor gaat terug naar homeposition
RichellBooyink 12:146ba6f95fe0 308 pc.printf("backhomeposition\n\r");
RichellBooyink 12:146ba6f95fe0 309 wait (1);
arunr 10:34ccb2fed2ef 310
RichellBooyink 12:146ba6f95fe0 311 ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
RichellBooyink 12:146ba6f95fe0 312 while(state == BACKTOHOMEPOSITION) {
arunr 17:416876824d8c 313 movetohome1();
arunr 17:416876824d8c 314 movetohome2();
RichellBooyink 12:146ba6f95fe0 315 while(regelaar_ticker_flag != true);
RichellBooyink 12:146ba6f95fe0 316 regelaar_ticker_flag = false;
arunr 10:34ccb2fed2ef 317
RichellBooyink 12:146ba6f95fe0 318 pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 319 pc.printf("referentie %f, %f \n\r", H1, H2);
RichellBooyink 12:146ba6f95fe0 320
RichellBooyink 12:146ba6f95fe0 321 if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
RichellBooyink 12:146ba6f95fe0 322 pc.printf("motor1 pos %d", motor1.getPosition());
RichellBooyink 12:146ba6f95fe0 323 pc.printf("motor2 pos %d", motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 324 pc.printf("referentie %f %f\n\r", H1, H2);
RichellBooyink 12:146ba6f95fe0 325 state = STOP;
RichellBooyink 12:146ba6f95fe0 326 pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
RichellBooyink 12:146ba6f95fe0 327 break;
arunr 10:34ccb2fed2ef 328 }
arunr 10:34ccb2fed2ef 329 }
RichellBooyink 12:146ba6f95fe0 330 }
RichellBooyink 12:146ba6f95fe0 331 case STOP: {
RichellBooyink 12:146ba6f95fe0 332 static int c;
RichellBooyink 12:146ba6f95fe0 333 while(state == STOP) {
RichellBooyink 12:146ba6f95fe0 334 motor1_speed = 0;
RichellBooyink 12:146ba6f95fe0 335 motor2_speed = 0;
RichellBooyink 12:146ba6f95fe0 336 if (c++ == 0) {
RichellBooyink 12:146ba6f95fe0 337 pc.printf("einde\n\r");
arunr 10:34ccb2fed2ef 338 }
riannebulthuis 2:866a8a9f2b93 339 }
RichellBooyink 12:146ba6f95fe0 340 break;
riannebulthuis 3:5f59cbe53d7d 341 }
riannebulthuis 3:5f59cbe53d7d 342 }
arunr 10:34ccb2fed2ef 343 }
RichellBooyink 12:146ba6f95fe0 344 }