Thomas Burgers / Mbed 2 deprecated ZZ-TheChenneRobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Encoder HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@33:5386ccaa5160, 2015-10-13 (annotated)

Committer:
ThomasBNL
Date:
Tue Oct 13 20:39:38 2015 +0000
Revision:
33:5386ccaa5160
Parent:
32:10e6160fdbaa 
ACTION controller (version 2) : changed some constant variables name to match with EMG measuring script

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomasBNL 0:40052f5ca77b 1 #include "mbed.h"
ThomasBNL 21:c75210216204 2 #include "HIDScope.h"
ThomasBNL 0:40052f5ca77b 3 #include "QEI.h"
ThomasBNL 0:40052f5ca77b 4 #include "MODSERIAL.h"
ThomasBNL 8:50d6e2323d3b 5 #include "biquadFilter.h"
ThomasBNL 0:40052f5ca77b 6 #include "encoder.h"
ThomasBNL 0:40052f5ca77b 7
ThomasBNL 24:8ca471562212 8 MODSERIAL pc(USBTX,USBRX);
ThomasBNL 28:482ac040fb0d 9
ThomasBNL 28:482ac040fb0d 10 // (DEBUGGING AND TESTING Tools) (0 when pressed and 1 when not pressed) //
ThomasBNL 8:50d6e2323d3b 11 DigitalIn buttonL1(PTC6); // Button 1 (laag op board) for testing at the lower board
ThomasBNL 8:50d6e2323d3b 12 DigitalIn buttonL2(PTA4); // Button 2 (laag op board) for testing at the lower board
ThomasBNL 8:50d6e2323d3b 13 DigitalIn buttonH1(D2); // Button 3 (hoog op board) for testing at the top board
ThomasBNL 8:50d6e2323d3b 14 DigitalIn buttonH2(D6); // Button 4 (hoog op board) for testing at the top board
ThomasBNL 0:40052f5ca77b 15
ThomasBNL 28:482ac040fb0d 16 DigitalOut debug_led_red(LED_RED); // Debug LED
ThomasBNL 28:482ac040fb0d 17 DigitalOut debug_led_green(LED_GREEN); // Debug LED
ThomasBNL 28:482ac040fb0d 18 DigitalOut debug_led_blue(LED_BLUE); // Debug LED
ThomasBNL 8:50d6e2323d3b 19
ThomasBNL 28:482ac040fb0d 20 HIDScope scope(2); // HIDSCOPE declared
ThomasBNL 28:482ac040fb0d 21 Ticker Hidscope_measure;
ThomasBNL 28:482ac040fb0d 22
ThomasBNL 28:482ac040fb0d 23 AnalogIn potmeter1(A0); // POTMETER -> DOEN ALSOF EMG SIGNAAL
ThomasBNL 28:482ac040fb0d 24 AnalogIn potmeter2(A1); // POTMETER -> DOEN ALSOF EMG SIGNAAL
ThomasBNL 8:50d6e2323d3b 25
ThomasBNL 33:5386ccaa5160 26 double EMG_L_max = 100;
ThomasBNL 28:482ac040fb0d 27 // Defining constants //
ThomasBNL 28:482ac040fb0d 28 volatile bool send_flag;
ThomasBNL 28:482ac040fb0d 29 const double cw=0; // zero is clockwise (front view)
ThomasBNL 28:482ac040fb0d 30 const double ccw=1; // one is counterclockwise (front view)
ThomasBNL 28:482ac040fb0d 31 const double off=1; //Led off
ThomasBNL 28:482ac040fb0d 32 const double on=0; //Led on
ThomasBNL 28:482ac040fb0d 33 const double sample_time=0.005; // tijd voor een sample (200Hz)
ThomasBNL 28:482ac040fb0d 34
ThomasBNL 28:482ac040fb0d 35
ThomasBNL 28:482ac040fb0d 36 // Functions used (described after main) //
ThomasBNL 28:482ac040fb0d 37 void send(void);
ThomasBNL 28:482ac040fb0d 38 void measure_Hidscope(void);
ThomasBNL 28:482ac040fb0d 39 void deactivate_PID_Controller_strike(void);
ThomasBNL 28:482ac040fb0d 40 void activate_PID_Controller_strike(void);
ThomasBNL 28:482ac040fb0d 41
ThomasBNL 28:482ac040fb0d 42
ThomasBNL 28:482ac040fb0d 43 // MAIN function //
ThomasBNL 28:482ac040fb0d 44
ThomasBNL 0:40052f5ca77b 45 int main() {
ThomasBNL 28:482ac040fb0d 46 QEI motor_turn(D12,D13,NC,32); // Encoder for motor Turn
ThomasBNL 28:482ac040fb0d 47 PwmOut pwm_motor_strike(D5); // Pwm for motor Turn
ThomasBNL 28:482ac040fb0d 48 DigitalOut motordirection_strike(D4); // Direction of the motor Turn
ThomasBNL 27:b7caf668a682 49
ThomasBNL 32:10e6160fdbaa 50 double reference_turn; // Set constant to store reference value of the Turn motor
ThomasBNL 28:482ac040fb0d 51 double position_turn; // Set constant to store current position of the Turn motor
ThomasBNL 28:482ac040fb0d 52 double position_strike;
ThomasBNL 33:5386ccaa5160 53 float EMG_Right_Bicep_filtered;
ThomasBNL 33:5386ccaa5160 54 float EMG_Left_Bicep_filtered;
ThomasBNL 28:482ac040fb0d 55 double n;
ThomasBNL 28:482ac040fb0d 56 debug_led_red=off;
ThomasBNL 28:482ac040fb0d 57 debug_led_blue=off;
ThomasBNL 28:482ac040fb0d 58 debug_led_green=off;
ThomasBNL 33:5386ccaa5160 59 double EMG_L_max;
ThomasBNL 33:5386ccaa5160 60 double EMG_L_min;
ThomasBNL 33:5386ccaa5160 61 double EMG_R_max;
ThomasBNL 33:5386ccaa5160 62 double EMG_R_min;
ThomasBNL 31:113f630f7e7d 63 double Hit;
ThomasBNL 28:482ac040fb0d 64
ThomasBNL 28:482ac040fb0d 65 // START OF CODE //
ThomasBNL 30:176ca1193a0a 66 pc.printf(" Start of code \n\r");
ThomasBNL 0:40052f5ca77b 67
ThomasBNL 32:10e6160fdbaa 68 reference_turn=0;
ThomasBNL 32:10e6160fdbaa 69
ThomasBNL 29:263c680068db 70 pc.baud(115200); // Set the baudrate
ThomasBNL 0:40052f5ca77b 71
ThomasBNL 28:482ac040fb0d 72 // Calibratie //
ThomasBNL 33:5386ccaa5160 73 EMG_L_max = 100; // Calibreren (max average over 5 seconde?) gemeten integraal EMG over tijd / (tijdsample stappen)=100
ThomasBNL 33:5386ccaa5160 74 EMG_L_min = 0;
ThomasBNL 33:5386ccaa5160 75 EMG_R_max = 100; // Calibreren
ThomasBNL 33:5386ccaa5160 76 EMG_R_min = 0;
ThomasBNL 31:113f630f7e7d 77 Hit=60; // position when bottle is hit
ThomasBNL 28:482ac040fb0d 78
ThomasBNL 31:113f630f7e7d 79
ThomasBNL 33:5386ccaa5160 80 const float Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2)+EMG_L_min;; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
ThomasBNL 33:5386ccaa5160 81 const float Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6)+EMG_L_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
ThomasBNL 33:5386ccaa5160 82 const float Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
ThomasBNL 33:5386ccaa5160 83 const float Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
ThomasBNL 31:113f630f7e7d 84 const float change_one_bottle=45; //(45 graden change)
ThomasBNL 30:176ca1193a0a 85
ThomasBNL 31:113f630f7e7d 86 pc.printf("left 1: %f left 2: %f right 1: %f right 2: %f \n\r", Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2);
ThomasBNL 0:40052f5ca77b 87
ThomasBNL 30:176ca1193a0a 88 // Tickers //
ThomasBNL 29:263c680068db 89 Hidscope_measure.attach(send, sample_time); // GAAT NOG NIET GOED WAARSCHIJNLIJK ALS EEN WHILE LOOP WORDT UTIGEVOERD
ThomasBNL 14:599896acf576 90
ThomasBNL 28:482ac040fb0d 91 pc.printf("wait \n\r");
ThomasBNL 28:482ac040fb0d 92 wait (3); // Wait before starting system
ThomasBNL 28:482ac040fb0d 93 pc.printf("start control \n\r");
ThomasBNL 0:40052f5ca77b 94
ThomasBNL 30:176ca1193a0a 95 //INFINITE LOOP //
ThomasBNL 5:8fb74a22fe3c 96 while(1)
ThomasBNL 28:482ac040fb0d 97 { // Start while loop (ACTION loop)
ThomasBNL 28:482ac040fb0d 98 // INPUT Filtered EMG SIGNAAL
ThomasBNL 27:b7caf668a682 99
ThomasBNL 28:482ac040fb0d 100 Nieuwe_actie: // start here again when action has finished
ThomasBNL 28:482ac040fb0d 101 debug_led_red=off;
ThomasBNL 28:482ac040fb0d 102 debug_led_blue=off;
ThomasBNL 28:482ac040fb0d 103 debug_led_green=on; // LED Turns green if ready for a new action
ThomasBNL 30:176ca1193a0a 104 wait(1);
ThomasBNL 33:5386ccaa5160 105 EMG_Right_Bicep_filtered = (potmeter1.read())*100; //EMG Right bicep (tussen nul en 100%)
ThomasBNL 33:5386ccaa5160 106 EMG_Left_Bicep_filtered = (potmeter2.read())*100; // EMG Left bicep (tussen nul en 100%)
ThomasBNL 33:5386ccaa5160 107 pc.printf("EMG_Left_Bicep_filtered = %f EMG_Right_Bicep_filtered = %f \n\r", EMG_Left_Bicep_filtered, EMG_Right_Bicep_filtered);
ThomasBNL 30:176ca1193a0a 108
ThomasBNL 0:40052f5ca77b 109
ThomasBNL 28:482ac040fb0d 110 // SLAG // (No LED -> 0.55s -> red on (turns on every time a new pwm is given) -> finish)
ThomasBNL 33:5386ccaa5160 111 if (EMG_Left_Bicep_filtered > Threshold_Bicep_Left_1 && EMG_Right_Bicep_filtered > Threshold_Bicep_Right_1)
ThomasBNL 27:b7caf668a682 112 {
ThomasBNL 28:482ac040fb0d 113
ThomasBNL 28:482ac040fb0d 114 debug_led_green=off;
ThomasBNL 28:482ac040fb0d 115 n=0;
ThomasBNL 28:482ac040fb0d 116 pc.printf("slag \n\r");
ThomasBNL 28:482ac040fb0d 117 wait(0.5);
ThomasBNL 28:482ac040fb0d 118
ThomasBNL 33:5386ccaa5160 119 while(EMG_Left_Bicep_filtered > Threshold_Bicep_Left_1 && EMG_Right_Bicep_filtered > Threshold_Bicep_Right_1) // Threshold statement still true after 0.5 seconds?
ThomasBNL 27:b7caf668a682 120 {
ThomasBNL 28:482ac040fb0d 121 if (n==0) //wordt maar 1 keer uitgevoerd
ThomasBNL 28:482ac040fb0d 122 {
ThomasBNL 28:482ac040fb0d 123 deactivate_PID_Controller_strike(); // function that sets P, I and D gain values to zero
ThomasBNL 28:482ac040fb0d 124 n=1;
ThomasBNL 28:482ac040fb0d 125 }
ThomasBNL 28:482ac040fb0d 126 debug_led_red=off;
ThomasBNL 30:176ca1193a0a 127 wait(0.10); // wait 20 samples
ThomasBNL 28:482ac040fb0d 128 debug_led_red=on;
ThomasBNL 33:5386ccaa5160 129 pwm_motor_strike=((EMG_Left_Bicep_filtered-EMG_L_min)+(EMG_Right_Bicep_filtered-EMG_R_min))/((EMG_L_max-EMG_L_min)+(EMG_R_max-EMG_R_min))*0.7 + 0.3; // min speed 0.3 en max 1
ThomasBNL 30:176ca1193a0a 130 wait(0.10); // wait 20 samples more (pwm changes per 0.1 seconds)
ThomasBNL 28:482ac040fb0d 131 motordirection_strike=cw; // towards bottle
ThomasBNL 28:482ac040fb0d 132
ThomasBNL 28:482ac040fb0d 133 if((position_strike-Hit)<2) // bottle is hit when position-hit <2 defrees
ThomasBNL 28:482ac040fb0d 134 {
ThomasBNL 28:482ac040fb0d 135 activate_PID_Controller_strike(); // function sets back P, I and D gain values
ThomasBNL 28:482ac040fb0d 136 pc.printf("einde \n\r");
ThomasBNL 28:482ac040fb0d 137 goto Nieuwe_actie; // Finished: Get Ready for new Action control
ThomasBNL 27:b7caf668a682 138 }
ThomasBNL 27:b7caf668a682 139 }
ThomasBNL 28:482ac040fb0d 140 }
ThomasBNL 27:b7caf668a682 141
ThomasBNL 28:482ac040fb0d 142 activate_PID_Controller_strike(); // function sets back P, I and D gain values
ThomasBNL 28:482ac040fb0d 143
ThomasBNL 28:482ac040fb0d 144 debug_led_red=off; // not inside an action loop (green light)
ThomasBNL 28:482ac040fb0d 145 debug_led_blue=off;
ThomasBNL 28:482ac040fb0d 146 debug_led_green=on; // want het kan zijn dat bovenste script half wordt uitgevoerd en dan moet het slag mechanisme wel weer terug
ThomasBNL 3:11a7da46e093 147
ThomasBNL 28:482ac040fb0d 148 // DRAAI LINKS // // (blue->blue off (very short/visible?)-> red<->blue<-> red -> klaar)
ThomasBNL 33:5386ccaa5160 149 if (EMG_Left_Bicep_filtered > Threshold_Bicep_Left_2 && EMG_Right_Bicep_filtered < Threshold_Bicep_Right_1)
ThomasBNL 27:b7caf668a682 150 {
ThomasBNL 28:482ac040fb0d 151 debug_led_green=off; // Executing action
ThomasBNL 27:b7caf668a682 152 n=0;
ThomasBNL 28:482ac040fb0d 153 pc.printf("links \n\r");
ThomasBNL 33:5386ccaa5160 154 while(EMG_Left_Bicep_filtered > Threshold_Bicep_Left_1 && EMG_Right_Bicep_filtered < Threshold_Bicep_Right_1)
ThomasBNL 27:b7caf668a682 155 {
ThomasBNL 28:482ac040fb0d 156 debug_led_blue=on;
ThomasBNL 28:482ac040fb0d 157 if (n==0) //wordt maar 1 keer uitgevoerd
ThomasBNL 27:b7caf668a682 158 {
ThomasBNL 28:482ac040fb0d 159 debug_led_blue=off;
ThomasBNL 28:482ac040fb0d 160 reference_turn=reference_turn+change_one_bottle;
ThomasBNL 27:b7caf668a682 161 n=1;
ThomasBNL 28:482ac040fb0d 162 }
ThomasBNL 27:b7caf668a682 163
ThomasBNL 29:263c680068db 164 if (fabs(position_turn-reference_turn)<2) // als error en kleiner dan twee graden
ThomasBNL 27:b7caf668a682 165 {
ThomasBNL 28:482ac040fb0d 166 debug_led_blue=off;
ThomasBNL 28:482ac040fb0d 167 debug_led_red=on;
ThomasBNL 27:b7caf668a682 168 wait(0.5);
ThomasBNL 29:263c680068db 169 if (fabs(position_turn-reference_turn)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
ThomasBNL 27:b7caf668a682 170 {
ThomasBNL 28:482ac040fb0d 171 goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
ThomasBNL 27:b7caf668a682 172 }
ThomasBNL 27:b7caf668a682 173 }
ThomasBNL 27:b7caf668a682 174 }
ThomasBNL 3:11a7da46e093 175 }
ThomasBNL 8:50d6e2323d3b 176
ThomasBNL 28:482ac040fb0d 177 debug_led_red=off; // not inside an action loop
ThomasBNL 28:482ac040fb0d 178 debug_led_blue=off;
ThomasBNL 28:482ac040fb0d 179 debug_led_green=on; // not executing an action
ThomasBNL 28:482ac040fb0d 180
ThomasBNL 28:482ac040fb0d 181
ThomasBNL 28:482ac040fb0d 182 // DRAAI RECHTS // (blue->blue uit (heel kort/zichtbaar?)-> red<->blue<-> red -> klaar)
ThomasBNL 33:5386ccaa5160 183 if (EMG_Right_Bicep_filtered > Threshold_Bicep_Right_2 && EMG_Left_Bicep_filtered < Threshold_Bicep_Right_1)
ThomasBNL 28:482ac040fb0d 184 {
ThomasBNL 28:482ac040fb0d 185 debug_led_green=off; // Executing action
ThomasBNL 28:482ac040fb0d 186 pc.printf("rechts \n\r");
ThomasBNL 27:b7caf668a682 187 n=0;
ThomasBNL 33:5386ccaa5160 188 while(EMG_Right_Bicep_filtered > Threshold_Bicep_Right_1 && EMG_Left_Bicep_filtered < Threshold_Bicep_Left_1)
ThomasBNL 27:b7caf668a682 189 {
ThomasBNL 28:482ac040fb0d 190 debug_led_blue=on;
ThomasBNL 27:b7caf668a682 191 if (n==0)
ThomasBNL 27:b7caf668a682 192 {
ThomasBNL 28:482ac040fb0d 193 debug_led_blue=off;
ThomasBNL 28:482ac040fb0d 194 reference_turn=reference_turn-change_one_bottle;
ThomasBNL 27:b7caf668a682 195 n=1;
ThomasBNL 32:10e6160fdbaa 196 }
ThomasBNL 32:10e6160fdbaa 197 //(45 graden naar rechts voor volgende fles) //wordt maar 1 keer uitgevoerd
ThomasBNL 32:10e6160fdbaa 198 pc.printf("Ref: %f Err: %f \n\r", reference_turn, position_turn);
ThomasBNL 27:b7caf668a682 199
ThomasBNL 29:263c680068db 200 if (abs(position_turn-reference_turn)<2) // als error en kleiner dan twee graden
ThomasBNL 27:b7caf668a682 201 {
ThomasBNL 28:482ac040fb0d 202 debug_led_blue=off;
ThomasBNL 28:482ac040fb0d 203 debug_led_red=on;
ThomasBNL 27:b7caf668a682 204 wait(0.5);
ThomasBNL 29:263c680068db 205 if (abs(position_turn-reference_turn)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
ThomasBNL 27:b7caf668a682 206 {
ThomasBNL 28:482ac040fb0d 207 goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
ThomasBNL 27:b7caf668a682 208 }
ThomasBNL 27:b7caf668a682 209 }
ThomasBNL 27:b7caf668a682 210 }
ThomasBNL 27:b7caf668a682 211 }
ThomasBNL 28:482ac040fb0d 212
ThomasBNL 28:482ac040fb0d 213 debug_led_red=off; // not inside an action loop
ThomasBNL 28:482ac040fb0d 214 debug_led_blue=off;
ThomasBNL 28:482ac040fb0d 215 debug_led_green=on; // not executing an action
ThomasBNL 28:482ac040fb0d 216
ThomasBNL 28:482ac040fb0d 217
ThomasBNL 28:482ac040fb0d 218 if(send_flag)
ThomasBNL 28:482ac040fb0d 219 {
ThomasBNL 28:482ac040fb0d 220 send_flag = false;
ThomasBNL 33:5386ccaa5160 221 scope.set(0,EMG_Right_Bicep_filtered); // HIDSCOPE channel 0 : EMG_Right_Bicep_filtered
ThomasBNL 33:5386ccaa5160 222 scope.set(1,EMG_Left_Bicep_filtered); // HIDSCOPE channel 1 : EMG_Left_Bicep_filtered
ThomasBNL 28:482ac040fb0d 223 scope.set(2,reference_turn); // HIDSCOPE channel 2 : Reference_Turn
ThomasBNL 28:482ac040fb0d 224 scope.send(); // Send channel info to HIDSCOPE
ThomasBNL 28:482ac040fb0d 225 }
ThomasBNL 28:482ac040fb0d 226 } // end while loop
ThomasBNL 28:482ac040fb0d 227 }
ThomasBNL 27:b7caf668a682 228
ThomasBNL 0:40052f5ca77b 229
ThomasBNL 0:40052f5ca77b 230 // Keep in range function
ThomasBNL 0:40052f5ca77b 231 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 232 {
ThomasBNL 0:40052f5ca77b 233 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 234 }
ThomasBNL 0:40052f5ca77b 235
ThomasBNL 0:40052f5ca77b 236 // Looptimerflag function
ThomasBNL 28:482ac040fb0d 237 void send(void)
ThomasBNL 0:40052f5ca77b 238 {
ThomasBNL 28:482ac040fb0d 239 send_flag = true;
ThomasBNL 1:dc683e88b44e 240 }
ThomasBNL 1:dc683e88b44e 241
ThomasBNL 28:482ac040fb0d 242 // Deactivate PID Controller strike
ThomasBNL 28:482ac040fb0d 243 void deactivate_PID_Controller_strike(void)
ThomasBNL 28:482ac040fb0d 244 {
ThomasBNL 28:482ac040fb0d 245 double P_gain_strike=0;
ThomasBNL 28:482ac040fb0d 246 double I_gain_strike=0;
ThomasBNL 28:482ac040fb0d 247 double D_gain_strike=0;
ThomasBNL 28:482ac040fb0d 248 }
ThomasBNL 28:482ac040fb0d 249
ThomasBNL 28:482ac040fb0d 250 // Activate PID Controller strike
ThomasBNL 28:482ac040fb0d 251 void activate_PID_Controller_strike(void)
ThomasBNL 28:482ac040fb0d 252 {
ThomasBNL 28:482ac040fb0d 253 double P_gain_strike=10;
ThomasBNL 28:482ac040fb0d 254 double I_gain_strike=0.1;
ThomasBNL 28:482ac040fb0d 255 double D_gain_strike=50;
ThomasBNL 28:482ac040fb0d 256 }