Iris van Leeuwen / Mbed 2 deprecated Lampje_EMG_Gr6

2 losse EMG signalen van de biceps en deltoid

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed Encoder

Fork of Lampje_EMG_Gr6 by Jesse Kaiser

main.cpp@27:691779624530, 2014-11-01 (annotated)

Committer:
irisl
Date:
Sat Nov 01 13:43:21 2014 +0000
Revision:
27:691779624530
Parent:
26:9859a71456fd
Child:
28:c33a0658605e
beide motoren werken nu goed. Er zijn wat fouten uit het script gehaald. NU moet de hoek goed bepaald worden

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jessekaiser 0:db396b9f4b4c 1 #include "mbed.h"
jessekaiser 0:db396b9f4b4c 2 #include "HIDScope.h"
jessekaiser 0:db396b9f4b4c 3 #include "arm_math.h"
jessekaiser 0:db396b9f4b4c 4 #include "MODSERIAL.h"
irisl 20:d40b6cba4280 5 #include "encoder.h"
irisl 21:674fafb6301d 6 #include "PwmOut.h"
jessekaiser 0:db396b9f4b4c 7
irisl 21:674fafb6301d 8 #define TSAMP 0.005
irisl 21:674fafb6301d 9 #define K_P1 (3.5) //voor motor1 is het 3.5
irisl 21:674fafb6301d 10 #define K_I1 (0.01 *TSAMP) //voor motor1 is het 0.01
irisl 23:94b5746e52f5 11 #define K_P2 (0.7)
irisl 21:674fafb6301d 12 #define K_I2 (0.01 *TSAMP)
irisl 20:d40b6cba4280 13 #define I_LIMIT 1.
irisl 21:674fafb6301d 14 //#define PI 3.14159265
irisl 21:674fafb6301d 15 #define l_arm 0.5
irisl 20:d40b6cba4280 16
irisl 20:d40b6cba4280 17 #define M1_PWM PTC8
irisl 20:d40b6cba4280 18 #define M1_DIR PTC9
irisl 20:d40b6cba4280 19 #define M2_PWM PTA5
irisl 20:d40b6cba4280 20 #define M2_DIR PTA4
irisl 20:d40b6cba4280 21
irisl 20:d40b6cba4280 22 //Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting
irisl 20:d40b6cba4280 23
irisl 20:d40b6cba4280 24 Serial pc(USBTX, USBRX);
irisl 20:d40b6cba4280 25
irisl 21:674fafb6301d 26 DigitalOut myledred(PTB3);
irisl 21:674fafb6301d 27 DigitalOut myledgreen(PTB1);
irisl 21:674fafb6301d 28 DigitalOut myledblue(PTB2);
jessekaiser 0:db396b9f4b4c 29
jessekaiser 0:db396b9f4b4c 30 //Define objects
irisl 21:674fafb6301d 31 AnalogIn emg0(PTB0); //Analog input
irisl 21:674fafb6301d 32 AnalogIn emg1(PTC2); //Analog input
irisl 25:a12d51345aa4 33 HIDScope scope(4);
jessekaiser 0:db396b9f4b4c 34
irisl 20:d40b6cba4280 35 //motor 25D
irisl 21:674fafb6301d 36 Encoder motor1(PTD3,PTD5); //wit, geel
irisl 20:d40b6cba4280 37 PwmOut pwm_motor1(M2_PWM);
irisl 20:d40b6cba4280 38 DigitalOut motordir1(M2_DIR);
irisl 20:d40b6cba4280 39
irisl 20:d40b6cba4280 40 //motor2 37D
irisl 21:674fafb6301d 41 Encoder motor2(PTD2, PTD0); //wit, geel
irisl 20:d40b6cba4280 42 PwmOut pwm_motor2(M1_PWM);
irisl 20:d40b6cba4280 43 DigitalOut motordir2(M1_DIR);
irisl 20:d40b6cba4280 44
irisl 21:674fafb6301d 45 float pwm1_percentage = 0;
irisl 21:674fafb6301d 46 float pwm2_percentage = 0;
irisl 21:674fafb6301d 47 int cur_pos_motor1;
irisl 21:674fafb6301d 48 int prev_pos_motor1 = 0;
irisl 21:674fafb6301d 49 int cur_pos_motor2;
irisl 21:674fafb6301d 50 int prev_pos_motor2 = 0;
irisl 21:674fafb6301d 51 float speed1_rad;
irisl 21:674fafb6301d 52 float speed2_rad;
irisl 21:674fafb6301d 53 float pos_motor1_rad;
irisl 21:674fafb6301d 54 float pos_motor2_rad;
irisl 21:674fafb6301d 55 int staat1 = 0;
irisl 21:674fafb6301d 56 int staat2 = 0;
irisl 22:14f5161d7d7b 57 volatile float arm_hoogte = 0;
irisl 22:14f5161d7d7b 58 volatile float batje_hoek = 0;
irisl 21:674fafb6301d 59 int wait_iterator1 = 0;
irisl 21:674fafb6301d 60 int wait_iterator2 = 0;
irisl 20:d40b6cba4280 61
irisl 20:d40b6cba4280 62
irisl 20:d40b6cba4280 63 // EMG
irisl 20:d40b6cba4280 64
irisl 9:f7ec578a17c0 65 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
irisl 9:f7ec578a17c0 66 arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
irisl 26:9859a71456fd 67 //lowpass filter settings: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB
irisl 26:9859a71456fd 68 float lowpass_const[] = {0.00015514839749793376, 0.00031029679499586753, 0.00015514839749793376, 1.9644602512795832, -0.9650808448695751};
jessekaiser 0:db396b9f4b4c 69 //state values
irisl 9:f7ec578a17c0 70 float lowpass_biceps_states[4];
irisl 9:f7ec578a17c0 71 float lowpass_deltoid_states[4];
irisl 9:f7ec578a17c0 72 arm_biquad_casd_df1_inst_f32 highnotch_biceps;
irisl 9:f7ec578a17c0 73 arm_biquad_casd_df1_inst_f32 highnotch_deltoid;
irisl 27:691779624530 74 //highpass filter settings: Fc = 10 Hz, Fs = 500 Hz, Gain = -3 dB, notch Fc = 50, Fs =500Hz, Gain = -3 dB
irisl 26:9859a71456fd 75 float highnotch_const[] = {0.9149684297741606, -1.8299368595483212, 0.9149684297741606, 1.8226935021735358, -0.8371802169231065 ,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
jessekaiser 0:db396b9f4b4c 76 //state values
irisl 9:f7ec578a17c0 77 float highnotch_biceps_states[8];
irisl 9:f7ec578a17c0 78 float highnotch_deltoid_states[8];
jessekaiser 2:39e1bde54e73 79
jessekaiser 2:39e1bde54e73 80 //De globale variabele voor het gefilterde EMG signaal
jessekaiser 2:39e1bde54e73 81 float filtered_biceps;
jessekaiser 2:39e1bde54e73 82 float filtered_deltoid;
irisl 12:9e6e49af9304 83 float filtered_average_bi;
irisl 12:9e6e49af9304 84 float filtered_average_del;
jessekaiser 0:db396b9f4b4c 85
jessekaiser 0:db396b9f4b4c 86
irisl 12:9e6e49af9304 87 void average_biceps(float filtered_biceps,float *average)
irisl 12:9e6e49af9304 88 {
irisl 12:9e6e49af9304 89 static float total=0;
irisl 12:9e6e49af9304 90 static float number=0;
irisl 12:9e6e49af9304 91 total = total + filtered_biceps;
irisl 12:9e6e49af9304 92 number = number + 1;
irisl 26:9859a71456fd 93 if ( number == 250) {
irisl 26:9859a71456fd 94 *average = total/250;
irisl 12:9e6e49af9304 95 total = 0;
irisl 12:9e6e49af9304 96 number = 0;
irisl 12:9e6e49af9304 97 }
irisl 12:9e6e49af9304 98 }
irisl 12:9e6e49af9304 99
irisl 12:9e6e49af9304 100 void average_deltoid(float filtered_input,float *average_output)
irisl 12:9e6e49af9304 101 {
irisl 12:9e6e49af9304 102 static float total=0;
irisl 12:9e6e49af9304 103 static float number=0;
irisl 12:9e6e49af9304 104 total = total + filtered_input;
irisl 12:9e6e49af9304 105 number = number + 1;
irisl 26:9859a71456fd 106 if ( number == 250) {
irisl 26:9859a71456fd 107 *average_output = total/250;
irisl 12:9e6e49af9304 108 total = 0;
irisl 12:9e6e49af9304 109 number = 0;
irisl 12:9e6e49af9304 110 }
irisl 12:9e6e49af9304 111 }
irisl 12:9e6e49af9304 112
jessekaiser 0:db396b9f4b4c 113 /** Looper function
jessekaiser 0:db396b9f4b4c 114 * functions used for Ticker and Timeout should be of type void <name>(void)
jessekaiser 0:db396b9f4b4c 115 * i.e. no input arguments, no output arguments.
jessekaiser 0:db396b9f4b4c 116 * if you want to change a variable that you use in other places (for example in main)
jessekaiser 0:db396b9f4b4c 117 * you will have to make that variable global in order to be able to reach it both from
jessekaiser 0:db396b9f4b4c 118 * the function called at interrupt time, and in the main function.
jessekaiser 0:db396b9f4b4c 119 * To make a variable global, define it under the includes.
jessekaiser 0:db396b9f4b4c 120 * variables that are changed in the interrupt routine (written to) should be made
jessekaiser 0:db396b9f4b4c 121 * 'volatile' to let the compiler know that those values may change outside the current context.
jessekaiser 0:db396b9f4b4c 122 * i.e.: "volatile uint16_t emg_value;" instead of "uint16_t emg_value"
jessekaiser 0:db396b9f4b4c 123 * in the example below, the variable is not re-used in the main function, and is thus declared
jessekaiser 0:db396b9f4b4c 124 * local in the looper function only.
jessekaiser 0:db396b9f4b4c 125 **/
jessekaiser 1:099b19376f16 126
jessekaiser 1:099b19376f16 127
jessekaiser 0:db396b9f4b4c 128 void looper()
jessekaiser 0:db396b9f4b4c 129 {
jessekaiser 1:099b19376f16 130 /*variable to store value in*/
jessekaiser 2:39e1bde54e73 131 uint16_t emg_value1;
jessekaiser 3:0895fa0a6ca4 132 uint16_t emg_value2;
jessekaiser 1:099b19376f16 133
jessekaiser 2:39e1bde54e73 134 float emg_value1_f32;
jessekaiser 2:39e1bde54e73 135 float emg_value2_f32;
jessekaiser 0:db396b9f4b4c 136 /*put raw emg value both in red and in emg_value*/
jessekaiser 2:39e1bde54e73 137 emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
jessekaiser 2:39e1bde54e73 138 emg_value1_f32 = emg0.read();
jessekaiser 3:0895fa0a6ca4 139
jessekaiser 2:39e1bde54e73 140 emg_value2 = emg1.read_u16();
jessekaiser 2:39e1bde54e73 141 emg_value2_f32 = emg1.read();
jessekaiser 0:db396b9f4b4c 142
jessekaiser 2:39e1bde54e73 143 //process emg biceps
irisl 9:f7ec578a17c0 144 arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 );
jessekaiser 2:39e1bde54e73 145 filtered_biceps = fabs(filtered_biceps);
irisl 9:f7ec578a17c0 146 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
irisl 12:9e6e49af9304 147 average_biceps(filtered_biceps,&filtered_average_bi);
jessekaiser 2:39e1bde54e73 148 //process emg deltoid
irisl 9:f7ec578a17c0 149 arm_biquad_cascade_df1_f32(&highnotch_deltoid, &emg_value2_f32, &filtered_deltoid, 1 );
jessekaiser 3:0895fa0a6ca4 150 filtered_deltoid = fabs(filtered_deltoid);
irisl 9:f7ec578a17c0 151 arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
irisl 12:9e6e49af9304 152 average_deltoid(filtered_deltoid, &filtered_average_del);
jessekaiser 1:099b19376f16 153
jessekaiser 0:db396b9f4b4c 154 /*send value to PC. */
irisl 9:f7ec578a17c0 155 //scope.set(0,emg_value1); //Raw EMG signal biceps
irisl 9:f7ec578a17c0 156 //scope.set(1,emg_value2); //Raw EMG signal Deltoid
irisl 25:a12d51345aa4 157 scope.set(0,filtered_biceps); //processed float biceps
irisl 25:a12d51345aa4 158 scope.set(1,filtered_average_bi); //processed float deltoid
irisl 25:a12d51345aa4 159 scope.set(2,filtered_deltoid); //processed float biceps
irisl 25:a12d51345aa4 160 scope.set(3,filtered_average_del); //processed float deltoid
jessekaiser 0:db396b9f4b4c 161 scope.send();
jessekaiser 0:db396b9f4b4c 162
jessekaiser 0:db396b9f4b4c 163 }
jessekaiser 0:db396b9f4b4c 164
irisl 16:d65458b85734 165 // LED AANSTURING
irisl 16:d65458b85734 166
irisl 16:d65458b85734 167 Ticker ledticker;
irisl 12:9e6e49af9304 168
irisl 12:9e6e49af9304 169
jessekaiser 0:db396b9f4b4c 170 void BlinkRed(int n)
jessekaiser 0:db396b9f4b4c 171 {
jessekaiser 0:db396b9f4b4c 172 for (int i=0; i<n; i++) {
irisl 21:674fafb6301d 173 myledred = 0;
irisl 21:674fafb6301d 174 myledgreen = 0;
irisl 21:674fafb6301d 175 myledblue = 0;
jessekaiser 3:0895fa0a6ca4 176 wait(0.1);
irisl 21:674fafb6301d 177 myledred = 1;
irisl 21:674fafb6301d 178 myledgreen = 0;
irisl 21:674fafb6301d 179 myledblue = 0;
jessekaiser 3:0895fa0a6ca4 180 wait(0.1);
jessekaiser 0:db396b9f4b4c 181 }
jessekaiser 0:db396b9f4b4c 182 }
jessekaiser 0:db396b9f4b4c 183
irisl 17:dbbe4e126203 184 void greenblink()
irisl 16:d65458b85734 185 {
irisl 21:674fafb6301d 186 if(myledgreen.read())
irisl 21:674fafb6301d 187 myledgreen = 0;
irisl 16:d65458b85734 188 else
irisl 21:674fafb6301d 189 myledgreen = 1;
irisl 16:d65458b85734 190 }
irisl 16:d65458b85734 191
irisl 17:dbbe4e126203 192 void BlinkGreen()
irisl 18:ed2afe6953de 193 {
irisl 21:674fafb6301d 194 myledred= 0;
irisl 21:674fafb6301d 195 myledblue =0;
irisl 18:ed2afe6953de 196 ledticker.attach(&greenblink,.5);
irisl 18:ed2afe6953de 197 /* myled1 = 1;
irisl 18:ed2afe6953de 198 myled2 = 1;
irisl 18:ed2afe6953de 199 myled3 = 1;
irisl 18:ed2afe6953de 200 wait(0.1);
irisl 18:ed2afe6953de 201 myled1 = 0;
irisl 18:ed2afe6953de 202 myled2 = 1;
irisl 18:ed2afe6953de 203 myled3 = 1;
irisl 18:ed2afe6953de 204 wait(0.1);*/
irisl 16:d65458b85734 205 }
irisl 16:d65458b85734 206
irisl 18:ed2afe6953de 207 void stopblinkgreen()
irisl 16:d65458b85734 208 {
irisl 16:d65458b85734 209 ledticker.detach();
irisl 16:d65458b85734 210 }
irisl 16:d65458b85734 211
irisl 16:d65458b85734 212
irisl 17:dbbe4e126203 213 void BlinkGreen1 ()
jessekaiser 3:0895fa0a6ca4 214 {
jessekaiser 3:0895fa0a6ca4 215
irisl 21:674fafb6301d 216 myledred = 0;
irisl 21:674fafb6301d 217 myledgreen = 0;
irisl 21:674fafb6301d 218 myledblue = 0;
jessekaiser 3:0895fa0a6ca4 219 wait(0.1);
irisl 21:674fafb6301d 220 myledred = 0;
irisl 21:674fafb6301d 221 myledgreen = 1;
irisl 21:674fafb6301d 222 myledblue = 0;
jessekaiser 3:0895fa0a6ca4 223 wait(0.1);
jessekaiser 3:0895fa0a6ca4 224 }
jessekaiser 3:0895fa0a6ca4 225
jessekaiser 3:0895fa0a6ca4 226
jessekaiser 3:0895fa0a6ca4 227 void BlinkBlue(int n)
jessekaiser 2:39e1bde54e73 228 {
jessekaiser 2:39e1bde54e73 229 for (int i=0; i<n; i++) {
irisl 21:674fafb6301d 230 myledred = 0;
irisl 21:674fafb6301d 231 myledgreen = 0;
irisl 21:674fafb6301d 232 myledblue = 0;
jessekaiser 3:0895fa0a6ca4 233 wait(0.1);
irisl 21:674fafb6301d 234 myledred = 0;
irisl 21:674fafb6301d 235 myledgreen = 0;
irisl 21:674fafb6301d 236 myledblue = 1;
jessekaiser 3:0895fa0a6ca4 237 wait(0.1);
jessekaiser 2:39e1bde54e73 238 }
jessekaiser 2:39e1bde54e73 239 }
jessekaiser 2:39e1bde54e73 240
irisl 13:493a953a2a85 241 void ShineGreen ()
irisl 13:493a953a2a85 242 {
irisl 21:674fafb6301d 243 myledred = 0;
irisl 21:674fafb6301d 244 myledgreen = 1;
irisl 21:674fafb6301d 245 myledblue = 0;
irisl 13:493a953a2a85 246 }
irisl 13:493a953a2a85 247
irisl 13:493a953a2a85 248 void ShineBlue ()
irisl 13:493a953a2a85 249 {
irisl 21:674fafb6301d 250 myledred = 0;
irisl 21:674fafb6301d 251 myledgreen = 0;
irisl 21:674fafb6301d 252 myledblue = 1;
irisl 13:493a953a2a85 253 }
irisl 13:493a953a2a85 254
irisl 13:493a953a2a85 255 void ShineRed ()
irisl 13:493a953a2a85 256 {
irisl 21:674fafb6301d 257 myledred = 1;
irisl 21:674fafb6301d 258 myledgreen = 0;
irisl 21:674fafb6301d 259 myledblue = 0;
irisl 13:493a953a2a85 260 }
irisl 13:493a953a2a85 261
irisl 20:d40b6cba4280 262 // MOTORFUNCTIES
irisl 20:d40b6cba4280 263
irisl 21:674fafb6301d 264 void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op
irisl 21:674fafb6301d 265 // maar de locatie van de variabele.
irisl 21:674fafb6301d 266 {
irisl 21:674fafb6301d 267 *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c
irisl 21:674fafb6301d 268 // *in = het getal dat staat op locatie van in --> waarde van new_pwm
irisl 21:674fafb6301d 269 }
irisl 21:674fafb6301d 270
irisl 21:674fafb6301d 271 float pid1(float setpoint1, float measurement1)
irisl 20:d40b6cba4280 272 {
irisl 21:674fafb6301d 273 float error1;
irisl 21:674fafb6301d 274 float out_p1 = 0;
irisl 21:674fafb6301d 275 static float out_i1 = 0;
irisl 21:674fafb6301d 276 error1 = (setpoint1 - measurement1);
irisl 21:674fafb6301d 277 out_p1 = error1*K_P1;
irisl 21:674fafb6301d 278 out_i1 += error1*K_I1;
irisl 21:674fafb6301d 279 clamp(&out_i1,-I_LIMIT,I_LIMIT);
irisl 21:674fafb6301d 280 return out_p1 + out_i1;
irisl 20:d40b6cba4280 281 }
irisl 20:d40b6cba4280 282
irisl 21:674fafb6301d 283 float pid2(float setpoint2, float measurement2)
irisl 20:d40b6cba4280 284 {
irisl 21:674fafb6301d 285 float error2;
irisl 21:674fafb6301d 286 float out_p2 = 0;
irisl 21:674fafb6301d 287 static float out_i2 = 0;
irisl 21:674fafb6301d 288 error2 = (setpoint2 - measurement2);
irisl 21:674fafb6301d 289 out_p2 = error2*K_P2;
irisl 21:674fafb6301d 290 out_i2 += error2*K_I2;
irisl 21:674fafb6301d 291 clamp(&out_i2,-I_LIMIT,I_LIMIT);
irisl 21:674fafb6301d 292 return out_p2 + out_i2;
irisl 21:674fafb6301d 293 }
irisl 21:674fafb6301d 294 float prev_setpoint1 = 0;
irisl 21:674fafb6301d 295 float setpoint1 = 0;
irisl 21:674fafb6301d 296 float prev_setpoint2 = 0;
irisl 21:674fafb6301d 297 float setpoint2 = 0;
irisl 21:674fafb6301d 298
irisl 21:674fafb6301d 299 void batje_links ()
irisl 21:674fafb6301d 300 {
irisl 21:674fafb6301d 301 speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 302 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 23:94b5746e52f5 303 if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden
irisl 23:94b5746e52f5 304 setpoint1 = (11.3*2.3*2.0*PI/360);
irisl 21:674fafb6301d 305 }
irisl 23:94b5746e52f5 306 if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
irisl 23:94b5746e52f5 307 setpoint1 = -(11.3*2.3*2.0*PI/360);
irisl 21:674fafb6301d 308 }
irisl 21:674fafb6301d 309 prev_setpoint1 = setpoint1;
irisl 20:d40b6cba4280 310 }
irisl 20:d40b6cba4280 311
irisl 21:674fafb6301d 312 void batje_rechts ()
irisl 20:d40b6cba4280 313 {
irisl 23:94b5746e52f5 314 speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 315 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 21:674fafb6301d 316 if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden
irisl 21:674fafb6301d 317 setpoint1 = (11.3*2.3*2.0*PI/360);
irisl 21:674fafb6301d 318 }
irisl 22:14f5161d7d7b 319 if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
irisl 22:14f5161d7d7b 320 setpoint1 = -(11.3*2.3*2.0*PI/360);
irisl 21:674fafb6301d 321 }
irisl 21:674fafb6301d 322 pwm_motor1.write(abs(pwm1_percentage));
irisl 21:674fafb6301d 323 prev_setpoint1 = setpoint1;
irisl 21:674fafb6301d 324 if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
irisl 21:674fafb6301d 325 staat1 = 1;
irisl 21:674fafb6301d 326 }
irisl 20:d40b6cba4280 327 }
irisl 20:d40b6cba4280 328
irisl 20:d40b6cba4280 329
irisl 21:674fafb6301d 330 void batje_begin_links ()
irisl 21:674fafb6301d 331 {
irisl 21:674fafb6301d 332 speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 333 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 22:14f5161d7d7b 334 if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden
irisl 22:14f5161d7d7b 335 setpoint1 = (0*2.3*2.0*PI/360);
irisl 21:674fafb6301d 336 }
irisl 22:14f5161d7d7b 337 if(setpoint1 < -(0*2.3*2.0*PI/360)) {
irisl 22:14f5161d7d7b 338 setpoint1 = -(0*2.3*2.0*PI/360);
irisl 21:674fafb6301d 339 }
irisl 21:674fafb6301d 340 prev_setpoint1 = setpoint1;
irisl 21:674fafb6301d 341 }
irisl 21:674fafb6301d 342
irisl 21:674fafb6301d 343 void batje_begin_rechts ()
irisl 21:674fafb6301d 344 {
irisl 23:94b5746e52f5 345 speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 346 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 22:14f5161d7d7b 347 if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden
irisl 22:14f5161d7d7b 348 setpoint1 = (0*2.3*2.0*PI/360);
irisl 21:674fafb6301d 349 }
irisl 21:674fafb6301d 350 if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
irisl 21:674fafb6301d 351 setpoint1 = -(0.0*2.3*2.0*PI/360);
irisl 21:674fafb6301d 352 }
irisl 21:674fafb6301d 353 prev_setpoint1 = setpoint1;
irisl 21:674fafb6301d 354 }
irisl 21:674fafb6301d 355
irisl 21:674fafb6301d 356 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
irisl 21:674fafb6301d 357 {
irisl 23:94b5746e52f5 358 speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 359 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 23:94b5746e52f5 360 if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
irisl 23:94b5746e52f5 361 setpoint2 = (155.0*2.0*PI/360);
irisl 21:674fafb6301d 362 }
irisl 23:94b5746e52f5 363 if(setpoint2 < -(155.0*2.0*PI/360)) {
irisl 23:94b5746e52f5 364 setpoint2 = -(155.0*2.0*PI/360);
irisl 21:674fafb6301d 365 }
irisl 21:674fafb6301d 366 prev_setpoint2 = setpoint2;
irisl 23:94b5746e52f5 367 if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
irisl 21:674fafb6301d 368 staat2 = 1;
irisl 21:674fafb6301d 369 }
irisl 21:674fafb6301d 370 }
irisl 21:674fafb6301d 371
irisl 21:674fafb6301d 372 void arm_mid ()
irisl 21:674fafb6301d 373 {
irisl 23:94b5746e52f5 374 speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 375 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 23:94b5746e52f5 376 if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
irisl 23:94b5746e52f5 377 setpoint2 = (155.0*2.0*PI/360);
irisl 21:674fafb6301d 378 }
irisl 23:94b5746e52f5 379 if(setpoint2 < -(155.0*2.0*PI/360)) {
irisl 23:94b5746e52f5 380 setpoint2 = -(155.0*2.0*PI/360);
irisl 21:674fafb6301d 381 }
irisl 21:674fafb6301d 382 prev_setpoint2 = setpoint2;
irisl 23:94b5746e52f5 383 if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
irisl 21:674fafb6301d 384 staat2 = 1;
irisl 21:674fafb6301d 385 }
irisl 21:674fafb6301d 386 }
irisl 21:674fafb6301d 387
irisl 21:674fafb6301d 388 void arm_laag ()
irisl 21:674fafb6301d 389 {
irisl 23:94b5746e52f5 390 speed2_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 391 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 23:94b5746e52f5 392 if(setpoint2 > (155*2.0*PI/360)) { //setpoint in graden
irisl 23:94b5746e52f5 393 setpoint2 = (155*2.0*PI/360);
irisl 21:674fafb6301d 394 }
irisl 23:94b5746e52f5 395 if(setpoint2 < -(155.0*2.0*PI/360)) {
irisl 23:94b5746e52f5 396 setpoint2 = -(155.0*2.0*PI/360);
irisl 21:674fafb6301d 397 }
irisl 21:674fafb6301d 398 prev_setpoint2 = setpoint2;
irisl 23:94b5746e52f5 399 if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
irisl 21:674fafb6301d 400 staat2 = 1;
irisl 21:674fafb6301d 401 }
irisl 21:674fafb6301d 402 }
irisl 21:674fafb6301d 403
irisl 21:674fafb6301d 404 void arm_begin ()
irisl 21:674fafb6301d 405 {
irisl 27:691779624530 406 speed2_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 407 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 21:674fafb6301d 408 if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden
irisl 21:674fafb6301d 409 setpoint2 = (0.0*2.0*PI/360);
irisl 21:674fafb6301d 410 }
irisl 21:674fafb6301d 411 if(setpoint2 < -(0.0*2.0*PI/360)) {
irisl 21:674fafb6301d 412 setpoint2 = -(0.0*2.0*PI/360);
irisl 21:674fafb6301d 413 }
irisl 21:674fafb6301d 414 prev_setpoint2 = setpoint2;
irisl 21:674fafb6301d 415 }
irisl 21:674fafb6301d 416
irisl 21:674fafb6301d 417 void looper_motor()
irisl 20:d40b6cba4280 418 {
irisl 21:674fafb6301d 419 //MOTOR1
irisl 21:674fafb6301d 420 pc.printf("%d \r\n", motor1.getPosition());
irisl 21:674fafb6301d 421 cur_pos_motor1 = motor1.getPosition();
irisl 21:674fafb6301d 422 pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //moet 4128
irisl 21:674fafb6301d 423 pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
irisl 21:674fafb6301d 424 if (pwm1_percentage < -1.0) {
irisl 21:674fafb6301d 425 pwm1_percentage = -1.0;
irisl 21:674fafb6301d 426 }
irisl 21:674fafb6301d 427 if (pwm1_percentage > 1.0) {
irisl 21:674fafb6301d 428 pwm1_percentage = 1.0;
irisl 21:674fafb6301d 429 }
irisl 21:674fafb6301d 430 pwm_motor1.write(abs(pwm1_percentage));
irisl 21:674fafb6301d 431 if(pwm1_percentage > 0) {
irisl 21:674fafb6301d 432 motordir1 = 0;
irisl 21:674fafb6301d 433 } else {
irisl 21:674fafb6301d 434 motordir1 = 1;
irisl 21:674fafb6301d 435 }
irisl 21:674fafb6301d 436
irisl 21:674fafb6301d 437 //MOTOR2
irisl 21:674fafb6301d 438 cur_pos_motor2 = motor2.getPosition();
irisl 21:674fafb6301d 439 pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
irisl 21:674fafb6301d 440 pwm2_percentage = pid2(setpoint2, pos_motor2_rad); //
irisl 21:674fafb6301d 441 if (pwm2_percentage < -1.0) {
irisl 21:674fafb6301d 442 pwm2_percentage = -1.0;
irisl 21:674fafb6301d 443 }
irisl 21:674fafb6301d 444 if (pwm2_percentage > 1.0) {
irisl 21:674fafb6301d 445 pwm2_percentage = 1.0;
irisl 21:674fafb6301d 446 }
irisl 21:674fafb6301d 447 pwm_motor2.write(abs(pwm2_percentage));
irisl 21:674fafb6301d 448 if(pwm2_percentage > 0) {
irisl 21:674fafb6301d 449 motordir2 = 0;
irisl 21:674fafb6301d 450 } else {
irisl 21:674fafb6301d 451 motordir2 = 1;
irisl 21:674fafb6301d 452 }
irisl 21:674fafb6301d 453
irisl 21:674fafb6301d 454
irisl 21:674fafb6301d 455 //STATES
irisl 21:674fafb6301d 456
irisl 22:14f5161d7d7b 457 if (batje_hoek == 1) {
irisl 21:674fafb6301d 458 if(staat1 == 0) {
irisl 21:674fafb6301d 459 batje_rechts();
irisl 21:674fafb6301d 460 wait_iterator1 = 0;
irisl 21:674fafb6301d 461 } else if(staat1 ==1) {
irisl 21:674fafb6301d 462 wait_iterator1++;
irisl 27:691779624530 463 if(wait_iterator1 > 400) {
irisl 21:674fafb6301d 464 staat1 = 2;
irisl 27:691779624530 465
irisl 27:691779624530 466 batje_begin_rechts();
irisl 27:691779624530 467 }
irisl 21:674fafb6301d 468 }
irisl 21:674fafb6301d 469 }
irisl 22:14f5161d7d7b 470 if (batje_hoek == 2) {
irisl 21:674fafb6301d 471 if(staat1 == 0) {
irisl 21:674fafb6301d 472 batje_links();
irisl 21:674fafb6301d 473 wait_iterator1 = 0;
irisl 21:674fafb6301d 474 } else if(staat1 ==1) {
irisl 21:674fafb6301d 475 wait_iterator1++;
irisl 27:691779624530 476 if(wait_iterator1 > 400) {
irisl 21:674fafb6301d 477 staat1 = 2;
irisl 27:691779624530 478
irisl 27:691779624530 479 batje_begin_links ();
irisl 27:691779624530 480 }
irisl 21:674fafb6301d 481 }
irisl 21:674fafb6301d 482 }
irisl 21:674fafb6301d 483
irisl 22:14f5161d7d7b 484 if(arm_hoogte == 1) {
irisl 21:674fafb6301d 485 if(staat2 == 0) {
irisl 21:674fafb6301d 486 arm_laag();
irisl 21:674fafb6301d 487 wait_iterator2 = 0;
irisl 21:674fafb6301d 488 } else if(staat2 == 1) {
irisl 21:674fafb6301d 489 wait_iterator2++;
irisl 27:691779624530 490 if(wait_iterator2 > 400) {
irisl 21:674fafb6301d 491 staat2 = 2;
irisl 27:691779624530 492
irisl 27:691779624530 493 arm_begin();
irisl 27:691779624530 494 }
irisl 21:674fafb6301d 495 }
irisl 21:674fafb6301d 496 }
irisl 22:14f5161d7d7b 497 if(arm_hoogte == 2) {
irisl 21:674fafb6301d 498 if(staat2 == 0) {
irisl 21:674fafb6301d 499 arm_mid();
irisl 21:674fafb6301d 500 wait_iterator2 = 0;
irisl 21:674fafb6301d 501 } else if(staat2 == 1) {
irisl 21:674fafb6301d 502 wait_iterator2++;
irisl 27:691779624530 503 if(wait_iterator2 > 400) {
irisl 21:674fafb6301d 504 staat2 = 2;
irisl 27:691779624530 505
irisl 27:691779624530 506 arm_begin();
irisl 27:691779624530 507 }
irisl 21:674fafb6301d 508 }
irisl 21:674fafb6301d 509 }
irisl 22:14f5161d7d7b 510 if(arm_hoogte == 3) {
irisl 21:674fafb6301d 511 if(staat2 == 0) {
irisl 21:674fafb6301d 512 arm_hoog();
irisl 21:674fafb6301d 513 wait_iterator2 = 0;
irisl 21:674fafb6301d 514 } else if(staat2 == 1) {
irisl 21:674fafb6301d 515 wait_iterator2++;
irisl 27:691779624530 516 if(wait_iterator2 > 400) {
irisl 21:674fafb6301d 517 staat2 = 2;
irisl 27:691779624530 518
irisl 27:691779624530 519 arm_begin();
irisl 27:691779624530 520 }
irisl 21:674fafb6301d 521 }
irisl 21:674fafb6301d 522 }
irisl 27:691779624530 523
irisl 20:d40b6cba4280 524 }
irisl 15:eb6498bb7ca0 525
irisl 27:691779624530 526
irisl 16:d65458b85734 527 int main()
jessekaiser 1:099b19376f16 528 {
irisl 21:674fafb6301d 529
irisl 21:674fafb6301d 530 pwm_motor1.period_us(100);
irisl 21:674fafb6301d 531 motor1.setPosition(0);
irisl 21:674fafb6301d 532 pwm_motor2.period_us(100);
irisl 21:674fafb6301d 533 motor2.setPosition(0);
jessekaiser 0:db396b9f4b4c 534 pc.baud(115200);
jessekaiser 1:099b19376f16 535
jessekaiser 0:db396b9f4b4c 536 Ticker log_timer;
jessekaiser 1:099b19376f16 537 //set up filters. Use external array for constants
irisl 9:f7ec578a17c0 538 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
irisl 9:f7ec578a17c0 539 arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
irisl 9:f7ec578a17c0 540 arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
irisl 9:f7ec578a17c0 541 arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
jessekaiser 0:db396b9f4b4c 542 /**Here you attach the 'void looper(void)' function to the Ticker object
jessekaiser 0:db396b9f4b4c 543 * The looper() function will be called every 0.01 seconds.
jessekaiser 0:db396b9f4b4c 544 * Please mind that the parentheses after looper are omitted when using attach.
jessekaiser 0:db396b9f4b4c 545 */
irisl 26:9859a71456fd 546 log_timer.attach(looper, 0.002);
irisl 21:674fafb6301d 547
irisl 21:674fafb6301d 548 Ticker looptimer;
irisl 21:674fafb6301d 549 looptimer.attach(looper_motor,TSAMP);
jessekaiser 1:099b19376f16 550 while(1) { //Loop
jessekaiser 1:099b19376f16 551 /*Empty!*/
jessekaiser 1:099b19376f16 552 /*Everything is handled by the interrupt routine now!*/
jessekaiser 6:8f4138a811e0 553
irisl 13:493a953a2a85 554 while(1) {
irisl 18:ed2afe6953de 555 pc.printf("Span de biceps aan om het instellen te starten.\n");
irisl 13:493a953a2a85 556 do {
irisl 16:d65458b85734 557 ShineRed();
irisl 17:dbbe4e126203 558 } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting
irisl 17:dbbe4e126203 559 if (filtered_average_bi > 0.05) {
irisl 18:ed2afe6953de 560 BlinkRed(10);
irisl 18:ed2afe6953de 561 BlinkGreen();
irisl 18:ed2afe6953de 562 while (1) {
irisl 18:ed2afe6953de 563 pc.printf("In de loop.\n");
irisl 26:9859a71456fd 564 if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) {
irisl 18:ed2afe6953de 565 stopblinkgreen();
irisl 20:d40b6cba4280 566 pc.printf("ShineGreen.\n");
irisl 20:d40b6cba4280 567 ShineGreen();
irisl 18:ed2afe6953de 568 wait (4);
irisl 18:ed2afe6953de 569 break;
irisl 18:ed2afe6953de 570 }
irisl 26:9859a71456fd 571 if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) {
irisl 18:ed2afe6953de 572 stopblinkgreen();
irisl 18:ed2afe6953de 573 pc.printf("ShineBlue.\n");
irisl 18:ed2afe6953de 574 ShineBlue();
irisl 22:14f5161d7d7b 575 batje_hoek = 2;
irisl 18:ed2afe6953de 576 wait(4);
irisl 18:ed2afe6953de 577 break;
irisl 26:9859a71456fd 578 } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045)
irisl 18:ed2afe6953de 579
irisl 18:ed2afe6953de 580 {
irisl 18:ed2afe6953de 581 stopblinkgreen();
irisl 20:d40b6cba4280 582 pc.printf("ShineRed.\n");
irisl 20:d40b6cba4280 583 ShineRed();
irisl 22:14f5161d7d7b 584 batje_hoek = 1;
irisl 18:ed2afe6953de 585 wait (4);
irisl 18:ed2afe6953de 586 break;
irisl 18:ed2afe6953de 587 }
irisl 14:257026c95f22 588 }
irisl 19:1bd2fc3bce1e 589 BlinkGreen();
irisl 19:1bd2fc3bce1e 590 while (1) {
irisl 19:1bd2fc3bce1e 591 pc.printf("In de loop.\n");
irisl 26:9859a71456fd 592 if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) {
irisl 19:1bd2fc3bce1e 593 stopblinkgreen();
irisl 20:d40b6cba4280 594 pc.printf("ShineGreen.\n");
irisl 20:d40b6cba4280 595 ShineGreen();
irisl 22:14f5161d7d7b 596 arm_hoogte = 3;
irisl 19:1bd2fc3bce1e 597 wait (4);
irisl 19:1bd2fc3bce1e 598 break;
irisl 19:1bd2fc3bce1e 599 }
irisl 26:9859a71456fd 600 if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) {
irisl 19:1bd2fc3bce1e 601 stopblinkgreen();
irisl 19:1bd2fc3bce1e 602 pc.printf("ShineBlue.\n");
irisl 19:1bd2fc3bce1e 603 ShineBlue();
irisl 22:14f5161d7d7b 604 arm_hoogte = 1;
irisl 19:1bd2fc3bce1e 605 wait(4);
irisl 19:1bd2fc3bce1e 606 break;
irisl 26:9859a71456fd 607 } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045)
irisl 19:1bd2fc3bce1e 608
irisl 19:1bd2fc3bce1e 609 {
irisl 19:1bd2fc3bce1e 610 stopblinkgreen();
irisl 20:d40b6cba4280 611 pc.printf("ShineRed.\n");
irisl 20:d40b6cba4280 612 ShineRed();
irisl 22:14f5161d7d7b 613 arm_hoogte = 2;
irisl 19:1bd2fc3bce1e 614 wait (4);
irisl 19:1bd2fc3bce1e 615 break;
irisl 19:1bd2fc3bce1e 616 }
irisl 19:1bd2fc3bce1e 617 }
irisl 26:9859a71456fd 618
irisl 14:257026c95f22 619 }
irisl 19:1bd2fc3bce1e 620
jessekaiser 0:db396b9f4b4c 621 }
irisl 16:d65458b85734 622 }
irisl 19:1bd2fc3bce1e 623 }