Thomas Burgers
/
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
Revision 51:ec278d148932, committed 2015-10-15
- Comitter:
- ThomasBNL
- Date:
- Thu Oct 15 12:40:11 2015 +0000
- Parent:
- 50:6060f45d343a
- Child:
- 52:b530adf72f79
- Commit message:
- nieuwe versie (backup) werkt nog niet;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 15 12:22:57 2015 +0000
+++ b/main.cpp Thu Oct 15 12:40:11 2015 +0000
@@ -270,9 +270,9 @@
//double pwm_to_motor_strike; // STRIKE: variable that is constructed by summing the values of the P, I and D action
//double reference_strike;
- //double P_gain_turn; // STRIKE: this gain gets multiplied with the error inside the P action of the PID controller
- //double I_gain_turn; // STRIKE: this gain gets multiplied with the error inside the I action of the PID controller
- //double D_gain_turn; // STRIKE: this gain gets multiplied with the error inside the D action of the PID controller
+ double P_gain_strike; // STRIKE: this gain gets multiplied with the error inside the P action of the PID controller
+ double I_gain_strike; // STRIKE: this gain gets multiplied with the error inside the I action of the PID controller
+ double D_gain_strike; // STRIKE: this gain gets multiplied with the error inside the D action of the PID controller
//double reference_strike; // STRIKE: reference position of the motor (position the motor 'likes' to get to)
@@ -544,133 +544,133 @@
// DO STUFF / DECIDE WHETHER TO DO SOMETHING OR NOT ==> TICKER loop moet dan wel uit (TICKER VOOR SAMPLE FILTER WEL AAN)
-
- Nieuwe_actie:
-// TICKER AAN // start here again when action has finished
- debug_led_red=off;
- debug_led_blue=off;
- debug_led_green=on; // LED Turns green if ready for a new action
- wait(1);
- //sample_filter();
- moving_average_right = (input1.read())*100; // EMG Right bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
- moving_average_left = (input2.read())*100; // EMG Left bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
- pc.printf("mov_r = %f, mov_l = %f \n\r", moving_average_right, moving_average_left);
- // EMG Left bicep (tussen nul en 100%)
- pc.printf("moving_average_left = %f moving_average_right = %f \n\r", moving_average_left, moving_average_right);
-
-
- // STRIKE // (No LED -> 0.55s -> red on (turns on every time a new pwm is given) -> finish)
- if (moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1)
- {
-// TICKER OFF
- debug_led_green=off;
- n=0;
- pc.printf("slag \n\r");
- wait(0.5);
-
- while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1) // Threshold statement still true after 0.5 seconds?
- {
- if (n==0) //wordt maar 1 keer uitgevoerd
- {
- deactivate_PID_Controller(P_gain_turn, I_gain_turn, D_gain_turn); // function that sets P, I and D gain values to zero
- n=1;
- }
- debug_led_red=off;
- wait(0.10); // wait 20 samples
- debug_led_red=on;
- pwm_motor_turn=((moving_average_left-EMG_L_min)+(moving_average_right-EMG_R_min))/((EMG_L_max-EMG_L_min)+(EMG_R_max-EMG_R_min))*0.95 + 0.05; // min speed 0.3 en max 1
- // TEMPORARY MOTOR TURN FOR TESTING
- wait(0.10); // wait 20 samples more (pwm changes per 0.1 seconds)
- motordirection_turn=cw; // towards bottle
-
- // motordirection_strike=cw; // towards bottle
- // if((position_strike-Hit)<2) // bottle is hit when position-hit <2 defrees
- if((position_turn-Hit)<2) // bottle is hit when position-hit <2 degrees
- {
- activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn); // function sets back P, I and D gain values
- pc.printf("einde \n\r");
- goto Nieuwe_actie; // Finished: Get Ready for new Action control
- }
-
- }
- }
-
- //activate_PID_Controller_stike(P_gain_turn, I_gain_turn, D_gain_turn); // function sets back P, I and D gain values
-// TICKER AAN
- debug_led_red=off; // not inside an action loop (green light)
- debug_led_blue=off;
- debug_led_green=on; // want het kan zijn dat bovenste script half wordt uitgevoerd en dan moet het slag mechanisme wel weer terug
-
- // TURN LEFT // // (blue->blue off (very short/visible?)-> red<->blue<-> red -> klaar)
- if (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1)
- {
- debug_led_green=off; // Executing action
-// TICKER UIT
- n=0;
- pc.printf("links \n\r");
- while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right < Threshold_Bicep_Right_1)
- {
- debug_led_blue=on;
- if (n==0) //wordt maar 1 keer uitgevoerd
- {
- debug_led_blue=off;
- reference_turn=reference_turn+change_one_bottle;
- n=1;
- }
-
- if (fabs(position_turn-reference_turn)<2) // als error en kleiner dan twee graden
- {
- debug_led_blue=off;
- debug_led_red=on;
- wait(0.5);
- if (fabs(position_turn-reference_turn)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
- {
- goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
- }
- }
- }
- }
-
- debug_led_red=off; // not inside an action loop
- debug_led_blue=off;
- debug_led_green=on; // not executing an action
-// TICKER AAN
-
- // TURN RIGHT // (blue->blue uit (heel kort/zichtbaar?)-> red<->blue<-> red -> klaar)
- if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Right_1)
- {
-// TICKER UIT
- debug_led_green=off; // Executing action
- pc.printf("rechts \n\r");
- n=0;
- while(moving_average_right > Threshold_Bicep_Right_1 && moving_average_left < Threshold_Bicep_Left_1)
- {
- debug_led_blue=on;
- if (n==0)
- {
- debug_led_blue=off;
- reference_turn=reference_turn-change_one_bottle;
- n=1;
- }
- //(45 graden naar rechts voor volgende fles) //wordt maar 1 keer uitgevoerd
- pc.printf("Ref: %f Err: %f \n\r", reference_turn, position_turn);
-
- if (abs(position_turn-reference_turn)<2) // als error en kleiner dan twee graden
- {
- debug_led_blue=off;
- debug_led_red=on;
- wait(0.5);
- if (abs(position_turn-reference_turn)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
- {
- goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
- }
- }
- }
- }
- debug_led_red=off; // not inside an action loop
- debug_led_blue=off;
- debug_led_green=on; // not executing an action
-
+//
+// Nieuwe_actie:
+//// TICKER AAN // start here again when action has finished
+// debug_led_red=off;
+// debug_led_blue=off;
+// debug_led_green=on; // LED Turns green if ready for a new action
+// wait(1);
+// //sample_filter();
+// moving_average_right = (input1.read())*100; // EMG Right bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
+// moving_average_left = (input2.read())*100; // EMG Left bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
+// pc.printf("mov_r = %f, mov_l = %f \n\r", moving_average_right, moving_average_left);
+// // EMG Left bicep (tussen nul en 100%)
+// pc.printf("moving_average_left = %f moving_average_right = %f \n\r", moving_average_left, moving_average_right);
+//
+//
+// // STRIKE // (No LED -> 0.55s -> red on (turns on every time a new pwm is given) -> finish)
+// if (moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1)
+// {
+//// TICKER OFF
+// debug_led_green=off;
+// n=0;
+// pc.printf("slag \n\r");
+// wait(0.5);
+//
+// while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1) // Threshold statement still true after 0.5 seconds?
+// {
+// if (n==0) //wordt maar 1 keer uitgevoerd
+// {
+// deactivate_PID_Controller(P_gain_turn, I_gain_turn, D_gain_turn); // function that sets P, I and D gain values to zero
+// n=1;
+// }
+// debug_led_red=off;
+// wait(0.10); // wait 20 samples
+// debug_led_red=on;
+// pwm_motor_turn=((moving_average_left-EMG_L_min)+(moving_average_right-EMG_R_min))/((EMG_L_max-EMG_L_min)+(EMG_R_max-EMG_R_min))*0.95 + 0.05; // min speed 0.3 en max 1
+// // TEMPORARY MOTOR TURN FOR TESTING
+// wait(0.10); // wait 20 samples more (pwm changes per 0.1 seconds)
+// motordirection_turn=cw; // towards bottle
+//
+// // motordirection_strike=cw; // towards bottle
+// // if((position_strike-Hit)<2) // bottle is hit when position-hit <2 defrees
+// if((position_turn-Hit)<2) // bottle is hit when position-hit <2 degrees
+// {
+// activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn); // function sets back P, I and D gain values
+// pc.printf("einde \n\r");
+// goto Nieuwe_actie; // Finished: Get Ready for new Action control
+// }
+//
+// }
+// }
+//
+// //activate_PID_Controller_stike(P_gain_turn, I_gain_turn, D_gain_turn); // function sets back P, I and D gain values
+//// TICKER AAN
+// debug_led_red=off; // not inside an action loop (green light)
+// debug_led_blue=off;
+// debug_led_green=on; // want het kan zijn dat bovenste script half wordt uitgevoerd en dan moet het slag mechanisme wel weer terug
+//
+// // TURN LEFT // // (blue->blue off (very short/visible?)-> red<->blue<-> red -> klaar)
+// if (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1)
+// {
+// debug_led_green=off; // Executing action
+//// TICKER UIT
+// n=0;
+// pc.printf("links \n\r");
+// while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right < Threshold_Bicep_Right_1)
+// {
+// debug_led_blue=on;
+// if (n==0) //wordt maar 1 keer uitgevoerd
+// {
+// debug_led_blue=off;
+// reference_turn=reference_turn+change_one_bottle;
+// n=1;
+// }
+//
+// if (fabs(position_turn-reference_turn)<2) // als error en kleiner dan twee graden
+// {
+// debug_led_blue=off;
+// debug_led_red=on;
+// wait(0.5);
+// if (fabs(position_turn-reference_turn)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
+// {
+// goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
+// }
+// }
+// }
+// }
+//
+// debug_led_red=off; // not inside an action loop
+// debug_led_blue=off;
+// debug_led_green=on; // not executing an action
+//// TICKER AAN
+//
+// // TURN RIGHT // (blue->blue uit (heel kort/zichtbaar?)-> red<->blue<-> red -> klaar)
+// if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Right_1)
+// {
+//// TICKER UIT
+// debug_led_green=off; // Executing action
+// pc.printf("rechts \n\r");
+// n=0;
+// while(moving_average_right > Threshold_Bicep_Right_1 && moving_average_left < Threshold_Bicep_Left_1)
+// {
+// debug_led_blue=on;
+// if (n==0)
+// {
+// debug_led_blue=off;
+// reference_turn=reference_turn-change_one_bottle;
+// n=1;
+// }
+// //(45 graden naar rechts voor volgende fles) //wordt maar 1 keer uitgevoerd
+// pc.printf("Ref: %f Err: %f \n\r", reference_turn, position_turn);
+//
+// if (abs(position_turn-reference_turn)<2) // als error en kleiner dan twee graden
+// {
+// debug_led_blue=off;
+// debug_led_red=on;
+// wait(0.5);
+// if (abs(position_turn-reference_turn)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
+// {
+// goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
+// }
+// }
+// }
+// }
+// debug_led_red=off; // not inside an action loop
+// debug_led_blue=off;
+// debug_led_green=on; // not executing an action
+//
// TICKER WEER AAN
@@ -822,16 +822,16 @@
void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
{
- P_gain=10; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
- I_gain=0.5;
- D_gain=50;
+ P_gain_turn=10; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
+ I_gain_turn=0.5;
+ D_gain_turn=50;
}
void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
{
- P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
- I_gain=0;
- D_gain=0;
+ P_gain_strike=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
+ I_gain_strike=0;
+ D_gain_strike=0;
}
//============================================================================================================================
