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@28:482ac040fb0d, 2015-10-10 (annotated)

Committer:
ThomasBNL
Date:
Sat Oct 10 21:48:01 2015 +0000
Revision:
28:482ac040fb0d
Parent:
27:b7caf668a682
Child:
29:263c680068db
Action controller         (EMG-> action controller -> PID)

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