Wouter Bos
werkend x-y control
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 8:649a5f555b7b
- Parent:
- 7:84abed2f376c
- Child:
- 9:5140b3a95dc9
--- a/main.cpp Thu Oct 22 16:24:44 2015 +0000
+++ b/main.cpp Fri Oct 23 11:16:46 2015 +0000
@@ -8,7 +8,7 @@
HIDScope scope(4); // definieerd het aantal kanalen van de scope
// Define Tickers and control frequencies
-Ticker controller1, controller2, main_filter, cartesian;
+Ticker controller1, controller2, main_filter, cartesian, send;
// Go flag variables belonging to Tickers
volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;
@@ -38,15 +38,18 @@
// control flow
DigitalIn buttonlinks(PTA4);
DigitalIn buttonrechts(PTC6);
+ DigitalIn reset_button(D1);
// init values
bool loop_start = false;
bool calib_start = false;
+ bool reset = false;
// axis control
DigitalIn switch_xy_button(D0);
// init values
bool switch_xy = false;
-
+// LED outputs on bioshield
+ DigitalOut led_right(D2);
// LED outputs on dev-board
DigitalOut ledred(LED1);
DigitalOut ledgreen(LED2);
@@ -55,6 +58,7 @@
//////////////////////////////////////////////////////////////////////////////////////////////
+double t = 0;
// physical constants
const double L = 36; // lenght of arms
const double pi = 3.1415926535897;
@@ -81,21 +85,25 @@
double phi_one;
double phi_two;
-// define start up variables
+// define start up variables (is to create a delayed start up instead of going to the ref value inmediately)
double start_up_time = 2;
double start_loops = start_up_time*controlfreq;
int rc1 = 0; // counters in function to enable slow start up
int rc2 = 0;
+// define return to start variables
+double reset_phi_one = 0;
+double reset_phi_two = 0;
-// REFERENCE SIGNAL SETTINGS
+// REFERENCE SETTINGS
double input_threshold = 0.25; // the minimum value the signal must have to change the reference.
- // Define storage variables for reference values
- double c_reference1 = 0;
- double c_reference2 = 0;
-
-// Define the maximum rate of change for the reference (velocity)
- double Vmax = 3; // rad/sec
+ // Define storage variables for reference values (also start position)
+ double c_reference_x = 60, c_reference_y = 0;
+// x-settings (no y-settings because these are calculated from the current x-position)
+ double x_min = 47, x_max = 70;
+ double xx,yy,y_min,y_max;
+ // Define the maximum rate of change for the reference (velocity)
+ double Vmax_x = 10, Vmax_y = 10; // [cm/s]
// CONTROLLER SETTINGS
@@ -104,9 +112,9 @@
const double m1_Ki = 0.5; // integration constant
const double m1_Kd = 0.4; // differentiation constant
// motor 2
- const double m2_Kp = 2;
+ const double m2_Kp = 3;
const double m2_Ki = 0.5;
- const double m2_Kd = 0.1;
+ const double m2_Kd = 0.3;
// storage variables. these variables are used to store data between controller iterations
// motor 1
double m1_err_int = 0;
@@ -136,11 +144,11 @@
double f1_v1t = 0, f1_v2t = 0, f2_v1t = 0, f2_v2t = 0, f3_v1t = 0, f3_v2t = 0,f4_v1t = 0, f4_v2t = 0;
// Filter coefficients
- // differential action filter (lowpass 5Hz at 50samples)
+ // differential action filter (lowpass 5Hz at 50Hz)
const double m_f_a1 = -1.1430, m_f_a2 = 0.4128, m_f_b0 = 0.0675, m_f_b1 = 0.1349, m_f_b2 = 0.0675;
// input filter (lowpass 1Hz at 50samples) (used to make the x-y angle signal as smooth as possible
const double r1_f_a1 = -1.6475, r1_f_a2 = 0.7009, r1_f_b0 = 0.0134, r1_f_b1 = 0.0267, r1_f_b2 = 0.0134;
- // EMG-Filter
+ // EMG-Filter (calculated for 500Hz)
// tweede orde notch filter 50 Hz
// biquad 1 coefficienten
const double numnotch50biq1_1 = 1, numnotch50biq1_2 = -1.61816178466632, numnotch50biq1_3 = 1.00000006127058, dennotch50biq1_2 = -1.59325742941798, dennotch50biq1_3 = 0.982171881701431;
@@ -175,8 +183,8 @@
// This functions takes a 0->1 input, uses passing by reference (&c_reference)
-// to create a reference that moves with a variable speed. It is meant for 0->1 values
-double reference_f(double input, double &c_reference, double limlow, double limhigh)
+// to create a reference that moves with a variable speed. It is meant for -1->1 values
+double reference_f(double input, double &c_reference, double limlow, double limhigh, double Vmax)
{
double reference = c_reference + input * controlstep * Vmax ;
// two if statements check if the reference exceeds the limits placed upon the arms
@@ -211,7 +219,7 @@
}
-// BIQUADFILTER CODE GIVEN IN SHEETS
+// BIQUADFILTER CODE GIVEN IN SHEETS (input format: den, den, nom, nom, nom)
double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
{
double v = u - a1*v1 - a2*v2;
@@ -222,7 +230,7 @@
}
// biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee)
-// (niet netjes maar werkt goed)
+// (niet netjes maar werkt)
double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t)
{
double vt = ut- a1t*v1t-a2t*v2t;
@@ -253,7 +261,7 @@
}
-// function that limits the angles that can be used as reference
+// 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)
@@ -267,6 +275,10 @@
return phi;
}
+void mod_send()
+{
+ pc.printf("xx = %f, yy = %f, phi1 = %f, phi2 = %f \n",xx,yy,phi_one,phi_two);
+}
/////////////////////////////////////////////////////////////////////
////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
///////////////////////////////////////////////////////////////////
@@ -299,29 +311,70 @@
//scope.set(0,output1);
//scope.set(1,output2);
- scope.set(0,output1_amp);
- scope.set(1,output2_amp);
- scope.send();
+ //scope.set(0,output1_amp);
+ //scope.set(1,output2_amp);
+ //scope.send();
}
// function that updates the required motor angles
void det_angles()
{
- // limit the output between 0 and 1
- if(output1_amp>1) {output1_amp = 1;}
- if(output2_amp>1) {output2_amp = 1;}
-
+ // convert global to local variable
+ double xy_input1 = output1_amp;
+ double xy_input2 = output2_amp;
+
// use potmeter for debugging purposes
- //output1 = potright.read();
- //output2 = potleft.read();
+ xy_input1 = potright.read();
+ xy_input2 = potleft.read();
+
+ // add threshold value for outputs
+ if(xy_input1 < input_threshold){xy_input1 = 0;}
+ if(xy_input2 < input_threshold){xy_input2 = 0;}
- double xx = 50+output1_amp*20;
+ // return the input to a value between 0 and 1 (otherwise you will get jumps in input)
+ xy_input1 = (xy_input1-input_threshold) * (1/(1-input_threshold));
+ xy_input2 = (xy_input2-input_threshold) * (1/(1-input_threshold)) ;
+ // if below 0 = 0
+ if(xy_input1 < 0){xy_input1 = 0;}
+ if(xy_input2 < 0){xy_input2 = 0;}
+
+
+ // limit signal to 1
+ if(xy_input1 > 1){xy_input1 = 1;}
+ if(xy_input2 > 1){xy_input2 = 1;}
+
+ 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);
- double ymin = -16; //- sqrt(4900 - pow(xx,2));
- double ymax = 16; //sqrt(4900 - pow(xx,2));
- double yy = ymin+output2_amp*(ymax-ymin);
+ // limit the output between -1 and 1
+ if(xy_main_input>1) {xy_main_input = 1;}
+ if(xy_main_input<-1) {xy_main_input = -1;}
+
+ if(switch_xy == false) // x-movement
+ {
+ xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x); // change the global x-reference
+ // calculate the y limits belonging to that particular x coordinate
+ y_min = - sqrt(4900 - pow(xx,2));
+ if(y_min<-35){y_min = -35;} // make sure the arm cannot hit the table (may later be removed)
+ y_max = sqrt(4900 - pow(xx,2));
+
+ }
+ if(switch_xy == true)
+ {
+ yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y); // change the y-reference
+
+ }
+ // last check for the depedent y-reference (otherwise if x is controlled after y, the circle is not followed).
+ if(yy < y_min){yy = y_min;}
+ if(yy > y_max){yy = y_max;}
+ // let the arm make a circle
+ // 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
@@ -346,13 +399,19 @@
phi_one = phi1;
phi_two = phi2;
- // debugging line
- pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two);
+ if(reset == true)
+ {
+ phi_one = reset_phi_one;
+ phi_two = reset_phi_two;
+ }
+
+ // modserial
+ // pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two);
}
// MOTOR 1
void motor1_control()
{
-
+ // change global into local variable
double reference1 = phi_one;
// add smooth start up
@@ -369,8 +428,10 @@
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);
-
- m_output1 = outputlimiter(m_output1,1); // relimit the output for safety
+ // check the real angles and stop if exceeded (NaN protection)
+ // 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
if(m_output1 > 0) { // uses the calculated output to determine the direction of the motor
motor1_rich.write(0);
motor1_aan.write(m_output1);
@@ -396,9 +457,13 @@
}
double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
- double error2 = (reference2 - rads2); // determine the error (reference - position)
+ 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);
@@ -411,23 +476,22 @@
// calibrate the emg-signal
// works bij taking a certain amount of samples adding them and then normalize to the desired value
-// STILL BUGGED!!!
+// STILL BUGGED!!! // went to max-value type. must be tested.!
void calibrate_amp()
{
- double total1 = 0;
- double total2 = 0;
+ double max1 = 0;
+ double max2 = 0;
for(int i = 0; i<cal_samples; i++)
{
EMG_filter(); // run filter
- double input1 = output1;
- total1 = total1 + input1; // sum inputs
-
+ double input1 = output1; // take data from global variable
+ if(input1>max1){max1 = input1;} // take max input
double input2 = output2;
- total2 = total2 + input2;
- wait(0.02);
+ if(input2>max2){max2 = input2;} // take max input
+ wait(controlstep);
}
- emg_gain1 = normalize_emg_value/(total1/cal_samples); // normalize the amplification so that the maximum signal hits the desired one
- emg_gain2 = normalize_emg_value/(total2/cal_samples);
+ 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);
}
@@ -447,7 +511,8 @@
main_filter.attach(&EMG_activate, controlstep);
cartesian.attach(&angle_activate, controlstep);
controller1.attach(&motor1_activate, controlstep); // call a go-flag
- controller2.attach(&motor2_activate, controlstep);
+ controller2.attach(&motor2_activate, controlstep);
+ send.attach(&mod_send, 1);
while(true)
{
// button press functions
@@ -469,7 +534,16 @@
{
switch_xy = !switch_xy;
wait(switch_xy_button.read() == 1);
- wait(0.3);
+ led_right.write(!led_right.read()); // turn on led when switched to y control
+ wait(0.2);
+ }
+ if(reset_button.read() == 0)
+ {
+ reset = !reset;
+ wait(reset_button.read() == 1);
+ wait(0.3);
+ rc1 = 0;
+ rc2 = 0;
}
//////////////////////////////////////////////////
// Main Control stuff and options
@@ -482,7 +556,7 @@
if(motor2_go) { motor2_go = false; motor2_control();}
}
// shut off both motors
- if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);}
+ if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);} // c_reference_x = 60; c_reference_y = 0;}
// turn green led on // start calibration procedures
if(loop_start == false && calib_start == true)
@@ -499,7 +573,6 @@
LED(0,1,1);
motor1_aan.write(0);
motor2_aan.write(0);
- // switch_type = !switch_type;
}
// turn leds off (both buttons false)