Wouter Bos
presentatie versie met potmeters enabled
Dependencies: Encoder HIDScope mbed
Diff: main.cpp
- Revision:
- 5:ad4ae6b65474
- Parent:
- 4:9684b6f8b63c
- Child:
- 6:30afff8ae34a
--- a/main.cpp Wed Oct 28 13:34:22 2015 +0000
+++ b/main.cpp Thu Oct 29 10:33:03 2015 +0000
@@ -1,19 +1,17 @@
#include "mbed.h"
-#include "MODSERIAL.h"
#include "encoder.h"
#include "HIDScope.h"
#include "math.h"
-Serial pc(USBTX,USBRX); // MODSERIAL output
HIDScope scope(4); // definieerd het aantal kanalen van de scope
// Define Tickers and control frequencies
-Ticker controller1, controller2, main_filter, cartesian, send;
+Ticker controller1, controller2, main_filter, cartesian;
// Go flag variables belonging to Tickers
volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;
// Frequency control
- double controlfreq = 50 ; // controlloops frequentie (Hz)
+ double controlfreq = 200 ; // controlloops frequentie (Hz)
double controlstep = 1/controlfreq; // timestep derived from controlfreq
double EMG_freq = 200;
@@ -47,9 +45,9 @@
bool reset = false;
// axis control
- DigitalIn switch_xy_button(D0);
+ DigitalIn program_button(D0);
// init values
- bool switch_xy = false;
+ bool program = false;
// LED outputs on bioshield
DigitalOut led_right(D2);
// LED outputs on dev-board
@@ -62,9 +60,9 @@
////////////////////////// GLOBAL VARIABLES ///////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
-double t = 0; // used for circle movement
-
-double sw1 = 0;
+ bool switch_xy = false; // bool for switching
+ double sw1 = 0; // counter for switching axes
+ double t_switch = 0.6; // seconds for switching
// physical constants
const double L = 36; // lenght of arms
const double pi = 3.1415926535897; // pi
@@ -144,7 +142,7 @@
double emg_gain2 = 7;
double cal_time = 2.5; // amount of time calibration should take (seconds)
-double cal_samples = controlfreq*cal_time; // amount of samples that is used (dependent on the frequency of the filter)
+double cal_samples = EMG_freq*cal_time; // amount of samples that is used (dependent on the frequency of the filter)
double normalize_emg_value = 1; // set the desired value to calibrate the signal to (eg max signal = 1)
// FILTER VARIABLES
@@ -198,7 +196,7 @@
// counter
double mt1 = 0;
// time
-double T1 = 4, T2 = T1 + 2, T3 = T2 + 6, T4 = T3 + 6, T5 = T4 + 2, T6 = T5 + 4;
+double T1 = 4, T2 = T1 + 2, T3 = T2 + 6, T4 = T3 + 1, T5 = T4 + 6, T6 = T5 + 4, T7 = T6 + 4, T8 = T7 + 1;
// x,y coordinates
//double x1 = 55, y1 = -15;
//double x2 = 55, y2 = 15;
@@ -209,9 +207,11 @@
double x1 = 50, y1 = -37;
double x2 = 60, y2 = -37;
double x3 = 60, y3 = 30;
-double x4 = 60, y4 = -37;
-double x5 = 50, y5 = -37 ;
-double x6 = 50, y6 = 0;
+double x4 = 60, y4 = 30;
+double x5 = 60, y5 = -37;
+double x6 = 50, y6 = -37 ;
+double x7 = 50, y7 = 0;
+double x8 = 50, y8 = 0;
////////////////////////////////////////////////////////////////
/////////////////// START OF SIDE FUNCTIONS ////////////////////
@@ -307,11 +307,11 @@
// function that limits the angles that can be used in the motor reference signal
double angle_limits(double phi, double limlow, double limhigh)
{
- if(phi < limlow)
+ if(phi < limlow) // determine if the reference is lower than the minimum angle
{
- phi = limlow;
+ phi = limlow; // if lower, set the lower limit as reference.
}
- if(phi > limhigh)
+ if(phi > limhigh) // determine if the reference is higher than the maximum angle
{
phi = limhigh;
}
@@ -323,7 +323,7 @@
double adapt_signal(double input)
{
// add threshold value for outputs
- if(input < input_threshold){input = 0;}
+ if(input < input_threshold){input = 0;} // set the input as zero if the signal is lower than the threshold
// return the input to a value between 0 and 1 (otherwise you will get jumps in input)
input = (input-input_threshold) * (1/(1-input_threshold));
@@ -336,49 +336,74 @@
return input;
}
-// send stuff to putty to check things
-void mod_send()
+
+void switch_axes (double input1,double input2)
{
- pc.printf("xx = %f, yy = %f, phi1 = %f, phi2 = %f \n",xx,yy,phi_one,phi_two);
+ if(input1 > input_threshold && input2 > input_threshold) // when both signals are above the threshold, add one to global counter
+ {
+ sw1++;
+ }
+ if(sw1 == t_switch*controlfreq) // if global counter > s*freq flip the bool.
+ {
+ switch_xy = !switch_xy;
+ led_right.write(!led_right.read()); // turn on led when switched
+ sw1 = 0;
+ }
+ if(input1 < input_threshold || input2 < input_threshold) // if one becomes lower than the threshold, set the global variable to zero
+ {
+ sw1 = 0;
+ }
}
+// this function allows the xx and yy variables to follow a figure determined by a set of coordinates
+// this function is very simple, can be (possibly) improved by writing a single loop.
void square_move()
{
if (mt1 > 0 && mt1 < T1*controlfreq) // horizontal movement from (65,-20) -> (55,-20)
{
- xx = x6 + (x1-x6)*(mt1/(T1*controlfreq));
- yy = y6 + (y1-y6)*(mt1/(T1*controlfreq));
+ xx = x8 + (x1-x8)*(mt1/(T1*controlfreq));
+ yy = y8 + (y1-y8)*(mt1/(T1*controlfreq));
}
- else if (mt1 >= T1*controlfreq && mt1 < T2*controlfreq) // vertical movement (55,-20) -> (55,20)
+ else if (mt1 >= T1*controlfreq && mt1 < T2*controlfreq)
{
xx = x1 + (x2-x1)*(mt1-T1*controlfreq)/(T2*controlfreq-T1*controlfreq) ;
yy = y1 + (y2-y1)*(mt1-T1*controlfreq)/(T2*controlfreq-T1*controlfreq) ;
}
- else if (mt1 >= T2*controlfreq && mt1 < T3*controlfreq) // horizontal movement (55,20) -> (65,20)
+ else if (mt1 >= T2*controlfreq && mt1 < T3*controlfreq)
{
xx = x2 + (x3-x2)*(mt1-T2*controlfreq)/(T3*controlfreq-T2*controlfreq) ;
yy = y2 + (y3-y2)*(mt1-T2*controlfreq)/(T3*controlfreq-T2*controlfreq);
}
- else if (mt1 >= T3*controlfreq && mt1 < T4*controlfreq) // vertical movement (65,20) -> (65,-20)
+ else if (mt1 >= T3*controlfreq && mt1 < T4*controlfreq)
{
xx = x3 + (x4-x3)*(mt1-T3*controlfreq)/(T4*controlfreq-T3*controlfreq) ;
yy = y3 + (y4-y3)*(mt1-T3*controlfreq)/(T4*controlfreq-T3*controlfreq) ;
}
- else if (mt1 >= T4*controlfreq && mt1 < T5*controlfreq) // vertical movement (65,20) -> (65,-20)
+ else if (mt1 >= T4*controlfreq && mt1 < T5*controlfreq)
{
xx = x4 + (x5-x4)*(mt1-T4*controlfreq)/(T5*controlfreq-T4*controlfreq) ;
yy = y4 + (y5-y4)*(mt1-T4*controlfreq)/(T5*controlfreq-T4*controlfreq) ;
}
- else if (mt1 >= T5*controlfreq && mt1 < T6*controlfreq) // vertical movement (65,20) -> (65,-20)
+ else if (mt1 >= T5*controlfreq && mt1 < T6*controlfreq)
{
xx = x5 + (x6-x5)*(mt1-T5*controlfreq)/(T6*controlfreq-T5*controlfreq) ;
yy = y5 + (y6-y5)*(mt1-T5*controlfreq)/(T6*controlfreq-T5*controlfreq) ;
}
- else if (mt1 >= T6*controlfreq)
+ else if (mt1 >= T6*controlfreq && mt1 < T7*controlfreq)
+ {
+ xx = x6 + (x7-x6)*(mt1-T6*controlfreq)/(T7*controlfreq-T6*controlfreq) ;
+ yy = y6 + (y7-y6)*(mt1-T6*controlfreq)/(T7*controlfreq-T6*controlfreq) ;
+ }
+ else if (mt1 >= T7*controlfreq && mt1 < T8*controlfreq)
+ {
+ xx = x7 + (x8-x7)*(mt1-T7*controlfreq)/(T8*controlfreq-T7*controlfreq) ;
+ yy = y7 + (y8-y7)*(mt1-T7*controlfreq)/(T8*controlfreq-T7*controlfreq) ;
+ }
+ else if (mt1 >= T8*controlfreq)
{
mt1 = 0;
- xx = x6;
- yy = y6;
+ xx = x8;
+ yy = y8;
}
mt1++;
}
@@ -390,7 +415,7 @@
// function that updates the values of the filtered emg-signal
void EMG_filter()
{
-// filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
+// filtering of EMG signal 1 (A0) first notch(2 biquads), then highpass, rectify(abs()), lowpass
double u1 = EMG_in.read();
double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
@@ -398,10 +423,10 @@
double y4 = abs(y3);
double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
// update global variables
- output1 = y5;
- output1_amp = y5*emg_gain1; // update global variable
+ output1 = y5; // output for calibration
+ output1_amp = y5*emg_gain1; // output for control loop
-// filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
+// filtering of EMG signal 2 (A2) same as before
double u1t = EMG_int.read();
double y1t = biquadfilter( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
double y2t = biquadfilter( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
@@ -411,12 +436,11 @@
// update global variables
output2 = y5t;
output2_amp = y5t*emg_gain2;
- //scope.set(0,output1_amp);
- //scope.set(1,output2_amp);
- //scope.set(2,u1);
- //scope.set(3,u1t);
- //scope.send();
- }
+
+ scope.set(0,output1_amp);
+ scope.set(1,output2_amp);
+ scope.send();
+}
// function that updates the required motor angles from the current filtered emg
@@ -430,59 +454,37 @@
//xy_input1 = potright.read();
//xy_input2 = potleft.read();
+ // add a threshold to the signals and limit to [0,1]
xy_input1 = adapt_signal(xy_input1);
xy_input2 = adapt_signal(xy_input2);
- if(xy_input1 > input_threshold && xy_input2 > input_threshold)
- {
- sw1++;
- }
- if(sw1 == 30)
- {
- switch_xy = !switch_xy;
- led_right.write(!led_right.read()); // turn on led when switched
- sw1 = 0;
- }
- if(xy_input1 < input_threshold || xy_input2 < input_threshold)
- {
- sw1 = 0;
- }
-
+ // function that when both muscles are above a threshold for 3/5s switches the axes
+ switch_axes(xy_input1,xy_input2);
+
double xy_main_input = xy_input1 - xy_input2 ; // subtract inputs to create a signal that can go from -1 to 1
//scope.set(0,xy_main_input);
// limit the output between -1 and 1 (signal is not supposed to be able to go above but last check)
if(xy_main_input>1) {xy_main_input = 1;}
if(xy_main_input<-1) {xy_main_input = -1;}
-
-
- // calculate the y limits belonging to that particular x coordinate and update global variables
+
+ // calculate the y limits belonging to that particular x coordinate and update global variables
y_min = - sqrt(5184 - pow(xx,2));
if(y_min<y_min_max){y_min = y_min_max;} // make sure the arm cannot hit the table (may later be removed)
y_max = sqrt(5184 - pow(xx,2));
- if(switch_xy == false) // use the signal to change the x-reference
- {
- xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x); // change the global x-reference
- }
- if(switch_xy == true) // use the signal to change the y-reference
- {
- yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y); // change the y-reference
- }
- square_move();
- // check the y-reference (otherwise if x is controlled after y has been controlled, the circle is not followed).
+ // use the signal to change the x-reference
+ if(switch_xy == false){xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x);}
+ // use the signal to change the y-reference
+ if(switch_xy == true){yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y);}
+
+ // start the pre-programmed movement if a button has been pressed
+ if(program){square_move();}
+
+ // check the y-reference (otherwise if x is controlled after y has been controlled, the limits are not followed).
if(yy < y_min){yy = y_min;}
if(yy > y_max){yy = y_max;}
- scope.set(0,xx);
- scope.set(1,yy);
- scope.send();
-
- // let the arm make a circle (testing)
- // xx = 60 + 5*cos(t);
- // yy = 5*sin(t);
- // t = t + 0.01;
-
// x-y to arm-angles math
double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
@@ -492,7 +494,8 @@
// convert arm-angles to motor angles( (x transmission) and offset (+ offset) to account for reset position)
double phi1 = 4*(theta_one) + phi_one_offset;
- double phi2 = 4*(theta_one+theta_two) + phi_two_offset; // math assumes angle relative to first arm. motor does not change relative orientation, so angle wrt horizontal position is needed.
+ // math assumes angle relative to first arm. motor does not change relative orientation, so angle wrt horizontal position is needed.
+ double phi2 = 4*(theta_one+theta_two) + phi_two_offset;
phi2 = -phi2; // reverse angle because of transmission.
// check the angles and apply the limits
@@ -506,7 +509,6 @@
// write into global variables
phi_one = phi1;
phi_two = phi2;
-
// if the reset button has been pressed, continiously write the start position into the global variables to reset the arm
if(reset == true)
{
@@ -530,14 +532,10 @@
rc1++;
reference1 = phi_one_curr + ((double) rc1/start_loops)*(reference1-phi_one_curr);
}
-
double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
double error1 = (reference1 - rads1); // determine the error (reference - position)
double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
- // check the real angles and stop if exceeded (NaN protection) // doesnt work because start angle is lower so output stays 0
- // if(rads1 < (limlow1 - 0.05)){m_output1 = 0;}
- // if(rads1 > (limhigh1 + 0.05)){m_output1 = 0;}
- m_output1 = outputlimiter(m_output1,1); // relimit the output between -1 and 1 for safety
+ m_output1 = outputlimiter(m_output1,1); // relimit the output between -1 and 1 for safety
if(m_output1 > 0) { // uses the calculated output to determine the direction of the motor
motor1_rich.write(0);
motor1_aan.write(m_output1);
@@ -559,14 +557,10 @@
{
rc2++;
reference2 = phi_two_curr + ((double) rc2/start_loops)*(reference2-phi_two_curr);
- }
-
+ }
double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
double error2 = (reference2 - rads2); // determine the error (reference - position)
double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
- // check the real angles and stop if exceeded
- // if(rads2 < limlow2 - 0.05){m_output2 = 0;}
- // if(rads2 > limhigh2 + 0.05){m_output2 = 0;}
m_output2 = outputlimiter(m_output2,1); // final output limit (not really needed, is for safety)
if(m_output2 > 0) { // uses the calculated output to determine the direction of the motor
motor2_rich.write(0);
@@ -591,13 +585,11 @@
if(input1>max1){max1 = input1;} // take max input
double input2 = output2;
if(input2>max2){max2 = input2;} // take max input
- wait(controlstep); // !! has to run at same interval as filter in main loop !! otherwise a 'different' signal will be used for calibration
+ wait(EMG_step); // !! has to run at same interval as filter in main loop !! otherwise a 'different' signal will be used for calibration
}
emg_gain1 = normalize_emg_value/max1; // normalize the amplification so that the maximum signal hits the desired one
emg_gain2 = normalize_emg_value/max2;
- pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2); // print the calculated gains to putty
-
-}
+ }
//////////////////////////////////////////////////////////////////
//////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
////////////////////////////////////////////////////////////////
@@ -610,12 +602,10 @@
int main()
{
- pc.baud(115200);
main_filter.attach(&EMG_activate, EMG_step);
cartesian.attach(&angle_activate, controlstep);
controller1.attach(&motor1_activate, controlstep); // call a go-flag
controller2.attach(&motor2_activate, controlstep);
- send.attach(&mod_send, 1); // send data (only global variables) to putty
while(true)
{
// button press functions
@@ -632,13 +622,14 @@
wait(0.2);
while(buttonrechts.read() == 0);
}
- // reverse buttons
- if(switch_xy_button.read() == 0)
+ // start pre-programmed movement
+ if(program_button.read() == 0)
{
- switch_xy = !switch_xy;
- led_right.write(!led_right.read()); // turn on led when switched to y control
+ program = !program;
+ rc1 = 0;
+ rc2 = 0;
wait(0.2);
- while(switch_xy_button.read() == 0);
+ while(program_button.read() == 0);
}
if(reset_button.read() == 0)
{
@@ -649,7 +640,6 @@
rc2 = 0;
wait(0.2);
while(reset_button.read() == 0);
-
}
//////////////////////////////////////////////////
// Main Control stuff and options