Casper Maas / Mbed 2 deprecated StateMachine_EMg_RKI_PID_MOTORwerkend

fork demo mode 20:58

Dependencies:   biquadFilter MODSERIAL QEI mbed

Fork of StateMachine_EMg_RKI_PID_MOTOR by Casper Maas

main.cpp@13:0e25698dce40, 2018-11-01 (annotated)

Committer:
cmaas
Date:
Thu Nov 01 18:56:23 2018 +0000
Revision:
13:0e25698dce40
Parent:
12:99e29b8d4155
Child:
14:fb5d8064830d
random doesnt work;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cmaas 9:40c9a18c3430 1 // EMG + KINEMATICS + PID + MOTOR CONTROL
cmaas 9:40c9a18c3430 2
cmaas 9:40c9a18c3430 3 //----------------~INITIATING-------------------------
aschut 0:90750f158475 4 #include "mbed.h"
cmaas 9:40c9a18c3430 5
cmaas 9:40c9a18c3430 6 // EMG -- DEPENDENCIES
aschut 0:90750f158475 7 #include <iostream>
gastongab 2:0a8622662f6d 8 #include "BiQuad.h"
gastongab 2:0a8622662f6d 9 #include "BiQuadchains_zelfbeun.h"
gastongab 2:0a8622662f6d 10 #include "MODSERIAL.h"
gastongab 2:0a8622662f6d 11
cmaas 9:40c9a18c3430 12 // KINEMATICS -- DEPENDENCIES
cmaas 9:40c9a18c3430 13 #include "stdio.h"
cmaas 9:40c9a18c3430 14 #define _USE_MATH_DEFINES
cmaas 9:40c9a18c3430 15 #include <math.h>
cmaas 9:40c9a18c3430 16 #define M_PI 3.14159265358979323846 /* pi */
cmaas 9:40c9a18c3430 17
cmaas 9:40c9a18c3430 18 // PID CONTROLLER -- DEPENDENCIES
cmaas 9:40c9a18c3430 19 #include "BiQuad.h"
cmaas 9:40c9a18c3430 20 #include "QEI.h"
cmaas 9:40c9a18c3430 21 //#include "HIDScope.h"
cmaas 9:40c9a18c3430 22
cmaas 9:40c9a18c3430 23
cmaas 9:40c9a18c3430 24 // GENERAL PIN DEFENITIONS
gastongab 2:0a8622662f6d 25 MODSERIAL pc(USBTX, USBRX);
gastongab 2:0a8622662f6d 26
cmaas 9:40c9a18c3430 27 // EMG -- PIN DEFENITIONS
aschut 0:90750f158475 28 DigitalOut gpo(D0);
aschut 0:90750f158475 29
cmaas 9:40c9a18c3430 30 DigitalIn button2(SW3);
aschut 0:90750f158475 31 DigitalIn button1(SW2); //or SW2
aschut 0:90750f158475 32
aschut 0:90750f158475 33 DigitalOut led1(LED_GREEN);
aschut 0:90750f158475 34 DigitalOut led2(LED_RED);
aschut 0:90750f158475 35 DigitalOut led3(LED_BLUE);
aschut 0:90750f158475 36
gastongab 2:0a8622662f6d 37 //EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
gastongab 2:0a8622662f6d 38 Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach
gastongab 4:c7be673eb4a1 39 Ticker threshold_check_ticker;
gastongab 2:0a8622662f6d 40 Timer t; //timer try out for Astrid
gastongab 2:0a8622662f6d 41 Timer timer_calibration; //timer for EMG calibration
gastongab 2:0a8622662f6d 42
gastongab 2:0a8622662f6d 43 //Input system
gastongab 2:0a8622662f6d 44 AnalogIn emg1(A0); //right biceps
gastongab 2:0a8622662f6d 45 AnalogIn emg2(A1); //right triceps
gastongab 2:0a8622662f6d 46 AnalogIn emg3(A2); //left biceps
gastongab 2:0a8622662f6d 47 AnalogIn emg4(A3); //left triceps
gastongab 2:0a8622662f6d 48
cmaas 9:40c9a18c3430 49
cmaas 9:40c9a18c3430 50 // PID CONTROLLER -- PIN DEFENITIONS
cmaas 12:99e29b8d4155 51 //AnalogIn button3(A4);
cmaas 12:99e29b8d4155 52 //AnalogIn button4(A5);
cmaas 9:40c9a18c3430 53
cmaas 9:40c9a18c3430 54 DigitalOut directionpin1(D7); // motor 1
cmaas 9:40c9a18c3430 55 DigitalOut directionpin2(D4); // motor 2
cmaas 9:40c9a18c3430 56 DigitalOut directionpin3(D13); // motor 3
cmaas 9:40c9a18c3430 57 PwmOut pwmpin1(D6); // motor 1
cmaas 9:40c9a18c3430 58 PwmOut pwmpin2(D5); // motor 2
cmaas 9:40c9a18c3430 59 PwmOut pwmpin3(D12); // motor 3
cmaas 9:40c9a18c3430 60
cmaas 9:40c9a18c3430 61 QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
cmaas 9:40c9a18c3430 62 QEI encoder2 (D11, D10, NC, 8400, QEI::X4_ENCODING); // motor 2
cmaas 9:40c9a18c3430 63 QEI encoder3 (D3, D2, NC, 8400, QEI::X4_ENCODING); // motor 3
cmaas 9:40c9a18c3430 64 // HIDScope scope(2);
cmaas 9:40c9a18c3430 65
cmaas 9:40c9a18c3430 66 // PID - TICKERS
cmaas 9:40c9a18c3430 67 Ticker ref_rot;
cmaas 9:40c9a18c3430 68 Ticker show_counts;
cmaas 9:40c9a18c3430 69 Ticker Scope_Data;
cmaas 9:40c9a18c3430 70
cmaas 10:2325e545ce11 71 //------------------------GLOBALS-----------------------------
cmaas 9:40c9a18c3430 72 // GLOBALS EMG
gastongab 2:0a8622662f6d 73 //Filtered EMG signals from the end of the chains
gastongab 4:c7be673eb4a1 74 volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
gastongab 4:c7be673eb4a1 75 int i = 0;
gastongab 2:0a8622662f6d 76
cmaas 9:40c9a18c3430 77 //Define doubles for calibration and ticker
cmaas 9:40c9a18c3430 78 double ts = 0.001; //tijdsstap
cmaas 9:40c9a18c3430 79 double calibration_time = 55; //time EMG calibration should take
cmaas 9:40c9a18c3430 80
cmaas 9:40c9a18c3430 81 volatile double temp_highest_emg1 = 0; //highest detected value right biceps
cmaas 9:40c9a18c3430 82 volatile double temp_highest_emg2 = 0;
cmaas 9:40c9a18c3430 83 volatile double temp_highest_emg3 = 0;
cmaas 9:40c9a18c3430 84 volatile double temp_highest_emg4 = 0;
cmaas 9:40c9a18c3430 85
cmaas 9:40c9a18c3430 86 //Doubles for calculation threshold
cmaas 9:40c9a18c3430 87 double biceps_p_t = 0.4; //set threshold at percentage of highest value
cmaas 9:40c9a18c3430 88 double triceps_p_t = 0.5; //set threshold at percentage of highest value
cmaas 9:40c9a18c3430 89 volatile double threshold1;
cmaas 9:40c9a18c3430 90 volatile double threshold2;
cmaas 9:40c9a18c3430 91 volatile double threshold3;
cmaas 9:40c9a18c3430 92 volatile double threshold4;
cmaas 9:40c9a18c3430 93
cmaas 9:40c9a18c3430 94 // thresholdreads bools
cmaas 9:40c9a18c3430 95 int bicepsR;
cmaas 9:40c9a18c3430 96 int tricepsR;
cmaas 9:40c9a18c3430 97 int bicepsL;
cmaas 9:40c9a18c3430 98 int tricepsL;
cmaas 9:40c9a18c3430 99
cmaas 9:40c9a18c3430 100 // VARIABLES ROBOT KINEMATICS
cmaas 9:40c9a18c3430 101 // constants
cmaas 9:40c9a18c3430 102 const float la = 0.256; // lengte actieve arm
cmaas 9:40c9a18c3430 103 const float lp = 0.21; // lengte passieve arm
cmaas 9:40c9a18c3430 104 const float rp = 0.052; // straal van midden end effector tot hoekpunt
cmaas 9:40c9a18c3430 105 const float rm = 0.23; // straal van global midden tot motor
cmaas 9:40c9a18c3430 106 const float a = 0.09; // zijde van de driehoek
cmaas 9:40c9a18c3430 107 const float xas = 0.40; // afstand van motor 1 tot motor 3
cmaas 9:40c9a18c3430 108 const float yas = 0.346; // afstand van xas tot motor 2
cmaas 9:40c9a18c3430 109 const float thetap = 0; // rotatiehoek van de end effector
cmaas 9:40c9a18c3430 110
cmaas 9:40c9a18c3430 111
cmaas 9:40c9a18c3430 112 // motor locatie
cmaas 9:40c9a18c3430 113 const int a1x = 0; //x locatie motor 1
cmaas 9:40c9a18c3430 114 const int a1y = 0; //y locatie motor 1
cmaas 9:40c9a18c3430 115 const float a2x = (0.5)*xas; // x locatie motor 2
cmaas 9:40c9a18c3430 116 const float a2y = yas; // y locatie motor 2
cmaas 9:40c9a18c3430 117 const float a3x = xas; // x locatie motor 3
cmaas 9:40c9a18c3430 118 const int a3y = 0; // y locatie motor 3
cmaas 9:40c9a18c3430 119
cmaas 9:40c9a18c3430 120 // script voor het bepalen van de desired position aan de hand van emg (1/0)
cmaas 9:40c9a18c3430 121
cmaas 9:40c9a18c3430 122 // EMG OUTPUT
cmaas 9:40c9a18c3430 123 int EMGxplus;
cmaas 9:40c9a18c3430 124 int EMGxmin ;
cmaas 9:40c9a18c3430 125 int EMGyplus;
cmaas 9:40c9a18c3430 126 int EMGymin ;
cmaas 9:40c9a18c3430 127
cmaas 9:40c9a18c3430 128 // Dit moet experimenteel geperfectioneerd worden
cmaas 9:40c9a18c3430 129 float tijdstap = 0.005; //nu wss heel langzaam, kan miss omhoog KEER V GEEFT VERANDERING IN POSITIE
cmaas 9:40c9a18c3430 130 float v = 0.1; // snelheid kan wss ook hoger
cmaas 9:40c9a18c3430 131
cmaas 9:40c9a18c3430 132 float px = 0.2; //starting x // BOUNDARIES
cmaas 9:40c9a18c3430 133 float py = 0.155; // starting y // BOUNDARIES
cmaas 9:40c9a18c3430 134
cmaas 9:40c9a18c3430 135 // verschil horizontale as met de actieve arm
cmaas 9:40c9a18c3430 136 float da1 = 1.619685; // verschil a1 hoek en motor
cmaas 9:40c9a18c3430 137 float da2 = -0.609780;
cmaas 9:40c9a18c3430 138 float da3 = 3.372859;
cmaas 9:40c9a18c3430 139
cmaas 9:40c9a18c3430 140 // limits (since no forward kinematics)
cmaas 9:40c9a18c3430 141 float upperxlim = 0.275; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan
cmaas 9:40c9a18c3430 142 float lowerxlim = 0.10;
cmaas 9:40c9a18c3430 143 float upperylim = 0.225;
cmaas 9:40c9a18c3430 144 float lowerylim = 0.03; //0.03 is goed
cmaas 9:40c9a18c3430 145
cmaas 9:40c9a18c3430 146 // VARIABLES PID CONTROLLER
cmaas 9:40c9a18c3430 147 double PI = M_PI;// CHANGE THIS INTO M_PI
cmaas 9:40c9a18c3430 148 double Kp = 14; //200 , 50
cmaas 9:40c9a18c3430 149 double Ki = 0; //1, 0.5
cmaas 9:40c9a18c3430 150 double Kd = 3; //200, 10
cmaas 9:40c9a18c3430 151 double Ts = 0.1; // Sample time in seconds
cmaas 9:40c9a18c3430 152 double reference_rotation; //define as radians
cmaas 9:40c9a18c3430 153 double motor_position;
cmaas 9:40c9a18c3430 154 bool AlwaysTrue;
cmaas 9:40c9a18c3430 155
cmaas 9:40c9a18c3430 156 //----------------FUNCTIONS--------------------------
cmaas 9:40c9a18c3430 157
cmaas 9:40c9a18c3430 158 // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
gastongab 7:88fa84742b3c 159 void emgsample()
gastongab 7:88fa84742b3c 160 {
gastongab 2:0a8622662f6d 161 //All EMG signal through Highpass
gastongab 2:0a8622662f6d 162 double emgread1 = emg1.read();
gastongab 2:0a8622662f6d 163 double emgread2 = emg2.read();
gastongab 2:0a8622662f6d 164 double emgread3 = emg3.read();
gastongab 2:0a8622662f6d 165 double emgread4 = emg4.read();
gastongab 7:88fa84742b3c 166
gastongab 2:0a8622662f6d 167 double emg1_highpassed = highp1.step(emgread1);
gastongab 2:0a8622662f6d 168 double emg2_highpassed = highp2.step(emgread2);
gastongab 2:0a8622662f6d 169 double emg3_highpassed = highp3.step(emgread3);
gastongab 2:0a8622662f6d 170 double emg4_highpassed = highp4.step(emgread4);
gastongab 7:88fa84742b3c 171
gastongab 2:0a8622662f6d 172 //All EMG highpassed through Notch
gastongab 2:0a8622662f6d 173 double emg1_notched = notch1.step(emg1_highpassed);
gastongab 2:0a8622662f6d 174 double emg2_notched = notch2.step(emg2_highpassed);
gastongab 2:0a8622662f6d 175 double emg3_notched = notch3.step(emg3_highpassed);
gastongab 2:0a8622662f6d 176 double emg4_notched = notch4.step(emg4_highpassed);
gastongab 7:88fa84742b3c 177
gastongab 2:0a8622662f6d 178 //All EMG notched rectify
gastongab 2:0a8622662f6d 179 double emg1_abs = abs(emg1_notched);
gastongab 2:0a8622662f6d 180 double emg2_abs = abs(emg2_notched);
gastongab 2:0a8622662f6d 181 double emg3_abs = abs(emg3_notched);
gastongab 2:0a8622662f6d 182 double emg4_abs = abs(emg4_notched);
gastongab 7:88fa84742b3c 183
gastongab 2:0a8622662f6d 184 //All EMG abs into lowpass
gastongab 2:0a8622662f6d 185 emg1_filtered = lowp1.step(emg1_abs);
gastongab 2:0a8622662f6d 186 emg2_filtered = lowp2.step(emg2_abs);
gastongab 2:0a8622662f6d 187 emg3_filtered = lowp3.step(emg3_abs);
gastongab 2:0a8622662f6d 188 emg4_filtered = lowp4.step(emg4_abs);
gastongab 7:88fa84742b3c 189 }
gastongab 7:88fa84742b3c 190
gastongab 7:88fa84742b3c 191 void CalibrationEMG()
gastongab 7:88fa84742b3c 192 {
gastongab 7:88fa84742b3c 193 //static float samples = calibration_time/ts;
gastongab 7:88fa84742b3c 194 while(timer_calibration<55) {
gastongab 7:88fa84742b3c 195 if(timer_calibration>0 && timer_calibration<10) {
gastongab 7:88fa84742b3c 196 led1=!led1;
gastongab 7:88fa84742b3c 197 if(emg1_filtered>temp_highest_emg1) {
gastongab 7:88fa84742b3c 198 temp_highest_emg1= emg1_filtered;
cmaas 13:0e25698dce40 199 //pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
gastongab 7:88fa84742b3c 200 }
gastongab 7:88fa84742b3c 201 }
gastongab 7:88fa84742b3c 202 if(timer_calibration>10 && timer_calibration<15) {
gastongab 7:88fa84742b3c 203 led1=0;
gastongab 7:88fa84742b3c 204 led2=0;
gastongab 7:88fa84742b3c 205 led3=0;
gastongab 7:88fa84742b3c 206 }
gastongab 7:88fa84742b3c 207 if(timer_calibration>15 && timer_calibration<25) {
gastongab 7:88fa84742b3c 208 led2=!led2;
gastongab 7:88fa84742b3c 209 if(emg2_filtered>temp_highest_emg2) {
gastongab 7:88fa84742b3c 210 temp_highest_emg2= emg2_filtered;
cmaas 13:0e25698dce40 211 //pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
gastongab 7:88fa84742b3c 212 }
gastongab 7:88fa84742b3c 213 }
gastongab 7:88fa84742b3c 214 if(timer_calibration>25 && timer_calibration<30) {
gastongab 7:88fa84742b3c 215 led1=0;
gastongab 7:88fa84742b3c 216 led2=0;
gastongab 7:88fa84742b3c 217 led3=0;
gastongab 7:88fa84742b3c 218 }
gastongab 7:88fa84742b3c 219 if(timer_calibration>30 && timer_calibration<40) {
gastongab 7:88fa84742b3c 220 led3=!led3;
gastongab 7:88fa84742b3c 221 if(emg3_filtered>temp_highest_emg3) {
gastongab 7:88fa84742b3c 222 temp_highest_emg3= emg3_filtered;
cmaas 13:0e25698dce40 223 //pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
gastongab 7:88fa84742b3c 224 }
gastongab 7:88fa84742b3c 225 }
gastongab 7:88fa84742b3c 226 if(timer_calibration>40 && timer_calibration<45) {
gastongab 7:88fa84742b3c 227 led1=0;
gastongab 7:88fa84742b3c 228 led2=0;
gastongab 7:88fa84742b3c 229 led3=0;
gastongab 7:88fa84742b3c 230 }
gastongab 7:88fa84742b3c 231 if(timer_calibration>45 && timer_calibration<55) {
gastongab 7:88fa84742b3c 232 led2=!led2;
gastongab 7:88fa84742b3c 233 led3=!led3;
gastongab 7:88fa84742b3c 234 if(emg4_filtered>temp_highest_emg4) {
gastongab 7:88fa84742b3c 235 temp_highest_emg4= emg4_filtered;
cmaas 13:0e25698dce40 236 //pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
gastongab 7:88fa84742b3c 237 }
gastongab 7:88fa84742b3c 238 }
gastongab 7:88fa84742b3c 239 led1=1;
gastongab 7:88fa84742b3c 240 led2=1;
gastongab 7:88fa84742b3c 241 led3=1;
gastongab 2:0a8622662f6d 242 }
cmaas 9:40c9a18c3430 243 /*
gastongab 2:0a8622662f6d 244 pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1);
gastongab 2:0a8622662f6d 245 pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2);
gastongab 2:0a8622662f6d 246 pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3);
gastongab 2:0a8622662f6d 247 pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4);
cmaas 9:40c9a18c3430 248 */
gastongab 7:88fa84742b3c 249
gastongab 6:f55ab7e38a7f 250 threshold1 = temp_highest_emg1*biceps_p_t; //Right Biceps
gastongab 6:f55ab7e38a7f 251 threshold2 = temp_highest_emg2*triceps_p_t; //Right Triceps
gastongab 6:f55ab7e38a7f 252 threshold3 = temp_highest_emg3*biceps_p_t; //Left Biceps
gastongab 7:88fa84742b3c 253 threshold4 = temp_highest_emg4*triceps_p_t; //Left Triceps
gastongab 2:0a8622662f6d 254 }
gastongab 2:0a8622662f6d 255
gastongab 4:c7be673eb4a1 256 //Check if emg_filtered has reached their threshold
gastongab 7:88fa84742b3c 257 void threshold_check()
gastongab 7:88fa84742b3c 258 {
gastongab 7:88fa84742b3c 259
gastongab 2:0a8622662f6d 260 //EMG1 threshold check
gastongab 7:88fa84742b3c 261 if(emg1_filtered>threshold1) {
gastongab 4:c7be673eb4a1 262 bicepsR = 1;
gastongab 7:88fa84742b3c 263 } else {
gastongab 4:c7be673eb4a1 264 bicepsR= 0;
gastongab 7:88fa84742b3c 265 }
gastongab 2:0a8622662f6d 266 //EMG2 threshold check
gastongab 7:88fa84742b3c 267 if(emg2_filtered>threshold2) {
gastongab 4:c7be673eb4a1 268 tricepsR = 1;
gastongab 7:88fa84742b3c 269 } else {
gastongab 4:c7be673eb4a1 270 tricepsR= 0;
gastongab 7:88fa84742b3c 271 }
gastongab 2:0a8622662f6d 272 //EMG3 threshold check
gastongab 7:88fa84742b3c 273 if(emg3_filtered>threshold3) {
gastongab 4:c7be673eb4a1 274 bicepsL = 1;
gastongab 7:88fa84742b3c 275 } else {
gastongab 4:c7be673eb4a1 276 bicepsL= 0;
gastongab 7:88fa84742b3c 277 }
gastongab 2:0a8622662f6d 278 //EMG4 threshold check
gastongab 7:88fa84742b3c 279 if(emg4_filtered>threshold4) {
gastongab 4:c7be673eb4a1 280 tricepsL = 1;
gastongab 7:88fa84742b3c 281 } else {
gastongab 4:c7be673eb4a1 282 tricepsL= 0;
gastongab 7:88fa84742b3c 283 }
gastongab 7:88fa84742b3c 284
gastongab 7:88fa84742b3c 285 /*
gastongab 4:c7be673eb4a1 286 pc.printf("Biceps Right = %i", bicepsR);
gastongab 7:88fa84742b3c 287 pc.printf("Triceps Right = %i",tricepsR);
gastongab 4:c7be673eb4a1 288 pc.printf("Biceps Left = %i", bicepsL);
gastongab 4:c7be673eb4a1 289 pc.printf("Triceps Left = %i", tricepsL);
gastongab 4:c7be673eb4a1 290 */
gastongab 2:0a8622662f6d 291 }
aschut 0:90750f158475 292
cmaas 13:0e25698dce40 293
gastongab 7:88fa84742b3c 294
cmaas 9:40c9a18c3430 295 // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~
cmaas 9:40c9a18c3430 296
cmaas 9:40c9a18c3430 297 // functie x positie
cmaas 9:40c9a18c3430 298 float positionx(int EMGxplus,int EMGxmin)
cmaas 9:40c9a18c3430 299 {
cmaas 9:40c9a18c3430 300 float EMGx = EMGxplus - EMGxmin;
cmaas 9:40c9a18c3430 301
cmaas 9:40c9a18c3430 302 float verplaatsingx = EMGx * tijdstap * v;
cmaas 9:40c9a18c3430 303 float pxnieuw = px + verplaatsingx;
cmaas 9:40c9a18c3430 304 // x limit
cmaas 9:40c9a18c3430 305 if (pxnieuw <= upperxlim && pxnieuw >= lowerxlim) {
cmaas 9:40c9a18c3430 306 px = pxnieuw;
cmaas 9:40c9a18c3430 307 } else {
cmaas 9:40c9a18c3430 308 if (pxnieuw >= lowerxlim) {
cmaas 9:40c9a18c3430 309 px = upperxlim;
cmaas 9:40c9a18c3430 310 } else {
cmaas 9:40c9a18c3430 311 px = lowerxlim;
cmaas 9:40c9a18c3430 312 }
cmaas 9:40c9a18c3430 313 }
cmaas 9:40c9a18c3430 314 //printf("X eindpunt (%f) en verplaatsing: (%f)\n\r",px,verplaatsingx);
cmaas 9:40c9a18c3430 315 return px;
cmaas 9:40c9a18c3430 316 }
cmaas 9:40c9a18c3430 317
cmaas 9:40c9a18c3430 318
cmaas 9:40c9a18c3430 319 // functie y positie
cmaas 9:40c9a18c3430 320 float positiony(int EMGyplus,int EMGymin)
cmaas 9:40c9a18c3430 321 {
cmaas 9:40c9a18c3430 322 float EMGy = EMGyplus - EMGymin;
cmaas 9:40c9a18c3430 323
cmaas 9:40c9a18c3430 324 float verplaatsingy = EMGy * tijdstap * v;
cmaas 9:40c9a18c3430 325 float pynieuw = py + verplaatsingy;
cmaas 9:40c9a18c3430 326
cmaas 9:40c9a18c3430 327 // y limit
cmaas 9:40c9a18c3430 328 if (pynieuw <= upperylim && pynieuw >= lowerylim) {
cmaas 9:40c9a18c3430 329 py = pynieuw;
cmaas 9:40c9a18c3430 330 } else {
cmaas 9:40c9a18c3430 331 if (pynieuw >= lowerylim) {
cmaas 9:40c9a18c3430 332 py = upperylim;
cmaas 9:40c9a18c3430 333 } else {
cmaas 9:40c9a18c3430 334 py = lowerylim;
cmaas 9:40c9a18c3430 335 }
cmaas 9:40c9a18c3430 336 }
cmaas 9:40c9a18c3430 337 //printf("Y eindpunt (%f) en verplaatsing: (%f) \n\r",py,verplaatsingy);
cmaas 9:40c9a18c3430 338 return (py);
cmaas 9:40c9a18c3430 339 }
cmaas 9:40c9a18c3430 340
cmaas 9:40c9a18c3430 341
cmaas 9:40c9a18c3430 342 //~~~~~~~~~~~~CALCULATIING MOTOR ANGLES ~~~~~~~~
cmaas 9:40c9a18c3430 343 // arm 1 --> reference angle motor 1
cmaas 9:40c9a18c3430 344 float hoek1(float px, float py) // input: ref x, ref y
cmaas 9:40c9a18c3430 345 {
cmaas 9:40c9a18c3430 346 float c1x = px - rp * cos(thetap +(M_PI/6)); // x locatie hoekpunt end-effector
cmaas 9:40c9a18c3430 347 float c1y = py - rp*sin(thetap+(M_PI/6)); // y locatie hoekpunt end-effector
cmaas 9:40c9a18c3430 348 float alpha1 = atan2((c1y-a1y),(c1x-a1x)); // hoek tussen horizontaal en lijn van motor naar bijbehorende end-effector punt
cmaas 9:40c9a18c3430 349 float psi1 = acos(( pow(la,2)-pow(lp,2)+pow((c1x-a1x),2)+pow((c1y-a1y),2))/(2*la*sqrt(pow ((c1x-a1x),2)+pow((c1y-a1y),2) ))); //Hoek tussen lijn van motor naar bijbehorende end=effector punt en actieve arm
cmaas 9:40c9a18c3430 350 float a1 = alpha1 + psi1 - da1; //hoek tussen horizontaal en actieve arm
cmaas 9:40c9a18c3430 351 //printf("arm 1 = %f \n\r",a1);
cmaas 9:40c9a18c3430 352 return a1;
cmaas 9:40c9a18c3430 353 }
cmaas 9:40c9a18c3430 354
cmaas 9:40c9a18c3430 355 // arm 2 --> reference angle motor 2
cmaas 9:40c9a18c3430 356 float hoek2(float px, float py)
cmaas 9:40c9a18c3430 357 {
cmaas 9:40c9a18c3430 358 float c2x = px - rp * cos(thetap -(M_PI/2));
cmaas 9:40c9a18c3430 359 float c2y = py - rp*sin(thetap-(M_PI/2));
cmaas 9:40c9a18c3430 360 float alpha2 = atan2((c2y-a2y),(c2x-a2x));
cmaas 9:40c9a18c3430 361 float psi2 = acos(( pow(la,2)-pow(lp,2)+pow((c2x-a2x),2)+pow((c2y-a2y),2))/(2*la*sqrt(pow ((c2x-a2x),2)+pow((c2y-a2y),2) )));
cmaas 9:40c9a18c3430 362 float a2 = alpha2 + psi2 - da2;
cmaas 9:40c9a18c3430 363 //printf("arm 2 = %f \n\r",a2);
cmaas 9:40c9a18c3430 364 return a2;
cmaas 9:40c9a18c3430 365 }
cmaas 9:40c9a18c3430 366
cmaas 9:40c9a18c3430 367 //arm 3 --> reference angle motor 3
cmaas 9:40c9a18c3430 368 float hoek3(float px, float py)
cmaas 9:40c9a18c3430 369 {
cmaas 9:40c9a18c3430 370 float c3x = px - rp * cos(thetap +(5*M_PI/6));
cmaas 9:40c9a18c3430 371 float c3y = py - rp*sin(thetap+(5*M_PI/6));
cmaas 9:40c9a18c3430 372 float alpha3 = atan2((c3y-a3y),(c3x-a3x));
cmaas 9:40c9a18c3430 373 float psi3 = acos(( pow(la,2)-pow(lp,2)+pow((c3x-a3x),2)+pow((c3y-a3y),2))/(2*la*sqrt(pow ((c3x-a3x),2)+pow((c3y-a3y),2) )));
cmaas 9:40c9a18c3430 374 float a3 = alpha3 + psi3 - da3;
cmaas 9:40c9a18c3430 375 //printf("arm 3 = %f \n\r",a3);
cmaas 9:40c9a18c3430 376 return a3;
cmaas 9:40c9a18c3430 377 }
cmaas 9:40c9a18c3430 378
cmaas 9:40c9a18c3430 379 // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~
cmaas 9:40c9a18c3430 380
cmaas 9:40c9a18c3430 381 double PID_controller(double error)
cmaas 9:40c9a18c3430 382 {
cmaas 9:40c9a18c3430 383 static double error_integral = 0;
cmaas 9:40c9a18c3430 384 static double error_prev = error; // initialization with this value only done once!
cmaas 9:40c9a18c3430 385 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
cmaas 9:40c9a18c3430 386
cmaas 9:40c9a18c3430 387 // Proportional part:
cmaas 9:40c9a18c3430 388 double u_k = Kp * error;
cmaas 9:40c9a18c3430 389
cmaas 9:40c9a18c3430 390 // Integral part
cmaas 9:40c9a18c3430 391 error_integral = error_integral + error * Ts;
cmaas 9:40c9a18c3430 392 double u_i = Ki * error_integral;
cmaas 9:40c9a18c3430 393
cmaas 9:40c9a18c3430 394 // Derivative part
cmaas 9:40c9a18c3430 395 double error_derivative = (error - error_prev)/Ts;
cmaas 9:40c9a18c3430 396 double filtered_error_derivative = LowPassFilter.step(error_derivative);
cmaas 9:40c9a18c3430 397 double u_d = Kd * filtered_error_derivative;
cmaas 9:40c9a18c3430 398 error_prev = error;
cmaas 9:40c9a18c3430 399
cmaas 9:40c9a18c3430 400 // Sum all parts and return it
cmaas 9:40c9a18c3430 401 return u_k + u_i + u_d;
cmaas 9:40c9a18c3430 402 }
cmaas 9:40c9a18c3430 403
cmaas 9:40c9a18c3430 404
cmaas 9:40c9a18c3430 405 // DIRECTON AND SPEED CONTROL
cmaas 9:40c9a18c3430 406 void moter_control(double u)
cmaas 9:40c9a18c3430 407 {
cmaas 9:40c9a18c3430 408
cmaas 9:40c9a18c3430 409 directionpin1= u > 0.0f; //eithertrueor false
cmaas 9:40c9a18c3430 410 if (fabs(u)> 0.7f) {
cmaas 9:40c9a18c3430 411 u = 0.7f;
cmaas 9:40c9a18c3430 412 } else {
cmaas 9:40c9a18c3430 413 u= u;
cmaas 9:40c9a18c3430 414 }
cmaas 9:40c9a18c3430 415 pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
cmaas 9:40c9a18c3430 416 }
cmaas 9:40c9a18c3430 417
cmaas 9:40c9a18c3430 418 void moter2_control(double u)
cmaas 9:40c9a18c3430 419 {
cmaas 9:40c9a18c3430 420 directionpin2= u > 0.0f; //eithertrueor false
cmaas 9:40c9a18c3430 421 if (fabs(u)> 0.7f) {
cmaas 9:40c9a18c3430 422 u = 0.7f;
cmaas 9:40c9a18c3430 423 } else {
cmaas 9:40c9a18c3430 424 u= u;
cmaas 9:40c9a18c3430 425 }
cmaas 9:40c9a18c3430 426 pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
cmaas 9:40c9a18c3430 427 }
cmaas 9:40c9a18c3430 428
cmaas 9:40c9a18c3430 429 void moter3_control(double u)
cmaas 9:40c9a18c3430 430 {
cmaas 9:40c9a18c3430 431 directionpin3= u > 0.0f; //eithertrueor false
cmaas 9:40c9a18c3430 432 if (fabs(u)> 0.7f) {
cmaas 9:40c9a18c3430 433 u = 0.7f;
cmaas 9:40c9a18c3430 434 } else {
cmaas 9:40c9a18c3430 435 u= u;
cmaas 9:40c9a18c3430 436 }
cmaas 9:40c9a18c3430 437 pwmpin3 = fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
cmaas 9:40c9a18c3430 438 }
cmaas 9:40c9a18c3430 439
cmaas 9:40c9a18c3430 440 // CONTROLLING THE MOTOR
cmaas 9:40c9a18c3430 441 void Motor_mover()
cmaas 9:40c9a18c3430 442 {
cmaas 13:0e25698dce40 443 float px = positionx(bicepsR,bicepsL); // EMG: +x, -x
cmaas 13:0e25698dce40 444 float py = positiony(tricepsL,tricepsL); // EMG: +y, -y
cmaas 13:0e25698dce40 445
cmaas 9:40c9a18c3430 446 double motor_position = encoder1.getPulses(); //output in counts
cmaas 9:40c9a18c3430 447 double reference_rotation = hoek1(px, py);
cmaas 9:40c9a18c3430 448 double error = reference_rotation - motor_position*(2*PI)/8400;
cmaas 9:40c9a18c3430 449 double u = PID_controller(error);
cmaas 9:40c9a18c3430 450 moter_control(u);
cmaas 9:40c9a18c3430 451
cmaas 9:40c9a18c3430 452 double motor_position2 = encoder2.getPulses(); //output in counts
cmaas 9:40c9a18c3430 453 double reference_rotation2 = hoek2(px, py);
cmaas 9:40c9a18c3430 454 double error_2 = reference_rotation2 - motor_position2*(2*PI)/8400;
cmaas 9:40c9a18c3430 455 double u_2 = PID_controller(error_2);
cmaas 9:40c9a18c3430 456 moter2_control(u_2);
cmaas 9:40c9a18c3430 457
cmaas 9:40c9a18c3430 458 double motor_position3 = encoder3.getPulses(); //output in counts
cmaas 9:40c9a18c3430 459 double reference_rotation3 = hoek3(px, py);
cmaas 9:40c9a18c3430 460 double error_3 = reference_rotation3 - motor_position3*(2*PI)/8400;
cmaas 9:40c9a18c3430 461 double u_3 = PID_controller(error_3);
cmaas 9:40c9a18c3430 462 moter3_control(u_3);
cmaas 9:40c9a18c3430 463
cmaas 9:40c9a18c3430 464
cmaas 9:40c9a18c3430 465 }
cmaas 9:40c9a18c3430 466
cmaas 13:0e25698dce40 467 //Activate ticker for Movement state, filtering and Threshold checking
cmaas 13:0e25698dce40 468 void emg_sample_ticker(){
cmaas 13:0e25698dce40 469 sample_ticker.attach(&emgsample, ts);
cmaas 13:0e25698dce40 470 //sample_ticker.attach(&Motor_mover, ts);
cmaas 13:0e25698dce40 471
cmaas 13:0e25698dce40 472 }
cmaas 13:0e25698dce40 473 void movement_ticker_activator()
cmaas 13:0e25698dce40 474 {
cmaas 13:0e25698dce40 475 sample_ticker.attach(&emgsample, ts);
cmaas 13:0e25698dce40 476 sample_ticker.attach(&threshold_check, ts);
cmaas 13:0e25698dce40 477 }
cmaas 13:0e25698dce40 478 void movement_ticker_deactivator()
cmaas 13:0e25698dce40 479 {
cmaas 13:0e25698dce40 480 sample_ticker.detach();
cmaas 13:0e25698dce40 481 //sample_ticker_ticker.detach();
cmaas 13:0e25698dce40 482 }
cmaas 9:40c9a18c3430 483
cmaas 9:40c9a18c3430 484 //-------------------- STATE MACHINE --------------------------
gastongab 7:88fa84742b3c 485 enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
gastongab 2:0a8622662f6d 486 states currentState = MOTORS_OFF; //Chosen startingposition for states
aschut 0:90750f158475 487 bool stateChanged = true; // Make sure the initialization of first state is executed
aschut 0:90750f158475 488
aschut 0:90750f158475 489 void ProcessStateMachine(void)
aschut 0:90750f158475 490 {
cmaas 13:0e25698dce40 491
gastongab 7:88fa84742b3c 492 switch (currentState) {
gastongab 7:88fa84742b3c 493 case MOTORS_OFF:
gastongab 7:88fa84742b3c 494 // Actions
gastongab 7:88fa84742b3c 495 if (stateChanged) {
gastongab 7:88fa84742b3c 496 // state initialization: rood
gastongab 7:88fa84742b3c 497 led1 = 1;
gastongab 7:88fa84742b3c 498 led2 = 0;
gastongab 7:88fa84742b3c 499 led3 = 1;
gastongab 7:88fa84742b3c 500 wait (1);
gastongab 7:88fa84742b3c 501 stateChanged = false;
gastongab 7:88fa84742b3c 502 }
gastongab 7:88fa84742b3c 503
gastongab 7:88fa84742b3c 504 // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
gastongab 7:88fa84742b3c 505 if (!button1) {
gastongab 7:88fa84742b3c 506 currentState = CALIBRATION;
gastongab 7:88fa84742b3c 507 stateChanged = true;
gastongab 7:88fa84742b3c 508 } else if (!button2) {
gastongab 7:88fa84742b3c 509 currentState = HOMING ;
gastongab 7:88fa84742b3c 510 stateChanged = true;
gastongab 7:88fa84742b3c 511 } else {
gastongab 7:88fa84742b3c 512 currentState = MOTORS_OFF;
gastongab 7:88fa84742b3c 513 stateChanged = true;
gastongab 7:88fa84742b3c 514 }
gastongab 7:88fa84742b3c 515
gastongab 7:88fa84742b3c 516 break;
gastongab 7:88fa84742b3c 517
gastongab 7:88fa84742b3c 518 case CALIBRATION:
gastongab 7:88fa84742b3c 519 // Actions
gastongab 7:88fa84742b3c 520 if (stateChanged) {
gastongab 7:88fa84742b3c 521 // state initialization: oranje
gastongab 7:88fa84742b3c 522 temp_highest_emg1 = 0; //highest detected value right biceps
gastongab 7:88fa84742b3c 523 temp_highest_emg2 = 0;
gastongab 7:88fa84742b3c 524 temp_highest_emg3 = 0;
gastongab 7:88fa84742b3c 525 temp_highest_emg4 = 0;
gastongab 7:88fa84742b3c 526
gastongab 7:88fa84742b3c 527 timer_calibration.reset();
gastongab 7:88fa84742b3c 528 timer_calibration.start();
gastongab 7:88fa84742b3c 529
cmaas 13:0e25698dce40 530
gastongab 7:88fa84742b3c 531 CalibrationEMG();
gastongab 7:88fa84742b3c 532 timer_calibration.stop();
aschut 0:90750f158475 533
gastongab 7:88fa84742b3c 534
gastongab 7:88fa84742b3c 535 stateChanged = false;
gastongab 7:88fa84742b3c 536 }
gastongab 7:88fa84742b3c 537
gastongab 7:88fa84742b3c 538 // State transition logic: automatisch terug naar motors off.
gastongab 7:88fa84742b3c 539
gastongab 7:88fa84742b3c 540 currentState = MOTORS_OFF;
gastongab 7:88fa84742b3c 541 stateChanged = true;
gastongab 7:88fa84742b3c 542 break;
gastongab 7:88fa84742b3c 543
gastongab 7:88fa84742b3c 544 case HOMING:
gastongab 7:88fa84742b3c 545 // Actions
gastongab 7:88fa84742b3c 546 if (stateChanged) {
gastongab 7:88fa84742b3c 547 // state initialization: green
gastongab 7:88fa84742b3c 548 t.reset();
gastongab 7:88fa84742b3c 549 t.start();
gastongab 7:88fa84742b3c 550 led1 = 0;
gastongab 7:88fa84742b3c 551 led2 = 1;
gastongab 7:88fa84742b3c 552 led3 = 1;
gastongab 7:88fa84742b3c 553 wait (1);
gastongab 7:88fa84742b3c 554
gastongab 7:88fa84742b3c 555 stateChanged = false;
gastongab 7:88fa84742b3c 556 }
gastongab 7:88fa84742b3c 557
gastongab 7:88fa84742b3c 558 // State transition logic: naar DEMO (button1), naar MOVEMENT(button2)
gastongab 7:88fa84742b3c 559 if (!button1) {
gastongab 7:88fa84742b3c 560 currentState = DEMO;
gastongab 7:88fa84742b3c 561 stateChanged = true;
gastongab 7:88fa84742b3c 562 } else if (!button2) {
gastongab 7:88fa84742b3c 563 currentState = MOVEMENT ;
gastongab 7:88fa84742b3c 564 stateChanged = true;
gastongab 7:88fa84742b3c 565 } else if (t>300) {
gastongab 7:88fa84742b3c 566 t.stop();
gastongab 7:88fa84742b3c 567 t.reset();
gastongab 7:88fa84742b3c 568 currentState = MOTORS_OFF ;
gastongab 7:88fa84742b3c 569 stateChanged = true;
gastongab 7:88fa84742b3c 570 } else {
gastongab 7:88fa84742b3c 571 currentState = HOMING ;
gastongab 7:88fa84742b3c 572 stateChanged = true;
gastongab 7:88fa84742b3c 573 }
gastongab 7:88fa84742b3c 574 break;
gastongab 7:88fa84742b3c 575
aschut 0:90750f158475 576 case DEMO:
gastongab 7:88fa84742b3c 577 // Actions
gastongab 7:88fa84742b3c 578 if (stateChanged) {
gastongab 7:88fa84742b3c 579 // state initialization: light blue
gastongab 7:88fa84742b3c 580 led1 = 0;
gastongab 7:88fa84742b3c 581 led2 = 1;
gastongab 7:88fa84742b3c 582 led3 = 0;
cmaas 13:0e25698dce40 583 //wait (1);
gastongab 7:88fa84742b3c 584
gastongab 7:88fa84742b3c 585 stateChanged = false;
gastongab 7:88fa84742b3c 586 }
gastongab 7:88fa84742b3c 587
gastongab 7:88fa84742b3c 588 // State transition logic: automatisch terug naar HOMING
gastongab 7:88fa84742b3c 589 currentState = HOMING;
gastongab 7:88fa84742b3c 590 stateChanged = true;
gastongab 7:88fa84742b3c 591 break;
gastongab 7:88fa84742b3c 592
gastongab 7:88fa84742b3c 593 case MOVEMENT:
gastongab 7:88fa84742b3c 594 // Actions
gastongab 7:88fa84742b3c 595 if (stateChanged) {
gastongab 7:88fa84742b3c 596 // state initialization: purple
gastongab 7:88fa84742b3c 597 //t.reset();
gastongab 7:88fa84742b3c 598 //t.start();
gastongab 7:88fa84742b3c 599
gastongab 7:88fa84742b3c 600 led1 = 1;
gastongab 7:88fa84742b3c 601 led2 = 0;
gastongab 7:88fa84742b3c 602 led3 = 0;
gastongab 7:88fa84742b3c 603 wait(2);
gastongab 7:88fa84742b3c 604
gastongab 7:88fa84742b3c 605 movement_ticker_activator();
gastongab 7:88fa84742b3c 606
gastongab 7:88fa84742b3c 607 led1 = 0;
gastongab 7:88fa84742b3c 608 led2 = 0;
gastongab 7:88fa84742b3c 609 led3 = 0;
gastongab 7:88fa84742b3c 610 wait(2);
gastongab 7:88fa84742b3c 611
gastongab 7:88fa84742b3c 612
gastongab 7:88fa84742b3c 613 stateChanged = false;
gastongab 7:88fa84742b3c 614 }
gastongab 7:88fa84742b3c 615
gastongab 7:88fa84742b3c 616 // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
gastongab 7:88fa84742b3c 617 if (!button1) {
gastongab 7:88fa84742b3c 618 movement_ticker_deactivator();
gastongab 7:88fa84742b3c 619 currentState = CLICK;
gastongab 7:88fa84742b3c 620 stateChanged = true;
gastongab 7:88fa84742b3c 621 } else if (!button2) {
gastongab 7:88fa84742b3c 622 movement_ticker_deactivator();
gastongab 7:88fa84742b3c 623 currentState = MOTORS_OFF ;
gastongab 7:88fa84742b3c 624 stateChanged = true;
gastongab 7:88fa84742b3c 625 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0) { //this check if person is idle for more than 300seconds
gastongab 7:88fa84742b3c 626 t.start();
gastongab 7:88fa84742b3c 627 } else if (bicepsR==1 || tricepsR==1 || bicepsL==1 || tricepsL==1) {
gastongab 7:88fa84742b3c 628 t.stop();
gastongab 7:88fa84742b3c 629 t.reset();
gastongab 7:88fa84742b3c 630 }
gastongab 7:88fa84742b3c 631
gastongab 7:88fa84742b3c 632 if(t>20) {
gastongab 7:88fa84742b3c 633 movement_ticker_deactivator();
gastongab 7:88fa84742b3c 634 t.stop();
gastongab 7:88fa84742b3c 635 t.reset();
gastongab 7:88fa84742b3c 636 currentState = HOMING ;
gastongab 7:88fa84742b3c 637 stateChanged = true;
gastongab 7:88fa84742b3c 638 }
gastongab 7:88fa84742b3c 639 // here ends the idle checking mode
gastongab 7:88fa84742b3c 640 else {
gastongab 7:88fa84742b3c 641 //For every muscle a different colour if threshold is passed
gastongab 7:88fa84742b3c 642 if(bicepsR==1) {
gastongab 7:88fa84742b3c 643 led1 = 0;
gastongab 7:88fa84742b3c 644 led2 = 1;
gastongab 7:88fa84742b3c 645 led3 = 1;
gastongab 7:88fa84742b3c 646 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
gastongab 7:88fa84742b3c 647 led1 = 1;
gastongab 7:88fa84742b3c 648 led2 = 1;
gastongab 7:88fa84742b3c 649 led3 = 1;
gastongab 7:88fa84742b3c 650 }
gastongab 7:88fa84742b3c 651 if(tricepsR==1) {
gastongab 7:88fa84742b3c 652 led1 = 1;
gastongab 7:88fa84742b3c 653 led2 = 0;
gastongab 7:88fa84742b3c 654 led3 = 1;
gastongab 7:88fa84742b3c 655 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
gastongab 7:88fa84742b3c 656 led1 = 1;
gastongab 7:88fa84742b3c 657 led2 = 1;
gastongab 7:88fa84742b3c 658 led3 = 1;
gastongab 7:88fa84742b3c 659 }
gastongab 7:88fa84742b3c 660 if(bicepsL==1) {
gastongab 7:88fa84742b3c 661 led1 = 1;
gastongab 7:88fa84742b3c 662 led2 = 1;
gastongab 7:88fa84742b3c 663 led3 = 0;
gastongab 7:88fa84742b3c 664 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
gastongab 7:88fa84742b3c 665 led1 = 1;
gastongab 7:88fa84742b3c 666 led2 = 1;
gastongab 7:88fa84742b3c 667 led3 = 1;
gastongab 7:88fa84742b3c 668 }
gastongab 7:88fa84742b3c 669 if(tricepsL==1) {
gastongab 7:88fa84742b3c 670 led1 = 1;
gastongab 7:88fa84742b3c 671 led2 = 0;
gastongab 7:88fa84742b3c 672 led3 = 0;
gastongab 7:88fa84742b3c 673 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
gastongab 7:88fa84742b3c 674 led1 = 1;
gastongab 7:88fa84742b3c 675 led2 = 1;
gastongab 7:88fa84742b3c 676 led3 = 1;
gastongab 7:88fa84742b3c 677 }
gastongab 7:88fa84742b3c 678 }
gastongab 7:88fa84742b3c 679
gastongab 7:88fa84742b3c 680 break;
gastongab 7:88fa84742b3c 681
aschut 0:90750f158475 682 case CLICK:
gastongab 7:88fa84742b3c 683 // Actions
gastongab 7:88fa84742b3c 684 if (stateChanged) {
gastongab 7:88fa84742b3c 685 // state initialization: blue
gastongab 7:88fa84742b3c 686 led1 = 1;
gastongab 7:88fa84742b3c 687 led2 = 1;
gastongab 7:88fa84742b3c 688 led3 = 0;
gastongab 7:88fa84742b3c 689 wait (1);
aschut 0:90750f158475 690
gastongab 7:88fa84742b3c 691 stateChanged = false;
gastongab 7:88fa84742b3c 692 }
gastongab 7:88fa84742b3c 693
gastongab 7:88fa84742b3c 694 // State transition logic: automatisch terug naar MOVEMENT.
gastongab 7:88fa84742b3c 695
gastongab 7:88fa84742b3c 696 currentState = MOVEMENT;
gastongab 7:88fa84742b3c 697 stateChanged = true;
gastongab 7:88fa84742b3c 698 break;
gastongab 7:88fa84742b3c 699
gastongab 4:c7be673eb4a1 700 }
aschut 0:90750f158475 701 }
gastongab 7:88fa84742b3c 702
cmaas 9:40c9a18c3430 703 // --------------------------MAIN--------------------
cmaas 9:40c9a18c3430 704
cmaas 9:40c9a18c3430 705
aschut 0:90750f158475 706 int main()
aschut 0:90750f158475 707 {
cmaas 13:0e25698dce40 708
gastongab 4:c7be673eb4a1 709 //BiQuad Chain add
gastongab 4:c7be673eb4a1 710 highp1.add( &highp1_1 ).add( &highp1_2 );
gastongab 4:c7be673eb4a1 711 notch1.add( &notch1_1 ).add( &notch1_2 );
gastongab 4:c7be673eb4a1 712 lowp1.add( &lowp1_1 ).add(&lowp1_2);
gastongab 7:88fa84742b3c 713
gastongab 4:c7be673eb4a1 714 highp2.add( &highp2_1 ).add( &highp2_2 );
gastongab 4:c7be673eb4a1 715 notch2.add( &notch2_1 ).add( &notch2_2 );
gastongab 7:88fa84742b3c 716 lowp2.add( &lowp2_1 ).add(&lowp2_2);
gastongab 7:88fa84742b3c 717
gastongab 4:c7be673eb4a1 718 highp3.add( &highp3_1 ).add( &highp3_2 );
gastongab 4:c7be673eb4a1 719 notch3.add( &notch3_1 ).add( &notch3_2 );
gastongab 4:c7be673eb4a1 720 lowp3.add( &lowp3_1 ).add(&lowp3_2);
gastongab 7:88fa84742b3c 721
gastongab 4:c7be673eb4a1 722 highp4.add( &highp4_1 ).add( &highp4_2 );
gastongab 4:c7be673eb4a1 723 notch4.add( &notch4_1 ).add( &notch4_2 );
gastongab 4:c7be673eb4a1 724 lowp4.add( &lowp4_1 ).add(&lowp4_2);
gastongab 7:88fa84742b3c 725
gastongab 2:0a8622662f6d 726 pc.baud(115200);
aschut 0:90750f158475 727 led1 = 1;
aschut 0:90750f158475 728 led2 = 1;
aschut 0:90750f158475 729 led3 = 1;
cmaas 9:40c9a18c3430 730
cmaas 9:40c9a18c3430 731 pwmpin1.period_us(60); // setup motor
cmaas 13:0e25698dce40 732 //ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE
cmaas 13:0e25698dce40 733 //movement_ticker_activator();
cmaas 13:0e25698dce40 734 emg_sample_ticker();
gastongab 7:88fa84742b3c 735 while (true) {
cmaas 13:0e25698dce40 736 ProcessStateMachine();
cmaas 13:0e25698dce40 737
cmaas 13:0e25698dce40 738 /*
cmaas 10:2325e545ce11 739 if (button2 == false) {
cmaas 10:2325e545ce11 740 wait(0.01f);
cmaas 10:2325e545ce11 741
cmaas 10:2325e545ce11 742 // berekenen positie
cmaas 10:2325e545ce11 743 float px = positionx(1,0); // EMG: +x, -x
cmaas 10:2325e545ce11 744 float py = positiony(0,0); // EMG: +y, -y
cmaas 10:2325e545ce11 745 //printf("positie (%f,%f)\n\r",px,py);
cmaas 10:2325e545ce11 746 }
cmaas 10:2325e545ce11 747
cmaas 10:2325e545ce11 748 if (button1 == false) {
cmaas 10:2325e545ce11 749 wait(0.01f);
cmaas 10:2325e545ce11 750 // berekenen positie
cmaas 10:2325e545ce11 751 float px = positionx(0,1); // EMG: +x, -x
cmaas 10:2325e545ce11 752 float py = positiony(0,0); // EMG: +y, -y
cmaas 10:2325e545ce11 753 //printf("positie (%f,%f)\n\r",px,py);
cmaas 10:2325e545ce11 754 }
cmaas 12:99e29b8d4155 755 /*
cmaas 10:2325e545ce11 756 if (button3 == false) {
cmaas 10:2325e545ce11 757 wait(0.01f);
cmaas 10:2325e545ce11 758 // berekenen positie
cmaas 10:2325e545ce11 759 float px = positionx(0,0); // EMG: +x, -x
cmaas 10:2325e545ce11 760 float py = positiony(1,0); // EMG: +y, -y
cmaas 10:2325e545ce11 761 //printf("positie (%f,%f)\n\r",px,py);
cmaas 10:2325e545ce11 762 }
cmaas 10:2325e545ce11 763
cmaas 10:2325e545ce11 764 if (button4 == false) {
cmaas 10:2325e545ce11 765 wait(0.01f);
cmaas 10:2325e545ce11 766 // berekenen positie
cmaas 10:2325e545ce11 767 float px = positionx(0,0); // EMG: +x, -x
cmaas 10:2325e545ce11 768 float py = positiony(0,1); // EMG: +y, -y
cmaas 10:2325e545ce11 769 //printf("positie (%f,%f)\n\r",px,py);
cmaas 10:2325e545ce11 770 }
cmaas 12:99e29b8d4155 771 */
aschut 0:90750f158475 772 }
gastongab 7:88fa84742b3c 773
aschut 0:90750f158475 774 }
aschut 0:90750f158475 775
aschut 0:90750f158475 776
aschut 0:90750f158475 777
aschut 0:90750f158475 778
aschut 0:90750f158475 779
aschut 0:90750f158475 780