Aukie Hooglugt / Mbed 2 deprecated PROJECT_FINAL_VERSLAG

verslag

Dependencies:   Encoder HIDScope MODSERIAL- mbed-dsp mbed

Fork of PROJECT_FINAL by Aukie Hooglugt

PROJECT_main.cpp@28:1e3fe7204529, 2014-11-04 (annotated)

Committer:
Hooglugt
Date:
Tue Nov 04 11:36:05 2014 +0000
Revision:
28:1e3fe7204529
Parent:
27:e33c352d6bb2 
final script, printfs eruit gehaald (pc (usbetc)) zit er nog in

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hooglugt 0:99cbc87af37c 1 #include "mbed.h"
Hooglugt 0:99cbc87af37c 2 #include "MODSERIAL.h"
Hooglugt 0:99cbc87af37c 3 #include "HIDScope.h"
Hooglugt 0:99cbc87af37c 4 #include "arm_math.h"
Hooglugt 0:99cbc87af37c 5 #include "encoder.h"
Hooglugt 0:99cbc87af37c 6
Hooglugt 0:99cbc87af37c 7 #define TSAMP 0.001 // sample freq encoder motor
Hooglugt 0:99cbc87af37c 8 #define TIMEB4NEXTCHOICE 1 // sec keuzelampje blijft aan
Hooglugt 1:d44a866de64f 9 #define TIMEBETWEENBLINK 100 // sec voor volgende blink
Hooglugt 0:99cbc87af37c 10 #define TSAMP_EMG 0.002 //sample frequency emg
Hooglugt 7:ca1ade91bd14 11 #define KALIBRATIONTIME 500 // 10 sec voor bepalen van maximale biceps/triceps waarde
Hooglugt 1:d44a866de64f 12 #define FACTOR 0.6 //factor*max_waarde = threshold emg
Hooglugt 7:ca1ade91bd14 13
Hooglugt 0:99cbc87af37c 14 //Define objects
Hooglugt 7:ca1ade91bd14 15
Hooglugt 0:99cbc87af37c 16 AnalogIn emg0(PTB1); //Analog input biceps
Hooglugt 0:99cbc87af37c 17 AnalogIn emg1(PTB2); //Analog input triceps
Hooglugt 0:99cbc87af37c 18
Hooglugt 0:99cbc87af37c 19 Ticker log_timer; //sample emg
Hooglugt 1:d44a866de64f 20 Ticker blink; //ledjes aan/uit
Hooglugt 1:d44a866de64f 21 Ticker blink2; //extra tikker zodat kalbi en kaltri tegelijkertijd aankunnen
Hooglugt 0:99cbc87af37c 22 Ticker looptimer; //motor regelaar
Hooglugt 0:99cbc87af37c 23
Hooglugt 0:99cbc87af37c 24 MODSERIAL pc(USBTX,USBRX);
Hooglugt 0:99cbc87af37c 25
Hooglugt 0:99cbc87af37c 26 arm_biquad_casd_df1_inst_f32 bihighpass;
Hooglugt 0:99cbc87af37c 27 float bihighpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
Hooglugt 0:99cbc87af37c 28 float bihighpass_states[4];
Hooglugt 0:99cbc87af37c 29
Hooglugt 0:99cbc87af37c 30 arm_biquad_casd_df1_inst_f32 binotch;
Hooglugt 0:99cbc87af37c 31 float binotch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
Hooglugt 0:99cbc87af37c 32 float binotch_states[4];
Hooglugt 0:99cbc87af37c 33
Hooglugt 0:99cbc87af37c 34 arm_biquad_casd_df1_inst_f32 trihighpass;
Hooglugt 0:99cbc87af37c 35 float trihighpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
Hooglugt 0:99cbc87af37c 36 float trihighpass_states[4];
Hooglugt 0:99cbc87af37c 37
Hooglugt 0:99cbc87af37c 38 arm_biquad_casd_df1_inst_f32 trinotch;
Hooglugt 0:99cbc87af37c 39 float trinotch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
Hooglugt 0:99cbc87af37c 40 float trinotch_states[4];
Hooglugt 0:99cbc87af37c 41
Hooglugt 0:99cbc87af37c 42 float bi_result = 0;
Hooglugt 0:99cbc87af37c 43 float tri_result = 0;
Hooglugt 0:99cbc87af37c 44
Hooglugt 0:99cbc87af37c 45 float bi_max = 0;
Hooglugt 0:99cbc87af37c 46 float tri_max = 0;
Hooglugt 0:99cbc87af37c 47
Hooglugt 0:99cbc87af37c 48 // variables for biceps MAF
Hooglugt 0:99cbc87af37c 49 float y0 = 0;
Hooglugt 0:99cbc87af37c 50 float y1 = 0;
Hooglugt 0:99cbc87af37c 51 float y2 = 0;
Hooglugt 0:99cbc87af37c 52 float y3 = 0;
Hooglugt 0:99cbc87af37c 53 float y4 = 0;
Hooglugt 0:99cbc87af37c 54 float y5 = 0;
Hooglugt 0:99cbc87af37c 55 float y6 = 0;
Hooglugt 0:99cbc87af37c 56 float y7 = 0;
Hooglugt 0:99cbc87af37c 57 float y8 = 0;
Hooglugt 0:99cbc87af37c 58 float y9 = 0;
Hooglugt 0:99cbc87af37c 59
Hooglugt 0:99cbc87af37c 60 // variables for triceps MAF
Hooglugt 0:99cbc87af37c 61 float x0 = 0;
Hooglugt 0:99cbc87af37c 62 float x1 = 0;
Hooglugt 0:99cbc87af37c 63 float x2 = 0;
Hooglugt 0:99cbc87af37c 64 float x3 = 0;
Hooglugt 0:99cbc87af37c 65 float x4 = 0;
Hooglugt 0:99cbc87af37c 66 float x5 = 0;
Hooglugt 0:99cbc87af37c 67 float x6 = 0;
Hooglugt 0:99cbc87af37c 68 float x7 = 0;
Hooglugt 0:99cbc87af37c 69 float x8 = 0;
Hooglugt 0:99cbc87af37c 70 float x9 = 0;
Hooglugt 0:99cbc87af37c 71
Hooglugt 0:99cbc87af37c 72 //LED interface
Hooglugt 0:99cbc87af37c 73 DigitalOut dir1(PTA1);
Hooglugt 0:99cbc87af37c 74 DigitalOut dir2(PTA2);
Hooglugt 0:99cbc87af37c 75 DigitalOut dir3(PTD4);
Hooglugt 0:99cbc87af37c 76 DigitalOut for1(PTA12);
Hooglugt 0:99cbc87af37c 77 DigitalOut for2(PTA13);
Hooglugt 0:99cbc87af37c 78 DigitalOut for3(PTD1);
Hooglugt 0:99cbc87af37c 79
Hooglugt 0:99cbc87af37c 80 uint8_t direction = 0;
Hooglugt 0:99cbc87af37c 81 uint8_t force = 0;
Hooglugt 0:99cbc87af37c 82
Hooglugt 0:99cbc87af37c 83 //motorcontrol objects
Hooglugt 0:99cbc87af37c 84
Hooglugt 0:99cbc87af37c 85 //motor 1, voltage pins op M2
Hooglugt 0:99cbc87af37c 86 Encoder motor1(PTD3, PTD5);
Hooglugt 0:99cbc87af37c 87 DigitalOut motor1dir(PTC9);
Hooglugt 0:99cbc87af37c 88 PwmOut pwm_motor1(PTC8);
Hooglugt 0:99cbc87af37c 89
Hooglugt 0:99cbc87af37c 90 //motor 2, voltage pins op M1
Hooglugt 0:99cbc87af37c 91 Encoder motor2(PTD2,PTD0);
Hooglugt 0:99cbc87af37c 92 DigitalOut motor2dir(PTA4);
Hooglugt 0:99cbc87af37c 93 PwmOut pwm_motor2(PTA5);
Hooglugt 0:99cbc87af37c 94
Hooglugt 0:99cbc87af37c 95 float integral = 0;
Hooglugt 7:ca1ade91bd14 96 float derivative = 0;
Hooglugt 4:697d5a806cc4 97 float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
Hooglugt 4:697d5a806cc4 98 float balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
Hooglugt 26:8d91c4c54bb6 99 float kalibratie = 0;
Hooglugt 0:99cbc87af37c 100 float controlerror = 0;
Hooglugt 6:14051758db6f 101 float previouserror = 0;
Hooglugt 0:99cbc87af37c 102 float pwm = 0;
Hooglugt 0:99cbc87af37c 103
Hooglugt 8:75980dc35763 104 float pwm1 =0;
Hooglugt 8:75980dc35763 105 float integral1 = 0;
Hooglugt 8:75980dc35763 106 float derivative1 = 0;
Hooglugt 8:75980dc35763 107 float controlerror1 = 0;
Hooglugt 8:75980dc35763 108 float previouserror1 = 0;
Hooglugt 8:75980dc35763 109
Hooglugt 8:75980dc35763 110 int state = 1;
Hooglugt 9:0bc7f83b761e 111 int count = 0;
Hooglugt 9:0bc7f83b761e 112 float angle = 0;
Hooglugt 8:75980dc35763 113
Hooglugt 0:99cbc87af37c 114 float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
Hooglugt 8:75980dc35763 115 float omrekenfactor2 = 0.0015213178; // 6.28/(24*172);
Hooglugt 0:99cbc87af37c 116
Hooglugt 1:d44a866de64f 117 float setpoint1 = 0; //te behalen speed van motor1 (37D)
Hooglugt 1:d44a866de64f 118 float setpoint2 = 0; //te behalen hoek van motor2 (25D)
Hooglugt 0:99cbc87af37c 119
Hooglugt 18:5467bcc5cbf5 120 float Kp1 = 2.0; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
Hooglugt 9:0bc7f83b761e 121 float Ki1 = 0.0; //Kp en Ki van motor1, voor de slag
Hooglugt 6:14051758db6f 122 float Kd1 = 0.0;
Hooglugt 6:14051758db6f 123
Hooglugt 22:77d7065cc431 124 float Kp2 = 0.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return
Hooglugt 20:5007ddcd03ef 125 float Ki2 = 0.0; //0.30 en 0.20
Hooglugt 9:0bc7f83b761e 126 float Kd2 = 0.0;
Hooglugt 0:99cbc87af37c 127
Hooglugt 0:99cbc87af37c 128 volatile bool looptimerflag; //voor motorcontrol TSAMP
Hooglugt 0:99cbc87af37c 129
Hooglugt 0:99cbc87af37c 130 //functies
Hooglugt 0:99cbc87af37c 131
Hooglugt 0:99cbc87af37c 132 void setlooptimerflag(void)
Hooglugt 0:99cbc87af37c 133 {
Hooglugt 0:99cbc87af37c 134 looptimerflag = true;
Hooglugt 10:6bf3e25f020a 135 }
Hooglugt 10:6bf3e25f020a 136
Hooglugt 0:99cbc87af37c 137 void keep_in_range(float * in, float min, float max)
Hooglugt 0:99cbc87af37c 138 {
Hooglugt 0:99cbc87af37c 139 *in > min ? *in < max? : *in = max: *in = max;
Hooglugt 0:99cbc87af37c 140 }
Hooglugt 0:99cbc87af37c 141
Hooglugt 0:99cbc87af37c 142 void looper()
Hooglugt 0:99cbc87af37c 143 {
Hooglugt 0:99cbc87af37c 144 //put raw emg value of biceps and triceps in emg_biceps and emg_triceps, respectively
Hooglugt 0:99cbc87af37c 145 float emg_biceps; //Float voor EMG-waarde biceps
Hooglugt 0:99cbc87af37c 146 float emg_triceps; //Float voor EMG-waarde triceps
Hooglugt 0:99cbc87af37c 147
Hooglugt 0:99cbc87af37c 148 emg_biceps = emg0.read(); // read float value (0..1 = 0..3.3V) biceps
Hooglugt 0:99cbc87af37c 149 emg_triceps = emg1.read(); // read float value (0..1 = 0..3.3V) triceps
Hooglugt 0:99cbc87af37c 150
Hooglugt 0:99cbc87af37c 151 //process emg biceps
Hooglugt 0:99cbc87af37c 152 arm_biquad_cascade_df1_f32(&bihighpass, &emg_biceps, &emg_biceps, 1 );
Hooglugt 0:99cbc87af37c 153 arm_biquad_cascade_df1_f32(&binotch, &emg_biceps, &emg_biceps, 1 );
Hooglugt 0:99cbc87af37c 154 y0 = fabs(emg_biceps);
Hooglugt 0:99cbc87af37c 155 bi_result = (y0*0.1 +y1*0.1 + y2*0.1 + y3*0.1 + y4*0.1 + y5*0.1 + y6*0.1 + y7*0.1 + y8*0.1 + y9*0.1);
Hooglugt 0:99cbc87af37c 156 y9=y8;
Hooglugt 0:99cbc87af37c 157 y8=y7;
Hooglugt 0:99cbc87af37c 158 y7=y6;
Hooglugt 0:99cbc87af37c 159 y6=y5;
Hooglugt 0:99cbc87af37c 160 y5=y4;
Hooglugt 0:99cbc87af37c 161 y4=y3;
Hooglugt 0:99cbc87af37c 162 y3=y2;
Hooglugt 0:99cbc87af37c 163 y2=y1;
Hooglugt 0:99cbc87af37c 164 y1=y0;
Hooglugt 0:99cbc87af37c 165
Hooglugt 0:99cbc87af37c 166 //process emg triceps
Hooglugt 0:99cbc87af37c 167 arm_biquad_cascade_df1_f32(&trihighpass, &emg_triceps, &emg_triceps, 1 );
Hooglugt 0:99cbc87af37c 168 arm_biquad_cascade_df1_f32(&trinotch, &emg_triceps, &emg_triceps, 1 );
Hooglugt 0:99cbc87af37c 169 x0 = fabs(emg_triceps);
Hooglugt 0:99cbc87af37c 170 tri_result = (x0*0.1 +x1*0.1 + x2*0.1 + x3*0.1 + x4*0.1 + x5*0.1 + x6*0.1 + x7*0.1 + x8*0.1 + x9*0.1);
Hooglugt 0:99cbc87af37c 171 x9=x8;
Hooglugt 0:99cbc87af37c 172 x8=x7;
Hooglugt 0:99cbc87af37c 173 x7=x6;
Hooglugt 0:99cbc87af37c 174 x6=x5;
Hooglugt 0:99cbc87af37c 175 x5=x4;
Hooglugt 0:99cbc87af37c 176 x4=x3;
Hooglugt 0:99cbc87af37c 177 x3=x2;
Hooglugt 0:99cbc87af37c 178 x2=x1;
Hooglugt 0:99cbc87af37c 179 x1=x0;
Hooglugt 0:99cbc87af37c 180 }
Hooglugt 0:99cbc87af37c 181
Hooglugt 0:99cbc87af37c 182 void kalbi() //blinking three lights, first row - 2nd row unlit
Hooglugt 0:99cbc87af37c 183 {
Hooglugt 0:99cbc87af37c 184 if(dir1==0) {
Hooglugt 0:99cbc87af37c 185 dir1 = dir2 = dir3 = 1;
Hooglugt 0:99cbc87af37c 186 } else {
Hooglugt 0:99cbc87af37c 187 dir1 = dir2 = dir3 = 0;
Hooglugt 0:99cbc87af37c 188 }
Hooglugt 0:99cbc87af37c 189 }
Hooglugt 0:99cbc87af37c 190
Hooglugt 0:99cbc87af37c 191 void kaltri() //blinking three lights, 2nd row - first row lit
Hooglugt 0:99cbc87af37c 192 {
Hooglugt 0:99cbc87af37c 193 if(for1==0) {
Hooglugt 0:99cbc87af37c 194 for1 = for2 = for3 = 1;
Hooglugt 0:99cbc87af37c 195 } else {
Hooglugt 0:99cbc87af37c 196 for1 = for2 = for3 = 0;
Hooglugt 0:99cbc87af37c 197 }
Hooglugt 0:99cbc87af37c 198 }
Hooglugt 0:99cbc87af37c 199
Hooglugt 0:99cbc87af37c 200 void okay() //blinking the two lights you have chosen (misschien is hier een betere manier van coderen voor :P)
Hooglugt 0:99cbc87af37c 201 {
Hooglugt 0:99cbc87af37c 202 if(direction == 1 && force == 1) { // links zwak
Hooglugt 0:99cbc87af37c 203 if(for1 == 0 && dir1 == 0) {
Hooglugt 0:99cbc87af37c 204 for1 = dir1 = 1;
Hooglugt 0:99cbc87af37c 205 } else {
Hooglugt 0:99cbc87af37c 206 for1 = dir1 = 0;
Hooglugt 0:99cbc87af37c 207 }
Hooglugt 0:99cbc87af37c 208 }
Hooglugt 0:99cbc87af37c 209 if(direction == 1 && force == 2) { // links normaal
Hooglugt 0:99cbc87af37c 210 if(for2 == 0 && dir1 == 0) {
Hooglugt 0:99cbc87af37c 211 for2 = dir1 = 1;
Hooglugt 0:99cbc87af37c 212 } else {
Hooglugt 0:99cbc87af37c 213 for2 = dir1 = 0;
Hooglugt 0:99cbc87af37c 214 }
Hooglugt 0:99cbc87af37c 215 }
Hooglugt 0:99cbc87af37c 216 if(direction == 1 && force == 3) { // links sterk
Hooglugt 0:99cbc87af37c 217 if(for3 == 0 && dir1 == 0) {
Hooglugt 0:99cbc87af37c 218 for3 = dir1 = 1;
Hooglugt 0:99cbc87af37c 219 } else {
Hooglugt 0:99cbc87af37c 220 for3 = dir1 = 0;
Hooglugt 0:99cbc87af37c 221 }
Hooglugt 0:99cbc87af37c 222 }
Hooglugt 0:99cbc87af37c 223 if(direction == 2 && force == 1) { // mid zwak
Hooglugt 0:99cbc87af37c 224 if(for1 == 0 && dir2 == 0) {
Hooglugt 0:99cbc87af37c 225 for1 = dir2 = 1;
Hooglugt 0:99cbc87af37c 226 } else {
Hooglugt 0:99cbc87af37c 227 for1 = dir2 = 0;
Hooglugt 0:99cbc87af37c 228 }
Hooglugt 0:99cbc87af37c 229 }
Hooglugt 0:99cbc87af37c 230 if(direction == 2 && force == 2) { // mid normaal
Hooglugt 0:99cbc87af37c 231 if(for2 == 0 && dir2 == 0) {
Hooglugt 0:99cbc87af37c 232 for2 = dir2 = 1;
Hooglugt 0:99cbc87af37c 233 } else {
Hooglugt 0:99cbc87af37c 234 for2 = dir2 = 0;
Hooglugt 0:99cbc87af37c 235 }
Hooglugt 0:99cbc87af37c 236 }
Hooglugt 0:99cbc87af37c 237 if(direction == 2 && force == 3) { // mid sterk
Hooglugt 0:99cbc87af37c 238 if(for3 == 0 && dir2 == 0) {
Hooglugt 0:99cbc87af37c 239 for3 = dir2 = 1;
Hooglugt 0:99cbc87af37c 240 } else {
Hooglugt 0:99cbc87af37c 241 for3 = dir2 = 0;
Hooglugt 0:99cbc87af37c 242 }
Hooglugt 0:99cbc87af37c 243 }
Hooglugt 0:99cbc87af37c 244 if(direction == 3 && force == 1) { // rechts zwak
Hooglugt 0:99cbc87af37c 245 if(for1 == 0 && dir3 == 0) {
Hooglugt 0:99cbc87af37c 246 for1 = dir3 = 1;
Hooglugt 0:99cbc87af37c 247 } else {
Hooglugt 0:99cbc87af37c 248 for1 = dir3 = 0;
Hooglugt 0:99cbc87af37c 249 }
Hooglugt 0:99cbc87af37c 250 }
Hooglugt 0:99cbc87af37c 251 if(direction == 3 && force == 2) { // rechts normaal
Hooglugt 0:99cbc87af37c 252 if(for2 == 0 && dir3 == 0) {
Hooglugt 0:99cbc87af37c 253 for2 = dir3 = 1;
Hooglugt 0:99cbc87af37c 254 } else {
Hooglugt 0:99cbc87af37c 255 for2 = dir3 = 0;
Hooglugt 0:99cbc87af37c 256 }
Hooglugt 0:99cbc87af37c 257 }
Hooglugt 0:99cbc87af37c 258 if(direction == 3 && force == 3) { // rechts sterk
Hooglugt 0:99cbc87af37c 259 if(for3 == 0 && dir3 == 0) {
Hooglugt 0:99cbc87af37c 260 for3 = dir3 = 1;
Hooglugt 0:99cbc87af37c 261 } else {
Hooglugt 0:99cbc87af37c 262 for3 = dir3 = 0;
Hooglugt 0:99cbc87af37c 263 }
Hooglugt 0:99cbc87af37c 264 }
Hooglugt 0:99cbc87af37c 265 }
Hooglugt 0:99cbc87af37c 266
Hooglugt 0:99cbc87af37c 267 int main()
Hooglugt 0:99cbc87af37c 268 {
Hooglugt 0:99cbc87af37c 269 pc.baud(115200); //baudrate instellen
Hooglugt 0:99cbc87af37c 270 log_timer.attach(looper, TSAMP_EMG); //EMG, Fsample 500 Hz
Hooglugt 0:99cbc87af37c 271 looptimer.attach(setlooptimerflag,TSAMP);
Hooglugt 0:99cbc87af37c 272 pwm_motor1.period_us(100); //10kHz PWM frequency
Hooglugt 0:99cbc87af37c 273 pwm_motor2.period_us(100); //10kHz PWM frequency
Hooglugt 0:99cbc87af37c 274
Hooglugt 0:99cbc87af37c 275 //set up filters
Hooglugt 0:99cbc87af37c 276 arm_biquad_cascade_df1_init_f32(&binotch, 1, binotch_const, binotch_states);
Hooglugt 0:99cbc87af37c 277 arm_biquad_cascade_df1_init_f32(&bihighpass, 1, bihighpass_const, bihighpass_states);
Hooglugt 0:99cbc87af37c 278
Hooglugt 0:99cbc87af37c 279 arm_biquad_cascade_df1_init_f32(&trinotch, 1, trinotch_const, trinotch_states);
Hooglugt 0:99cbc87af37c 280 arm_biquad_cascade_df1_init_f32(&trihighpass, 1, trihighpass_const, trihighpass_states);
Hooglugt 0:99cbc87af37c 281
Hooglugt 0:99cbc87af37c 282 //kalibratie
Hooglugt 0:99cbc87af37c 283
Hooglugt 18:5467bcc5cbf5 284 motor2cal:
Hooglugt 18:5467bcc5cbf5 285
Hooglugt 0:99cbc87af37c 286 //motorarm naar nul-positie
Hooglugt 20:5007ddcd03ef 287 blink.attach(kalbi, 0.4);
Hooglugt 1:d44a866de64f 288 blink2.attach(kaltri, 0.2);
Hooglugt 0:99cbc87af37c 289
Hooglugt 0:99cbc87af37c 290 //calibration motor 2
Hooglugt 7:ca1ade91bd14 291 pwm_motor2.write(0.65); //lage PWM
Hooglugt 5:e5ca53305b87 292 motor2dir = 0; //rechtsom
Hooglugt 0:99cbc87af37c 293 wait(1); // anders wordt de while(1) meteen onderbroken
Hooglugt 4:697d5a806cc4 294 while(1) {
Hooglugt 5:e5ca53305b87 295 if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // motor2.getSpeed()*omrekenfactor2 > -0.70), 0.70 is nog aan te passen
Hooglugt 0:99cbc87af37c 296 pwm_motor2.write(0);
Hooglugt 4:697d5a806cc4 297 motor2.setPosition(0);
Hooglugt 4:697d5a806cc4 298 goto motor1cal;
Hooglugt 0:99cbc87af37c 299 }
Hooglugt 0:99cbc87af37c 300 wait(0.01);
Hooglugt 0:99cbc87af37c 301 }
Hooglugt 4:697d5a806cc4 302 motor1cal:
Hooglugt 0:99cbc87af37c 303 //calibration motor 1
Hooglugt 7:ca1ade91bd14 304 pwm_motor1.write(0.65); //lage PWM
Hooglugt 7:ca1ade91bd14 305 motor1dir = 0; //linksom
Hooglugt 0:99cbc87af37c 306 wait(1); // anders wordt de while(1) meteen onderbroken
Hooglugt 4:697d5a806cc4 307 while(1) {
Hooglugt 8:75980dc35763 308 if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // motor1.getSpeed()*omrekenfactor1 < 0.20, 0.20 is nog aan te passen
Hooglugt 0:99cbc87af37c 309 pwm_motor1.write(0);
Hooglugt 4:697d5a806cc4 310 motor1.setPosition(0);
Hooglugt 4:697d5a806cc4 311 goto emgcal;
Hooglugt 0:99cbc87af37c 312 }
Hooglugt 0:99cbc87af37c 313 wait(0.01);
Hooglugt 0:99cbc87af37c 314 }
Hooglugt 4:697d5a806cc4 315 emgcal:
Hooglugt 0:99cbc87af37c 316 blink.detach();
Hooglugt 1:d44a866de64f 317 blink2.detach();
Hooglugt 0:99cbc87af37c 318 dir1 = dir2 = dir3 = 1;
Hooglugt 4:697d5a806cc4 319 for1 = for2 = for3 = 1;
Hooglugt 0:99cbc87af37c 320 wait (1);
Hooglugt 0:99cbc87af37c 321 for1 = for2 = for3 = 0;
Hooglugt 0:99cbc87af37c 322
Hooglugt 8:75980dc35763 323 if(kalibratie==0) {
Hooglugt 8:75980dc35763 324 //biceps kalibratie
Hooglugt 8:75980dc35763 325 blink.attach(kalbi, 0.2);
Hooglugt 8:75980dc35763 326 for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
Hooglugt 8:75980dc35763 327 if (bi_max < bi_result) {
Hooglugt 8:75980dc35763 328 bi_max = bi_result;
Hooglugt 8:75980dc35763 329 }
Hooglugt 8:75980dc35763 330 wait (0.01);
Hooglugt 0:99cbc87af37c 331 }
Hooglugt 8:75980dc35763 332 blink.detach();
Hooglugt 8:75980dc35763 333 dir1 = dir2 = dir3 = 1;
Hooglugt 8:75980dc35763 334 wait (1);
Hooglugt 0:99cbc87af37c 335
Hooglugt 8:75980dc35763 336 //triceps kalibratie
Hooglugt 8:75980dc35763 337 blink.attach(kaltri, 0.2);
Hooglugt 8:75980dc35763 338 for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
Hooglugt 8:75980dc35763 339 if (tri_max < tri_result) {
Hooglugt 8:75980dc35763 340 tri_max = tri_result;
Hooglugt 8:75980dc35763 341 }
Hooglugt 8:75980dc35763 342 wait (0.01);
Hooglugt 0:99cbc87af37c 343 }
Hooglugt 8:75980dc35763 344 blink.detach();
Hooglugt 8:75980dc35763 345 for1 = for2 = for3 = 1;
Hooglugt 8:75980dc35763 346 wait (1);
Hooglugt 8:75980dc35763 347 for1 = for2 = for3 = 0;
Hooglugt 8:75980dc35763 348 kalibratie = 1;
Hooglugt 0:99cbc87af37c 349 }
Hooglugt 0:99cbc87af37c 350
Hooglugt 4:697d5a806cc4 351 directionchoice:
Hooglugt 1:d44a866de64f 352 log_timer.attach(looper, TSAMP_EMG);
Hooglugt 0:99cbc87af37c 353
Hooglugt 4:697d5a806cc4 354 while(1) { //Loop keuze DIRECTION
Hooglugt 0:99cbc87af37c 355 for(int i=1; i<4; i++) {
Hooglugt 0:99cbc87af37c 356 if(i==1) { //red
Hooglugt 0:99cbc87af37c 357 dir1=1;
Hooglugt 0:99cbc87af37c 358 dir2=0;
Hooglugt 0:99cbc87af37c 359 dir3=0;
Hooglugt 0:99cbc87af37c 360 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 361 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 362 direction = 1;
Hooglugt 0:99cbc87af37c 363 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 364 goto forcechoice; // goes to second while(1) for the deciding the force
Hooglugt 0:99cbc87af37c 365 } else {
Hooglugt 0:99cbc87af37c 366 wait(0.01);
Hooglugt 0:99cbc87af37c 367 }
Hooglugt 0:99cbc87af37c 368 }
Hooglugt 0:99cbc87af37c 369 }
Hooglugt 0:99cbc87af37c 370 if(i==2) { //green
Hooglugt 0:99cbc87af37c 371 dir1 =0;
Hooglugt 0:99cbc87af37c 372 dir2 =1;
Hooglugt 0:99cbc87af37c 373 dir3 =0;
Hooglugt 0:99cbc87af37c 374 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 375 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 376 direction = 2;
Hooglugt 0:99cbc87af37c 377 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 378 goto forcechoice;
Hooglugt 0:99cbc87af37c 379 } else {
Hooglugt 0:99cbc87af37c 380 wait(0.01);
Hooglugt 0:99cbc87af37c 381 }
Hooglugt 0:99cbc87af37c 382 }
Hooglugt 0:99cbc87af37c 383 }
Hooglugt 0:99cbc87af37c 384 if(i==3) { //blue
Hooglugt 0:99cbc87af37c 385 dir1 =0;
Hooglugt 0:99cbc87af37c 386 dir2 =0;
Hooglugt 0:99cbc87af37c 387 dir3 =1;
Hooglugt 0:99cbc87af37c 388 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 389 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 390 direction = 3;
Hooglugt 0:99cbc87af37c 391 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 392 goto forcechoice;
Hooglugt 0:99cbc87af37c 393 } else {
Hooglugt 0:99cbc87af37c 394 wait(0.01);
Hooglugt 0:99cbc87af37c 395 }
Hooglugt 0:99cbc87af37c 396 }
Hooglugt 0:99cbc87af37c 397 }
Hooglugt 0:99cbc87af37c 398 }
Hooglugt 0:99cbc87af37c 399 }
Hooglugt 4:697d5a806cc4 400 forcechoice:
Hooglugt 4:697d5a806cc4 401 while(1) { //Loop keuze FORCE
Hooglugt 0:99cbc87af37c 402 for(int j=1; j<4; j++) {
Hooglugt 0:99cbc87af37c 403 if(j==1) { //red
Hooglugt 0:99cbc87af37c 404 for1=1;
Hooglugt 0:99cbc87af37c 405 for2=0;
Hooglugt 0:99cbc87af37c 406 for3=0;
Hooglugt 0:99cbc87af37c 407 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 408 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 409 for1 = for2 = for3 = 0;
Hooglugt 0:99cbc87af37c 410 goto directionchoice;
Hooglugt 0:99cbc87af37c 411 } else {
Hooglugt 1:d44a866de64f 412 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 413 force = 1;
Hooglugt 0:99cbc87af37c 414 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 415 goto choicesmade;
Hooglugt 0:99cbc87af37c 416 } else {
Hooglugt 0:99cbc87af37c 417 wait(0.01);
Hooglugt 0:99cbc87af37c 418 }
Hooglugt 0:99cbc87af37c 419 }
Hooglugt 0:99cbc87af37c 420 }
Hooglugt 0:99cbc87af37c 421 }
Hooglugt 0:99cbc87af37c 422 if(j==2) { //green
Hooglugt 0:99cbc87af37c 423 for1=0;
Hooglugt 0:99cbc87af37c 424 for2=1;
Hooglugt 0:99cbc87af37c 425 for3=0;
Hooglugt 0:99cbc87af37c 426 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 427 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 428 for1 = for2 = for3 = 0;
Hooglugt 0:99cbc87af37c 429 goto directionchoice;
Hooglugt 0:99cbc87af37c 430 } else {
Hooglugt 1:d44a866de64f 431 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 432 force = 2;
Hooglugt 0:99cbc87af37c 433 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 434 goto choicesmade;
Hooglugt 0:99cbc87af37c 435 } else {
Hooglugt 0:99cbc87af37c 436 wait(0.01);
Hooglugt 0:99cbc87af37c 437 }
Hooglugt 0:99cbc87af37c 438 }
Hooglugt 0:99cbc87af37c 439 }
Hooglugt 0:99cbc87af37c 440 }
Hooglugt 0:99cbc87af37c 441 if(j==3) { //blue
Hooglugt 0:99cbc87af37c 442 for1=0;
Hooglugt 0:99cbc87af37c 443 for2=0;
Hooglugt 0:99cbc87af37c 444 for3=1;
Hooglugt 0:99cbc87af37c 445 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
Hooglugt 1:d44a866de64f 446 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 447 for1 = for2 = for3 = 0;
Hooglugt 0:99cbc87af37c 448 goto directionchoice;
Hooglugt 0:99cbc87af37c 449 } else {
Hooglugt 1:d44a866de64f 450 if(bi_result>FACTOR*bi_max) {
Hooglugt 0:99cbc87af37c 451 force = 3;
Hooglugt 0:99cbc87af37c 452 wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
Hooglugt 0:99cbc87af37c 453 goto choicesmade;
Hooglugt 0:99cbc87af37c 454 } else {
Hooglugt 0:99cbc87af37c 455 wait(0.01);
Hooglugt 0:99cbc87af37c 456 }
Hooglugt 0:99cbc87af37c 457 }
Hooglugt 0:99cbc87af37c 458 }
Hooglugt 0:99cbc87af37c 459 }
Hooglugt 0:99cbc87af37c 460 }
Hooglugt 0:99cbc87af37c 461 }
Hooglugt 0:99cbc87af37c 462
Hooglugt 0:99cbc87af37c 463 choicesmade:
Hooglugt 0:99cbc87af37c 464 blink.attach(okay, 0.2);
Hooglugt 4:697d5a806cc4 465 while(1) {
Hooglugt 4:697d5a806cc4 466 if(tri_result>FACTOR*tri_max) {
Hooglugt 0:99cbc87af37c 467 blink.detach();
Hooglugt 1:d44a866de64f 468 switch (direction) {
Hooglugt 1:d44a866de64f 469 case 1:
Hooglugt 1:d44a866de64f 470 dir1 = 1;
Hooglugt 1:d44a866de64f 471 for1 = 1;
Hooglugt 1:d44a866de64f 472 for2 = for3 = 0;
Hooglugt 1:d44a866de64f 473 break;
Hooglugt 1:d44a866de64f 474 case 2:
Hooglugt 1:d44a866de64f 475 dir2 = 1;
Hooglugt 1:d44a866de64f 476 for1 = 1;
Hooglugt 1:d44a866de64f 477 for2 = for3 = 0;
Hooglugt 1:d44a866de64f 478 break;
Hooglugt 1:d44a866de64f 479 case 3:
Hooglugt 1:d44a866de64f 480 dir3 = 1;
Hooglugt 1:d44a866de64f 481 for1 = 1;
Hooglugt 1:d44a866de64f 482 for2 = for3 = 0;
Hooglugt 1:d44a866de64f 483 break;
Hooglugt 1:d44a866de64f 484 }
Hooglugt 4:697d5a806cc4 485
Hooglugt 4:697d5a806cc4 486 wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze
Hooglugt 4:697d5a806cc4 487 goto forcechoice;
Hooglugt 0:99cbc87af37c 488 } else {
Hooglugt 1:d44a866de64f 489 if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) {
Hooglugt 4:697d5a806cc4 490 blink.detach();
Hooglugt 4:697d5a806cc4 491 log_timer.detach();
Hooglugt 4:697d5a806cc4 492 goto motorcontrol;
Hooglugt 0:99cbc87af37c 493 } else {
Hooglugt 0:99cbc87af37c 494 wait(0.01); // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje)
Hooglugt 0:99cbc87af37c 495 }
Hooglugt 0:99cbc87af37c 496 }
Hooglugt 0:99cbc87af37c 497 }
Hooglugt 4:697d5a806cc4 498
Hooglugt 4:697d5a806cc4 499 motorcontrol:
Hooglugt 4:697d5a806cc4 500
Hooglugt 0:99cbc87af37c 501 /* Vanaf hier komt de aansturing van de motor */
Hooglugt 0:99cbc87af37c 502
Hooglugt 8:75980dc35763 503 setpoint1=0;
Hooglugt 8:75980dc35763 504 setpoint2=0;
Hooglugt 8:75980dc35763 505 integral1 = integral = 0;
Hooglugt 8:75980dc35763 506 previouserror1 = previouserror = 0;
Hooglugt 11:b517e73a98ab 507
Hooglugt 11:b517e73a98ab 508
Hooglugt 8:75980dc35763 509 while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
Hooglugt 11:b517e73a98ab 510 while(!looptimerflag);
Hooglugt 9:0bc7f83b761e 511 looptimerflag = false; //clear flag
Hooglugt 11:b517e73a98ab 512
Hooglugt 11:b517e73a98ab 513 switch(state) {
Hooglugt 25:1ec0da9b26a5 514 case 1:
Hooglugt 11:b517e73a98ab 515 switch (direction) {
Hooglugt 11:b517e73a98ab 516 case 1:
Hooglugt 22:77d7065cc431 517 pwm_motor2.write(0.8);
Hooglugt 22:77d7065cc431 518 motor2dir = 1;
Hooglugt 22:77d7065cc431 519 count++;
Hooglugt 25:1ec0da9b26a5 520 if (count>170) {
Hooglugt 22:77d7065cc431 521 state=2;
Hooglugt 22:77d7065cc431 522 count = 0;
Hooglugt 22:77d7065cc431 523 pwm_motor2.write(0);
Hooglugt 22:77d7065cc431 524 }
Hooglugt 11:b517e73a98ab 525 break;
Hooglugt 25:1ec0da9b26a5 526
Hooglugt 11:b517e73a98ab 527 case 2:
Hooglugt 22:77d7065cc431 528 pwm_motor2.write(0.8);
Hooglugt 22:77d7065cc431 529 motor2dir = 1;
Hooglugt 22:77d7065cc431 530 count++;
Hooglugt 22:77d7065cc431 531 if (count>100) {
Hooglugt 22:77d7065cc431 532 state=2;
Hooglugt 22:77d7065cc431 533 count = 0;
Hooglugt 22:77d7065cc431 534 pwm_motor2.write(0);
Hooglugt 22:77d7065cc431 535 }
Hooglugt 11:b517e73a98ab 536 break;
Hooglugt 11:b517e73a98ab 537 case 3:
Hooglugt 23:913bf8a6f7d8 538 pwm_motor2.write(0.8);
Hooglugt 23:913bf8a6f7d8 539 motor2dir = 1;
Hooglugt 23:913bf8a6f7d8 540 count++;
Hooglugt 23:913bf8a6f7d8 541 if (count>50) {
Hooglugt 23:913bf8a6f7d8 542 state=2;
Hooglugt 23:913bf8a6f7d8 543 count = 0;
Hooglugt 23:913bf8a6f7d8 544 pwm_motor2.write(0);
Hooglugt 23:913bf8a6f7d8 545 }
Hooglugt 19:fe284ddaa88a 546 break;
Hooglugt 23:913bf8a6f7d8 547 }
Hooglugt 17:7641d7934b91 548 case 2: {
Hooglugt 11:b517e73a98ab 549 count++;
Hooglugt 11:b517e73a98ab 550 if(count>1000) {
Hooglugt 11:b517e73a98ab 551 count = 0;
Hooglugt 11:b517e73a98ab 552 state = 3;
Hooglugt 11:b517e73a98ab 553 }
Hooglugt 11:b517e73a98ab 554 break;
Hooglugt 9:0bc7f83b761e 555 }
Hooglugt 11:b517e73a98ab 556 case 3: {
Hooglugt 11:b517e73a98ab 557 switch (force) {
Hooglugt 11:b517e73a98ab 558 case 1:
Hooglugt 25:1ec0da9b26a5 559 pwm_motor1.write(1);
Hooglugt 25:1ec0da9b26a5 560 motor1dir = 1;
Hooglugt 11:b517e73a98ab 561 break;
Hooglugt 11:b517e73a98ab 562 case 2:
Hooglugt 25:1ec0da9b26a5 563 pwm_motor1.write(1); //net niet haalbaar
Hooglugt 25:1ec0da9b26a5 564 motor1dir = 1;
Hooglugt 11:b517e73a98ab 565 break;
Hooglugt 11:b517e73a98ab 566 case 3:
Hooglugt 25:1ec0da9b26a5 567 pwm_motor1.write(1); //niet haalbaar
Hooglugt 25:1ec0da9b26a5 568 motor1dir = 1;
Hooglugt 11:b517e73a98ab 569 break;
Hooglugt 11:b517e73a98ab 570 }
Hooglugt 25:1ec0da9b26a5 571 if(fabs(motor1.getPosition()*omrekenfactor1)>7.0) {
Hooglugt 25:1ec0da9b26a5 572 pwm_motor1.write(0);
Hooglugt 11:b517e73a98ab 573 state = 4;
Hooglugt 11:b517e73a98ab 574 }
Hooglugt 11:b517e73a98ab 575 break;
Hooglugt 8:75980dc35763 576 }
Hooglugt 11:b517e73a98ab 577 case 4: {
Hooglugt 11:b517e73a98ab 578 count++;
Hooglugt 25:1ec0da9b26a5 579 if(count>4000) {
Hooglugt 11:b517e73a98ab 580 count = 0;
Hooglugt 11:b517e73a98ab 581 state = 1;
Hooglugt 18:5467bcc5cbf5 582 goto motor2cal;
Hooglugt 11:b517e73a98ab 583 }
Hooglugt 11:b517e73a98ab 584 break;
Hooglugt 24:1b812b393264 585 }
Hooglugt 11:b517e73a98ab 586 }
Hooglugt 25:1ec0da9b26a5 587 }
Hooglugt 11:b517e73a98ab 588 } // end main