Biorobotics
Yea it is working. done programinging for a while!
Dependencies: HIDScope QEI mbed
Diff: main.cpp
- Revision:
- 2:e84adf0a960f
- Parent:
- 1:267ff9a39ee4
--- a/main.cpp Thu Oct 29 14:29:21 2015 +0000
+++ b/main.cpp Mon Nov 02 15:03:25 2015 +0000
@@ -37,7 +37,7 @@
}
-
+/*************************************************************Begin Filtering **********************************************************************************************************/
double input_left = 0 ;
double input_right = 0 ;
//double input = 0;
@@ -45,10 +45,10 @@
double filter_signal_hid_right = 0;
//double input_right = 0;
// defining threshold
-double high_threshold = 1000;
+double high_threshold = 900;
double low_threshold = 125;
-//*** making the v for everything and conquer the world***
+//*** making the v for everything and conquer the world ***
//for left
//for Notchfilter
@@ -115,9 +115,6 @@
double n5_b = 0;
-double filter_left;
-double filter_right;
-
//general biquad filter that can be called in all the filter functions
double biquad(double u, double &v1, double &v2, const double a1,
const double a2, const double b0, const double b1, const double b2)
@@ -142,14 +139,7 @@
return average;
}
-/*
-double threshold(double signal, const double lowtreshold, const double hightreshold)
-{
- if (signal > hightreshold)
- left = true;
- else if (signal <lowtreshold)
- left = false;
-}
+
*/
//Specifying filter coefficients highpass
@@ -227,7 +217,6 @@
{
input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER
- //input_right = analog_emg_right.read();
// notch filter
double y1 = biquad(input, notch_v11, notch_v21, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
@@ -257,7 +246,6 @@
{
input_b = input_b-0.45; //FIRST SUBTRACT MEAN THEN FILTER
- //input_right = analog_emg_right.read();
// notch filter
double y1_b = biquad(input_b, notch_v11_b, notch_v21_b, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
@@ -283,13 +271,17 @@
return(filter_signal_b);
}
-/*************************************************************BEGIN motor control******************************************************************************************************/
+
+/*************************************************************END Filtering **********************************************************************************************************/
+/*-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/
+/*************************************************************BEGIN motor control*****************************************************************************************************/
// Define pin for motor control
DigitalOut directionPin(D4);
PwmOut PWM(D5);
DigitalOut directionPin_key(D7);
PwmOut PWM_key(D6);
-Ticker both_timer;
+
+Ticker both_timer; // to make a timer to prevent loping of the key press function
// define rotation direction and begin possition
const int cw = 1;
const int ccw = 0;
@@ -303,15 +295,11 @@
}
-/*
-double Rotation = -2; // rotation
-double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
-*/
-
+/********* the old PI controller ***********/
/*
-// Reusable P controller
+// Reusable PI controller
double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
{
@@ -354,12 +342,13 @@
DigitalIn button_song(PTB9);
DigitalIn button_do_nothing(PTD1);
-//int to check if the button is pressed
+//int to check if the button is pressed, 0 is pressed.
const int Button_only_key_on = 0;
const int Button_demo_on = 0;
const int Button_song_on = 0;
const int Button_do_nothing_on = 0;
+//To say the buttons is not jet pressed.
volatile bool only_key = false;
volatile bool demo = false ;
@@ -384,7 +373,7 @@
both_timer.attach(&Go_flag_both, 2);
while(1) {
-
+/********************* Code for starting filtering ****************/
if(Flag_filteren) {
Flag_filteren = false;
// filter left and set bool
@@ -408,14 +397,14 @@
}
-
+/********************* Code to look how the EMG/ filtered EMG signal looks like on hidsope ****************/
if(Flag_HIDScope) {
Flag_HIDScope = false;
HIDScope_kijken();
}
-
+/********************* Code to switch between modes (demo = left/right and keypress are active; only key = only the key press is active ****************/
if (button_only_key.read() == Button_only_key_on){
demo = false;
@@ -430,6 +419,7 @@
pc.printf("demo /n");
}
+/********************* Code Demo ****************/
if(demo){
// Pussing buttons to get input signal
if( left_movement and right_movement == false) {
@@ -446,12 +436,12 @@
if( left_movement and right_movement and flag_both){
directionPin_key.write(cw);
PWM_key.write(1);
- wait(0.14);
+ wait(0.05);
PWM_key.write(0);
- wait(1);
+ wait(0.3);
directionPin_key.write(ccw);
PWM_key.write(1);
- wait(0.165f);
+ wait(0.039f);
PWM_key.write(0);
flag_both = false;
}
@@ -460,40 +450,42 @@
}
}
+/********************* Code only key ****************/
+
if(only_key){
if(left_movement and flag_both){
directionPin_key.write(cw);
PWM_key.write(1);
- wait(0.14);
+ wait(0.05);
PWM_key.write(0);
- wait(1);
+ wait(0.3);
directionPin_key.write(ccw);
PWM_key.write(1);
- wait(0.165f);
+ wait(0.039f);
PWM_key.write(0);
flag_both = false;
}
}
-
+/********************* Code to kill the emg controll to set the system at 'stand by'****************/
if (button_do_nothing.read() == Button_do_nothing_on){
+ demo = false;
+ only_key = false;
}
+/********************* Code with pre programed feedforward commands to play the first bit of the dutch song 'Vader Jacob'. ****************/
if (button_song.read() == Button_song_on){
//pressing va
- directionPin_key.write(cw);
- PWM_key.write(1);
- wait(0.14); //time it takes pwm of one to reach the key
- //pause at the key for one second
- PWM_key.write(0);
- wait(1);
- //move bakc to the starting position
- directionPin_key.write(ccw);
- PWM_key.write(1);
- //time needs to be slightly longer than moving forward (no answer why)
- wait(0.165f);
- PWM_key.write(0);
+ directionPin_key.write(cw);
+ PWM_key.write(1);
+ wait(0.05);
+ PWM_key.write(0);
+ wait(0.3);
+ directionPin_key.write(ccw);
+ PWM_key.write(1);
+ wait(0.039f);
+ PWM_key.write(0);
// go 1 key to right
directionPin.write(cw);
@@ -503,17 +495,14 @@
//pressing der
directionPin_key.write(cw);
- PWM_key.write(1);
- wait(0.14); //time it takes pwm of one to reach the key
- //pause at the key for one second
- PWM_key.write(0);
- wait(1);
- //move bakc to the starting position
- directionPin_key.write(ccw);
- PWM_key.write(1);
- //time needs to be slightly longer than moving forward (no answer why)
- wait(0.165f);
- PWM_key.write(0);
+ PWM_key.write(1);
+ wait(0.05);
+ PWM_key.write(0);
+ wait(0.3);
+ directionPin_key.write(ccw);
+ PWM_key.write(1);
+ wait(0.039f);
+ PWM_key.write(0);
// go 1 key to right
directionPin.write(cw);
@@ -522,19 +511,16 @@
PWM.write(0);
//pressing ja
- directionPin_key.write(cw);
- PWM_key.write(1);
- wait(0.14); //time it takes pwm of one to reach the key
- //pause at the key for one second
- PWM_key.write(0);
- wait(1);
- //move bakc to the starting position
- directionPin_key.write(ccw);
- PWM_key.write(1);
- //time needs to be slightly longer than moving forward (no answer why)
- wait(0.165f);
- PWM_key.write(0);
-
+ directionPin_key.write(cw);
+ PWM_key.write(1);
+ wait(0.05);
+ PWM_key.write(0);
+ wait(0.3);
+ directionPin_key.write(ccw);
+ PWM_key.write(1);
+ wait(0.039f);
+ PWM_key.write(0);
+
// go 2 key to left
directionPin.write(ccw);
PWM.write(1);
@@ -543,34 +529,28 @@
//pressing cob
directionPin_key.write(cw);
- PWM_key.write(1);
- wait(0.14); //time it takes pwm of one to reach the key
- //pause at the key for one second
- PWM_key.write(0);
- wait(1);
- //move bakc to the starting position
- directionPin_key.write(ccw);
- PWM_key.write(1);
- //time needs to be slightly longer than moving forward (no answer why)
- wait(0.165f);
- PWM_key.write(0);
+ PWM_key.write(1);
+ wait(0.05);
+ PWM_key.write(0);
+ wait(0.3);
+ directionPin_key.write(ccw);
+ PWM_key.write(1);
+ wait(0.039f);
+ PWM_key.write(0);
- wait(1);
+ wait(0.5);
//pressing va
- directionPin_key.write(cw);
- PWM_key.write(1);
- wait(0.14); //time it takes pwm of one to reach the key
- //pause at the key for one second
- PWM_key.write(0);
- wait(1);
- //move bakc to the starting position
- directionPin_key.write(ccw);
- PWM_key.write(1);
- //time needs to be slightly longer than moving forward (no answer why)
- wait(0.165f);
- PWM_key.write(0);
-
+ directionPin_key.write(cw);
+ PWM_key.write(1);
+ wait(0.05);
+ PWM_key.write(0);
+ wait(0.3);
+ directionPin_key.write(ccw);
+ PWM_key.write(1);
+ wait(0.039f);
+ PWM_key.write(0);
+
// go 1 key to right
directionPin.write(cw);
PWM.write(1);
@@ -578,19 +558,16 @@
PWM.write(0);
//pressing der
- directionPin_key.write(cw);
- PWM_key.write(1);
- wait(0.14); //time it takes pwm of one to reach the key
- //pause at the key for one second
- PWM_key.write(0);
- wait(1);
- //move bakc to the starting position
- directionPin_key.write(ccw);
- PWM_key.write(1);
- //time needs to be slightly longer than moving forward (no answer why)
- wait(0.165f);
- PWM_key.write(0);
-
+ directionPin_key.write(cw);
+ PWM_key.write(1);
+ wait(0.05);
+ PWM_key.write(0);
+ wait(0.3);
+ directionPin_key.write(ccw);
+ PWM_key.write(1);
+ wait(0.039f);
+ PWM_key.write(0);
+
// go 1 key to right
directionPin.write(cw);
PWM.write(1);
@@ -598,18 +575,15 @@
PWM.write(0);
//pressing ja
- directionPin_key.write(cw);
- PWM_key.write(1);
- wait(0.14); //time it takes pwm of one to reach the key
- //pause at the key for one second
- PWM_key.write(0);
- wait(1);
- //move bakc to the starting position
- directionPin_key.write(ccw);
- PWM_key.write(1);
- //time needs to be slightly longer than moving forward (no answer why)
- wait(0.165f);
- PWM_key.write(0);
+ directionPin_key.write(cw);
+ PWM_key.write(1);
+ wait(0.05);
+ PWM_key.write(0);
+ wait(0.3);
+ directionPin_key.write(ccw);
+ PWM_key.write(1);
+ wait(0.039f);
+ PWM_key.write(0);
// go 2 key to left
directionPin.write(ccw);
@@ -618,18 +592,15 @@
PWM.write(0);
//pressing cob
- directionPin_key.write(cw);
- PWM_key.write(1);
- wait(0.14); //time it takes pwm of one to reach the key
- //pause at the key for one second
- PWM_key.write(0);
- wait(1);
- //move bakc to the starting position
- directionPin_key.write(ccw);
- PWM_key.write(1);
- //time needs to be slightly longer than moving forward (no answer why)
- wait(0.165f);
- PWM_key.write(0);
+ directionPin_key.write(cw);
+ PWM_key.write(1);
+ wait(0.05);
+ PWM_key.write(0);
+ wait(0.3);
+ directionPin_key.write(ccw);
+ PWM_key.write(1);
+ wait(0.039f);
+ PWM_key.write(0);
}
}
}