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
Diff: main.cpp
- Revision:
- 33:5386ccaa5160
- Parent:
- 32:10e6160fdbaa
--- a/main.cpp Mon Oct 12 21:37:11 2015 +0000
+++ b/main.cpp Tue Oct 13 20:39:38 2015 +0000
@@ -23,7 +23,7 @@
AnalogIn potmeter1(A0); // POTMETER -> DOEN ALSOF EMG SIGNAAL
AnalogIn potmeter2(A1); // POTMETER -> DOEN ALSOF EMG SIGNAAL
-double max_L_EMG = 100;
+double EMG_L_max = 100;
// Defining constants //
volatile bool send_flag;
const double cw=0; // zero is clockwise (front view)
@@ -50,16 +50,16 @@
double reference_turn; // Set constant to store reference value of the Turn motor
double position_turn; // Set constant to store current position of the Turn motor
double position_strike;
- float EMG_R;
- float EMG_L;
+ float EMG_Right_Bicep_filtered;
+ float EMG_Left_Bicep_filtered;
double n;
debug_led_red=off;
debug_led_blue=off;
debug_led_green=off;
- double max_L_EMG;
- double min_L_EMG;
- double max_R_EMG;
- double min_R_EMG;
+ double EMG_L_max;
+ double EMG_L_min;
+ double EMG_R_max;
+ double EMG_R_min;
double Hit;
// START OF CODE //
@@ -70,17 +70,17 @@
pc.baud(115200); // Set the baudrate
// Calibratie //
- max_L_EMG = 100; // Calibreren (max average over 5 seconde?) gemeten integraal EMG over tijd / (tijdsample stappen)=100
- min_L_EMG = 0;
- max_R_EMG = 100; // Calibreren
- min_R_EMG = 0;
+ EMG_L_max = 100; // Calibreren (max average over 5 seconde?) gemeten integraal EMG over tijd / (tijdsample stappen)=100
+ EMG_L_min = 0;
+ EMG_R_max = 100; // Calibreren
+ EMG_R_min = 0;
Hit=60; // position when bottle is hit
- const float Threshold_Bicep_Left_1=((max_L_EMG-min_L_EMG)*0.2)+min_L_EMG;; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
- const float Threshold_Bicep_Left_2=((max_L_EMG-min_L_EMG)*0.6)+min_L_EMG; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
- const float Threshold_Bicep_Right_1=((max_R_EMG-min_R_EMG)*0.2)+min_R_EMG; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
- const float Threshold_Bicep_Right_2=((max_R_EMG-min_R_EMG)*0.6)+min_R_EMG; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
+ 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
+ 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);
+ 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
+ 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);
const float change_one_bottle=45; //(45 graden change)
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);
@@ -102,13 +102,13 @@
debug_led_blue=off;
debug_led_green=on; // LED Turns green if ready for a new action
wait(1);
- EMG_R = (potmeter1.read())*100; //EMG Right bicep (tussen nul en 100%)
- EMG_L = (potmeter2.read())*100; // EMG Left bicep (tussen nul en 100%)
- pc.printf("EMG_L = %f EMG_R = %f \n\r", EMG_L, EMG_R);
+ EMG_Right_Bicep_filtered = (potmeter1.read())*100; //EMG Right bicep (tussen nul en 100%)
+ EMG_Left_Bicep_filtered = (potmeter2.read())*100; // EMG Left bicep (tussen nul en 100%)
+ pc.printf("EMG_Left_Bicep_filtered = %f EMG_Right_Bicep_filtered = %f \n\r", EMG_Left_Bicep_filtered, EMG_Right_Bicep_filtered);
// SLAG // (No LED -> 0.55s -> red on (turns on every time a new pwm is given) -> finish)
- if (EMG_L > Threshold_Bicep_Left_1 && EMG_R > Threshold_Bicep_Right_1)
+ if (EMG_Left_Bicep_filtered > Threshold_Bicep_Left_1 && EMG_Right_Bicep_filtered > Threshold_Bicep_Right_1)
{
debug_led_green=off;
@@ -116,7 +116,7 @@
pc.printf("slag \n\r");
wait(0.5);
- while(EMG_L > Threshold_Bicep_Left_1 && EMG_R > Threshold_Bicep_Right_1) // Threshold statement still true after 0.5 seconds?
+ 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?
{
if (n==0) //wordt maar 1 keer uitgevoerd
{
@@ -126,7 +126,7 @@
debug_led_red=off;
wait(0.10); // wait 20 samples
debug_led_red=on;
- 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
+ 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
wait(0.10); // wait 20 samples more (pwm changes per 0.1 seconds)
motordirection_strike=cw; // towards bottle
@@ -146,12 +146,12 @@
debug_led_green=on; // want het kan zijn dat bovenste script half wordt uitgevoerd en dan moet het slag mechanisme wel weer terug
// DRAAI LINKS // // (blue->blue off (very short/visible?)-> red<->blue<-> red -> klaar)
- if (EMG_L > Threshold_Bicep_Left_2 && EMG_R < Threshold_Bicep_Right_1)
+ if (EMG_Left_Bicep_filtered > Threshold_Bicep_Left_2 && EMG_Right_Bicep_filtered < Threshold_Bicep_Right_1)
{
debug_led_green=off; // Executing action
n=0;
pc.printf("links \n\r");
- while(EMG_L > Threshold_Bicep_Left_1 && EMG_R < Threshold_Bicep_Right_1)
+ while(EMG_Left_Bicep_filtered > Threshold_Bicep_Left_1 && EMG_Right_Bicep_filtered < Threshold_Bicep_Right_1)
{
debug_led_blue=on;
if (n==0) //wordt maar 1 keer uitgevoerd
@@ -180,12 +180,12 @@
// DRAAI RECHTS // (blue->blue uit (heel kort/zichtbaar?)-> red<->blue<-> red -> klaar)
- if (EMG_R > Threshold_Bicep_Right_2 && EMG_L < Threshold_Bicep_Right_1)
+ if (EMG_Right_Bicep_filtered > Threshold_Bicep_Right_2 && EMG_Left_Bicep_filtered < Threshold_Bicep_Right_1)
{
debug_led_green=off; // Executing action
pc.printf("rechts \n\r");
n=0;
- while(EMG_R > Threshold_Bicep_Right_1 && EMG_L < Threshold_Bicep_Left_1)
+ while(EMG_Right_Bicep_filtered > Threshold_Bicep_Right_1 && EMG_Left_Bicep_filtered < Threshold_Bicep_Left_1)
{
debug_led_blue=on;
if (n==0)
@@ -218,8 +218,8 @@
if(send_flag)
{
send_flag = false;
- scope.set(0,EMG_R); // HIDSCOPE channel 0 : EMG_R
- scope.set(1,EMG_L); // HIDSCOPE channel 1 : EMG_L
+ scope.set(0,EMG_Right_Bicep_filtered); // HIDSCOPE channel 0 : EMG_Right_Bicep_filtered
+ scope.set(1,EMG_Left_Bicep_filtered); // HIDSCOPE channel 1 : EMG_Left_Bicep_filtered
scope.set(2,reference_turn); // HIDSCOPE channel 2 : Reference_Turn
scope.send(); // Send channel info to HIDSCOPE
}
