Gaston Gabriël / Mbed 2 deprecated StateMachine_EMg_RKI_PID_MOTORwerkend_10milli

This is with minumum of 10 milliseconds threshold delay 00:53

Dependencies:   biquadFilter MODSERIAL QEI Servo mbed

Fork of StateMachine_EMg_RKI_PID_MOTOR_metklikenalles by Gaston Gabriël

main.cpp@18:9d59ee0e459f, 2018-11-02 (annotated)

Committer:
gastongab
Date:
Fri Nov 02 07:14:51 2018 +0000
Revision:
18:9d59ee0e459f
Parent:
17:008587f85301 
Finalversion 2-11-2018

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