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@27:b7caf668a682, 2015-10-08 (annotated)
- Committer:
- ThomasBNL
- Date:
- Thu Oct 08 20:49:58 2015 +0000
- Revision:
- 27:b7caf668a682
- Parent:
- 26:b3693f431d6f
- Child:
- 28:482ac040fb0d
REGELAAR begin: EMG--> REGELAAR -> CONTROLLER
Who changed what in which revision?
User | Revision | Line number | New 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 | 24:8ca471562212 | 9 | DigitalOut debug_led_red(LED_RED); // Debug LED |
ThomasBNL | 24:8ca471562212 | 10 | DigitalOut debug_led_green(LED_GREEN); // Debug LED |
ThomasBNL | 25:d536056a2666 | 11 | DigitalOut debug_led_blue(LED_BLUE); // Debug LED |
ThomasBNL | 8:50d6e2323d3b | 12 | |
ThomasBNL | 27:b7caf668a682 | 13 | //HIDScope scope(2); // HIDSCOPE declared |
ThomasBNL | 14:599896acf576 | 14 | |
ThomasBNL | 8:50d6e2323d3b | 15 | // (DEBUGGING AND TESTING BUTTONS) (0 when pressed and 1 when not pressed) |
ThomasBNL | 8:50d6e2323d3b | 16 | DigitalIn buttonL1(PTC6); // Button 1 (laag op board) for testing at the lower board |
ThomasBNL | 8:50d6e2323d3b | 17 | DigitalIn buttonL2(PTA4); // Button 2 (laag op board) for testing at the lower board |
ThomasBNL | 8:50d6e2323d3b | 18 | DigitalIn buttonH1(D2); // Button 3 (hoog op board) for testing at the top board |
ThomasBNL | 8:50d6e2323d3b | 19 | DigitalIn buttonH2(D6); // Button 4 (hoog op board) for testing at the top board |
ThomasBNL | 0:40052f5ca77b | 20 | |
ThomasBNL | 27:b7caf668a682 | 21 | //volatile bool looptimerflag; |
ThomasBNL | 8:50d6e2323d3b | 22 | const double cw=0; // zero is clockwise (front view) |
ThomasBNL | 8:50d6e2323d3b | 23 | const double ccw=1; // one is counterclockwise (front view) |
ThomasBNL | 8:50d6e2323d3b | 24 | |
ThomasBNL | 8:50d6e2323d3b | 25 | const double sample_time=0.01; // tijd voor een sample (100Hz) |
ThomasBNL | 27:b7caf668a682 | 26 | |
ThomasBNL | 7:ddd7fb357786 | 27 | // Functions used (described after main) |
ThomasBNL | 27:b7caf668a682 | 28 | // void setlooptimerflag(void); |
ThomasBNL | 8:50d6e2323d3b | 29 | |
ThomasBNL | 0:40052f5ca77b | 30 | |
ThomasBNL | 7:ddd7fb357786 | 31 | // MAIN function |
ThomasBNL | 0:40052f5ca77b | 32 | int main() { |
ThomasBNL | 27:b7caf668a682 | 33 | debug_led_red=0; |
ThomasBNL | 27:b7caf668a682 | 34 | debug_led_blue=0; |
ThomasBNL | 27:b7caf668a682 | 35 | debug_led_green=0; |
ThomasBNL | 27:b7caf668a682 | 36 | AnalogIn potmeter1(A0); // POTMETER -> DOEN ALSOF EMG SIGNAAL |
ThomasBNL | 27:b7caf668a682 | 37 | AnalogIn potmeter2(A1); // POTMETER -> DOEN ALSOF EMG SIGNAAL |
ThomasBNL | 27:b7caf668a682 | 38 | |
ThomasBNL | 8:50d6e2323d3b | 39 | QEI motor_turn(D12,D13,NC,32); // Encoder for motor Turn |
ThomasBNL | 8:50d6e2323d3b | 40 | PwmOut pwm_motor_turn(D5); // Pwm for motor Turn |
ThomasBNL | 8:50d6e2323d3b | 41 | DigitalOut motordirection_turn(D4); // Direction of the motor Turn |
ThomasBNL | 27:b7caf668a682 | 42 | double reference_turn=0; // Set constant to store reference value of the Turn motor |
ThomasBNL | 8:50d6e2323d3b | 43 | double position_turn; // Set constant to store current position of the Turn motor |
ThomasBNL | 8:50d6e2323d3b | 44 | double error; |
ThomasBNL | 10:09ba965045a7 | 45 | double pwm_to_motor_turn; |
ThomasBNL | 10:09ba965045a7 | 46 | double pwm_motor_turn_P; |
ThomasBNL | 10:09ba965045a7 | 47 | double pwm_motor_turn_I; |
ThomasBNL | 10:09ba965045a7 | 48 | double pwm_motor_turn_D; |
ThomasBNL | 0:40052f5ca77b | 49 | |
ThomasBNL | 7:ddd7fb357786 | 50 | //START OF CODE |
ThomasBNL | 7:ddd7fb357786 | 51 | pc.printf("Start of code \n\r"); |
ThomasBNL | 0:40052f5ca77b | 52 | |
ThomasBNL | 23:335a3b843a5e | 53 | pc.baud(115200); // Set the baudrate |
ThomasBNL | 0:40052f5ca77b | 54 | |
ThomasBNL | 7:ddd7fb357786 | 55 | // Tickers |
ThomasBNL | 8:50d6e2323d3b | 56 | Ticker looptimer; // Ticker called looptimer to set a looptimerflag |
ThomasBNL | 8:50d6e2323d3b | 57 | looptimer.attach(setlooptimerflag,sample_time); // calls the looptimer flag every 0.01s |
ThomasBNL | 0:40052f5ca77b | 58 | |
ThomasBNL | 22:eaf4cbd1dcec | 59 | //Sample_Ticker.attach(&get_sample, sample_time); // HIDSCOPE sample Ticker |
ThomasBNL | 14:599896acf576 | 60 | |
ThomasBNL | 7:ddd7fb357786 | 61 | pc.printf("Start infinite loop \n\r"); |
ThomasBNL | 16:a8d2c721cf56 | 62 | wait (3); // Wait before starting system |
ThomasBNL | 0:40052f5ca77b | 63 | |
ThomasBNL | 0:40052f5ca77b | 64 | //INFINITE LOOP |
ThomasBNL | 5:8fb74a22fe3c | 65 | while(1) |
ThomasBNL | 27:b7caf668a682 | 66 | { // Start while loop} |
ThomasBNL | 8:50d6e2323d3b | 67 | // Wait until looptimer flag is true then execute PID controller. |
ThomasBNL | 0:40052f5ca77b | 68 | while(looptimerflag != true); |
ThomasBNL | 0:40052f5ca77b | 69 | looptimerflag = false; |
ThomasBNL | 0:40052f5ca77b | 70 | |
ThomasBNL | 27:b7caf668a682 | 71 | // INPUT EMG SIGNAAL |
ThomasBNL | 27:b7caf668a682 | 72 | EMG_R = (potmeter1.read()-0.5)*2000; //EMG Right bicep |
ThomasBNL | 27:b7caf668a682 | 73 | EMG_L = (potmeter2.read()-0.5)*2000; // EMG Left bicep |
ThomasBNL | 27:b7caf668a682 | 74 | |
ThomasBNL | 27:b7caf668a682 | 75 | // calibratie |
ThomasBNL | 27:b7caf668a682 | 76 | double max_EMG = 40; // Calibreren |
ThomasBNL | 27:b7caf668a682 | 77 | double min_EMG = 0; |
ThomasBNL | 27:b7caf668a682 | 78 | double Hit=60; // position when bottle is hit |
ThomasBNL | 0:40052f5ca77b | 79 | |
ThomasBNL | 27:b7caf668a682 | 80 | // REGELAAR (wat gaat de controller doen om bestuurd te worden) |
ThomasBNL | 27:b7caf668a682 | 81 | const double Threshold_Bicep_Left_1=10; // percentages van min en max EMG |
ThomasBNL | 27:b7caf668a682 | 82 | const double Threshold_Bicep_Left_2=20; |
ThomasBNL | 27:b7caf668a682 | 83 | const double Threshold_Bicep_Left_3=30; |
ThomasBNL | 27:b7caf668a682 | 84 | const double Threshold_Bicep_Right_1=10; |
ThomasBNL | 27:b7caf668a682 | 85 | const double Threshold_Bicep_Right_2=20; |
ThomasBNL | 3:11a7da46e093 | 86 | |
ThomasBNL | 27:b7caf668a682 | 87 | Nieuwe_actie: // Komt hier weer terecht na |
ThomasBNL | 3:11a7da46e093 | 88 | |
ThomasBNL | 0:40052f5ca77b | 89 | |
ThomasBNL | 27:b7caf668a682 | 90 | // SLAG |
ThomasBNL | 27:b7caf668a682 | 91 | if (EMG_L > Threshold_Bicep_Left_1 || EMG_R > Threshold_Bicep_Right_1) |
ThomasBNL | 27:b7caf668a682 | 92 | { |
ThomasBNL | 27:b7caf668a682 | 93 | wait(0.5); //(wacht 0.5s om te kijken hoe hard geslagen moet worden) |
ThomasBNL | 27:b7caf668a682 | 94 | while(EMG_L > Threshold_Bicep_Left_1 || EMG_R > Threshold_Bicep_Right_1) // Nog steeds over de threshold? |
ThomasBNL | 27:b7caf668a682 | 95 | { |
ThomasBNL | 27:b7caf668a682 | 96 | // deactivate_PID_Controller_strike // function that sets P, I and D gain values to zero |
ThomasBNL | 27:b7caf668a682 | 97 | wait(0.1); // wacht 10 samples |
ThomasBNL | 27:b7caf668a682 | 98 | pwm_to_motor_strike=((EMG_L+EMG_R)/(max_EMG+min_EMG))*0.7 + 0.3 // min speed 0.3 en max 1 |
ThomasBNL | 27:b7caf668a682 | 99 | motor_strike_direction=cw; // towards bottle |
ThomasBNL | 11:ecd83b303252 | 100 | |
ThomasBNL | 27:b7caf668a682 | 101 | if((Position-Hit)<2) // geraakt als position-hit <2 graden |
ThomasBNL | 27:b7caf668a682 | 102 | ( |
ThomasBNL | 27:b7caf668a682 | 103 | // activate_PID_Controller_strike // function sets back P, I and D gain values |
ThomasBNL | 27:b7caf668a682 | 104 | goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd |
ThomasBNL | 27:b7caf668a682 | 105 | } |
ThomasBNL | 27:b7caf668a682 | 106 | } |
ThomasBNL | 27:b7caf668a682 | 107 | |
ThomasBNL | 27:b7caf668a682 | 108 | // activate_PID_Controller_strike // function sets back P, I and D gain values |
ThomasBNL | 27:b7caf668a682 | 109 | // want het kan zijn dat bovenste script half wordt uitgevoerd en dan moet het slag mechanisme wel weer terug |
ThomasBNL | 3:11a7da46e093 | 110 | |
ThomasBNL | 27:b7caf668a682 | 111 | // DRAAI LINKS |
ThomasBNL | 27:b7caf668a682 | 112 | else if (EMG_L > Threshold_Bicep_Left_2 || EMG_R < Threshold_Bicep_Right_1) |
ThomasBNL | 27:b7caf668a682 | 113 | { |
ThomasBNL | 27:b7caf668a682 | 114 | |
ThomasBNL | 27:b7caf668a682 | 115 | n=0; |
ThomasBNL | 27:b7caf668a682 | 116 | while(EMG_L > Threshold_Bicep_Left_1 || EMG_R < Threshold_Bicep_Right_1) |
ThomasBNL | 27:b7caf668a682 | 117 | { |
ThomasBNL | 27:b7caf668a682 | 118 | if (n==0) |
ThomasBNL | 27:b7caf668a682 | 119 | { |
ThomasBNL | 27:b7caf668a682 | 120 | Reference=Reference+45; |
ThomasBNL | 27:b7caf668a682 | 121 | n=1; |
ThomasBNL | 27:b7caf668a682 | 122 | } //(45 graden naar links voor volgende fles) //wordt maar 1 keer uitgevoerd |
ThomasBNL | 27:b7caf668a682 | 123 | |
ThomasBNL | 27:b7caf668a682 | 124 | if (abs(error)<2) // als error en kleiner dan twee graden |
ThomasBNL | 27:b7caf668a682 | 125 | { |
ThomasBNL | 27:b7caf668a682 | 126 | wait(0.5); |
ThomasBNL | 27:b7caf668a682 | 127 | if (abs(error)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden? |
ThomasBNL | 27:b7caf668a682 | 128 | { |
ThomasBNL | 27:b7caf668a682 | 129 | goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd |
ThomasBNL | 27:b7caf668a682 | 130 | } |
ThomasBNL | 27:b7caf668a682 | 131 | } |
ThomasBNL | 27:b7caf668a682 | 132 | } |
ThomasBNL | 3:11a7da46e093 | 133 | } |
ThomasBNL | 8:50d6e2323d3b | 134 | |
ThomasBNL | 27:b7caf668a682 | 135 | // DRAAI RECHTS |
ThomasBNL | 27:b7caf668a682 | 136 | else if (EMG_R > Threshold_Bicep_Left_2 || EMG_L < Threshold_Bicep_Right_1) |
ThomasBNL | 27:b7caf668a682 | 137 | { |
ThomasBNL | 27:b7caf668a682 | 138 | n=0; |
ThomasBNL | 27:b7caf668a682 | 139 | while(EMG_R > Threshold_Bicep_Left_1 || EMG_L < Threshold_Bicep_Right_1) |
ThomasBNL | 27:b7caf668a682 | 140 | { |
ThomasBNL | 27:b7caf668a682 | 141 | if (n==0) |
ThomasBNL | 27:b7caf668a682 | 142 | { |
ThomasBNL | 27:b7caf668a682 | 143 | Reference=Reference-45; |
ThomasBNL | 27:b7caf668a682 | 144 | n=1; |
ThomasBNL | 27:b7caf668a682 | 145 | } //(45 graden naar rechts voor volgende fles) //wordt maar 1 keer uitgevoerd |
ThomasBNL | 27:b7caf668a682 | 146 | |
ThomasBNL | 27:b7caf668a682 | 147 | if (abs(error)<2) // als error en kleiner dan twee graden |
ThomasBNL | 27:b7caf668a682 | 148 | { |
ThomasBNL | 27:b7caf668a682 | 149 | wait(0.5); |
ThomasBNL | 27:b7caf668a682 | 150 | if (abs(error)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden? |
ThomasBNL | 27:b7caf668a682 | 151 | { |
ThomasBNL | 27:b7caf668a682 | 152 | goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd |
ThomasBNL | 27:b7caf668a682 | 153 | } |
ThomasBNL | 27:b7caf668a682 | 154 | } |
ThomasBNL | 27:b7caf668a682 | 155 | } |
ThomasBNL | 27:b7caf668a682 | 156 | } |
ThomasBNL | 27:b7caf668a682 | 157 | |
ThomasBNL | 27:b7caf668a682 | 158 | // Send HIDSCOPE data |
ThomasBNL | 27:b7caf668a682 | 159 | // scope.set(0,reference_turn); // HIDSCOPE channel 0 : Current Reference |
ThomasBNL | 27:b7caf668a682 | 160 | // scope.set(0,position_turn); // HIDSCOPE channel 0 : Position_turn |
ThomasBNL | 27:b7caf668a682 | 161 | // scope.set(1,pwm_to_motor_turn); // HIDSCOPE channel 1 : Pwm_to_motor_turn |
ThomasBNL | 27:b7caf668a682 | 162 | // scope.send(); // Send channel info to HIDSCOPE |
ThomasBNL | 22:eaf4cbd1dcec | 163 | // } |
ThomasBNL | 0:40052f5ca77b | 164 | } |
ThomasBNL | 0:40052f5ca77b | 165 | } |
ThomasBNL | 5:8fb74a22fe3c | 166 | } |
ThomasBNL | 0:40052f5ca77b | 167 | |
ThomasBNL | 0:40052f5ca77b | 168 | // Keep in range function |
ThomasBNL | 0:40052f5ca77b | 169 | void keep_in_range(double * in, double min, double max) |
ThomasBNL | 0:40052f5ca77b | 170 | { |
ThomasBNL | 0:40052f5ca77b | 171 | *in > min ? *in < max? : *in = max: *in = min; |
ThomasBNL | 0:40052f5ca77b | 172 | } |
ThomasBNL | 0:40052f5ca77b | 173 | |
ThomasBNL | 0:40052f5ca77b | 174 | // Looptimerflag function |
ThomasBNL | 0:40052f5ca77b | 175 | void setlooptimerflag(void) |
ThomasBNL | 0:40052f5ca77b | 176 | { |
ThomasBNL | 0:40052f5ca77b | 177 | looptimerflag = true; |
ThomasBNL | 1:dc683e88b44e | 178 | } |
ThomasBNL | 1:dc683e88b44e | 179 | |
ThomasBNL | 22:eaf4cbd1dcec | 180 | //// Get sample |
ThomasBNL | 22:eaf4cbd1dcec | 181 | //void get_sample(void) // HIDSCOPE sample fuction |
ThomasBNL | 22:eaf4cbd1dcec | 182 | //{ |
ThomasBNL | 22:eaf4cbd1dcec | 183 | // sample = true; |
ThomasBNL | 22:eaf4cbd1dcec | 184 | //} |