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@24:a61c2cadbd36, 2014-10-31 (annotated)

Committer:
irisl
Date:
Fri Oct 31 16:36:30 2014 +0000
Revision:
24:a61c2cadbd36
Parent:
23:94b5746e52f5
Child:
25:a12d51345aa4
Terug draaien van arm doet raar...

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 13:493a953a2a85 33 HIDScope scope(2);
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;
jessekaiser 2:39e1bde54e73 67 //lowpass filter settings: Fc = 225 Hz, Fs = 500 Hz, Gain = -3 dB
jessekaiser 0:db396b9f4b4c 68 float lowpass_const[] = {0.8005910266528647, 1.6011820533057295, 0.8005910266528647, -1.5610153912536877, -0.6413487153577715};
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;
jessekaiser 2:39e1bde54e73 74 //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB
irisl 21:674fafb6301d 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 18:ed2afe6953de 93 if ( number == 500) {
irisl 18:ed2afe6953de 94 *average = total/500;
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 18:ed2afe6953de 106 if ( number == 500) {
irisl 18:ed2afe6953de 107 *average_output = total/500;
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 13:493a953a2a85 157 //scope.set(0,filtered_biceps); //processed float biceps
irisl 13:493a953a2a85 158 scope.set(0,filtered_average_bi); //processed float deltoid
irisl 14:257026c95f22 159 //scope.set(2,filtered_deltoid); //processed float biceps
irisl 13:493a953a2a85 160 scope.set(1,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 24:a61c2cadbd36 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 23:94b5746e52f5 463 if(wait_iterator1 > 1000)
irisl 21:674fafb6301d 464 staat1 = 2;
irisl 21:674fafb6301d 465 } else {
irisl 21:674fafb6301d 466 batje_begin_rechts();
irisl 21:674fafb6301d 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 23:94b5746e52f5 476 if(wait_iterator1 > 1000)
irisl 21:674fafb6301d 477 staat1 = 2;
irisl 21:674fafb6301d 478 } else {
irisl 21:674fafb6301d 479 batje_begin_links ();
irisl 21:674fafb6301d 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 23:94b5746e52f5 490 if(wait_iterator2 > 400)
irisl 21:674fafb6301d 491 staat2 = 2;
irisl 21:674fafb6301d 492 } else {
irisl 21:674fafb6301d 493 arm_begin();
irisl 21:674fafb6301d 494 }
irisl 21:674fafb6301d 495 }
irisl 22:14f5161d7d7b 496 if(arm_hoogte == 2) {
irisl 21:674fafb6301d 497 if(staat2 == 0) {
irisl 21:674fafb6301d 498 arm_mid();
irisl 21:674fafb6301d 499 wait_iterator2 = 0;
irisl 21:674fafb6301d 500 } else if(staat2 == 1) {
irisl 21:674fafb6301d 501 wait_iterator2++;
irisl 23:94b5746e52f5 502 if(wait_iterator2 > 400)
irisl 21:674fafb6301d 503 staat2 = 2;
irisl 21:674fafb6301d 504 } else {
irisl 21:674fafb6301d 505 arm_begin();
irisl 21:674fafb6301d 506 }
irisl 21:674fafb6301d 507 }
irisl 22:14f5161d7d7b 508 if(arm_hoogte == 3) {
irisl 21:674fafb6301d 509 if(staat2 == 0) {
irisl 21:674fafb6301d 510 arm_hoog();
irisl 21:674fafb6301d 511 wait_iterator2 = 0;
irisl 21:674fafb6301d 512 } else if(staat2 == 1) {
irisl 21:674fafb6301d 513 wait_iterator2++;
irisl 23:94b5746e52f5 514 if(wait_iterator2 > 400)
irisl 21:674fafb6301d 515 staat2 = 2;
irisl 21:674fafb6301d 516 } else {
irisl 21:674fafb6301d 517 arm_begin();
irisl 21:674fafb6301d 518 }
irisl 21:674fafb6301d 519 }
irisl 20:d40b6cba4280 520 }
irisl 15:eb6498bb7ca0 521
irisl 16:d65458b85734 522 int main()
jessekaiser 1:099b19376f16 523 {
irisl 21:674fafb6301d 524
irisl 21:674fafb6301d 525 pwm_motor1.period_us(100);
irisl 21:674fafb6301d 526 motor1.setPosition(0);
irisl 21:674fafb6301d 527 pwm_motor2.period_us(100);
irisl 21:674fafb6301d 528 motor2.setPosition(0);
jessekaiser 0:db396b9f4b4c 529 pc.baud(115200);
jessekaiser 1:099b19376f16 530
jessekaiser 0:db396b9f4b4c 531 Ticker log_timer;
jessekaiser 1:099b19376f16 532 //set up filters. Use external array for constants
irisl 9:f7ec578a17c0 533 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
irisl 9:f7ec578a17c0 534 arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
irisl 9:f7ec578a17c0 535 arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
irisl 9:f7ec578a17c0 536 arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
jessekaiser 0:db396b9f4b4c 537 /**Here you attach the 'void looper(void)' function to the Ticker object
jessekaiser 0:db396b9f4b4c 538 * The looper() function will be called every 0.01 seconds.
jessekaiser 0:db396b9f4b4c 539 * Please mind that the parentheses after looper are omitted when using attach.
jessekaiser 0:db396b9f4b4c 540 */
jessekaiser 0:db396b9f4b4c 541 log_timer.attach(looper, 0.001);
irisl 21:674fafb6301d 542
irisl 21:674fafb6301d 543 Ticker looptimer;
irisl 21:674fafb6301d 544 looptimer.attach(looper_motor,TSAMP);
jessekaiser 1:099b19376f16 545 while(1) { //Loop
jessekaiser 1:099b19376f16 546 /*Empty!*/
jessekaiser 1:099b19376f16 547 /*Everything is handled by the interrupt routine now!*/
jessekaiser 6:8f4138a811e0 548
irisl 13:493a953a2a85 549 while(1) {
irisl 18:ed2afe6953de 550 pc.printf("Span de biceps aan om het instellen te starten.\n");
irisl 13:493a953a2a85 551 do {
irisl 16:d65458b85734 552 ShineRed();
irisl 17:dbbe4e126203 553 } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting
irisl 17:dbbe4e126203 554 if (filtered_average_bi > 0.05) {
irisl 18:ed2afe6953de 555 BlinkRed(10);
irisl 18:ed2afe6953de 556 BlinkGreen();
irisl 18:ed2afe6953de 557 while (1) {
irisl 18:ed2afe6953de 558 pc.printf("In de loop.\n");
irisl 20:d40b6cba4280 559 if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) {
irisl 18:ed2afe6953de 560 stopblinkgreen();
irisl 20:d40b6cba4280 561 pc.printf("ShineGreen.\n");
irisl 20:d40b6cba4280 562 ShineGreen();
irisl 18:ed2afe6953de 563 wait (4);
irisl 18:ed2afe6953de 564 break;
irisl 18:ed2afe6953de 565 }
irisl 18:ed2afe6953de 566 if (filtered_average_bi < 0.05 && filtered_average_del > 0.05) {
irisl 18:ed2afe6953de 567 stopblinkgreen();
irisl 18:ed2afe6953de 568 pc.printf("ShineBlue.\n");
irisl 18:ed2afe6953de 569 ShineBlue();
irisl 22:14f5161d7d7b 570 batje_hoek = 2;
irisl 18:ed2afe6953de 571 wait(4);
irisl 18:ed2afe6953de 572 break;
irisl 20:d40b6cba4280 573 } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05)
irisl 18:ed2afe6953de 574
irisl 18:ed2afe6953de 575 {
irisl 18:ed2afe6953de 576 stopblinkgreen();
irisl 20:d40b6cba4280 577 pc.printf("ShineRed.\n");
irisl 20:d40b6cba4280 578 ShineRed();
irisl 22:14f5161d7d7b 579 batje_hoek = 1;
irisl 18:ed2afe6953de 580 wait (4);
irisl 18:ed2afe6953de 581 break;
irisl 18:ed2afe6953de 582 }
irisl 14:257026c95f22 583 }
irisl 19:1bd2fc3bce1e 584 BlinkGreen();
irisl 19:1bd2fc3bce1e 585 while (1) {
irisl 19:1bd2fc3bce1e 586 pc.printf("In de loop.\n");
irisl 20:d40b6cba4280 587 if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) {
irisl 19:1bd2fc3bce1e 588 stopblinkgreen();
irisl 20:d40b6cba4280 589 pc.printf("ShineGreen.\n");
irisl 20:d40b6cba4280 590 ShineGreen();
irisl 22:14f5161d7d7b 591 arm_hoogte = 3;
irisl 19:1bd2fc3bce1e 592 wait (4);
irisl 19:1bd2fc3bce1e 593 break;
irisl 19:1bd2fc3bce1e 594 }
irisl 19:1bd2fc3bce1e 595 if (filtered_average_bi < 0.05 && filtered_average_del > 0.05) {
irisl 19:1bd2fc3bce1e 596 stopblinkgreen();
irisl 19:1bd2fc3bce1e 597 pc.printf("ShineBlue.\n");
irisl 19:1bd2fc3bce1e 598 ShineBlue();
irisl 22:14f5161d7d7b 599 arm_hoogte = 1;
irisl 19:1bd2fc3bce1e 600 wait(4);
irisl 19:1bd2fc3bce1e 601 break;
irisl 20:d40b6cba4280 602 } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05)
irisl 19:1bd2fc3bce1e 603
irisl 19:1bd2fc3bce1e 604 {
irisl 19:1bd2fc3bce1e 605 stopblinkgreen();
irisl 20:d40b6cba4280 606 pc.printf("ShineRed.\n");
irisl 20:d40b6cba4280 607 ShineRed();
irisl 22:14f5161d7d7b 608 arm_hoogte = 2;
irisl 19:1bd2fc3bce1e 609 wait (4);
irisl 19:1bd2fc3bce1e 610 break;
irisl 19:1bd2fc3bce1e 611 }
irisl 19:1bd2fc3bce1e 612 }
jessekaiser 6:8f4138a811e0 613
irisl 14:257026c95f22 614 }
irisl 19:1bd2fc3bce1e 615
jessekaiser 0:db396b9f4b4c 616 }
irisl 16:d65458b85734 617 }
irisl 19:1bd2fc3bce1e 618 }