Marijke Zondag
werkt nog niet
Dependencies: HIDScope MODSERIAL biquadFilter mbed QEI
Fork of
Project_script_union
by 
Marijke Zondag
Diff: main.cpp
- Revision:
- 30:8191e8541a0a
- Parent:
- 29:df10cb76ef26
- Child:
- 31:481ad25a40c3
--- a/main.cpp Thu Nov 01 08:21:31 2018 +0000
+++ b/main.cpp Thu Nov 01 12:46:20 2018 +0000
@@ -22,11 +22,11 @@
InterruptIn button1 (D10);
InterruptIn button2 (D11);
-DigitalOut directionpin1 (D7);
-DigitalOut directionpin2 (D4);
+DigitalOut directionpin1 (D4);
+DigitalOut directionpin2 (D7);
-PwmOut pwmpin1 (D6);
-PwmOut pwmpin2 (D5);
+PwmOut pwmpin1 (D5);
+PwmOut pwmpin2 (D6);
DigitalOut ledr (LED_RED);
DigitalOut ledb (LED_BLUE);
@@ -34,21 +34,18 @@
MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
-QEI encoder1 (D9, D8, NC, 8400,QEI::X4_ENCODING);
-QEI encoder2 (D12, D13, NC, 8400,QEI::X4_ENCODING);
+QEI encoder1 (D9, D8, NC, 8400 , QEI::X4_ENCODING);
+QEI encoder2 (D12, D13, NC, 8400 , QEI::X4_ENCODING);
//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
//Tickers
Ticker func_tick;
-Ticker engine_control1_tick;
-Ticker engine_control2_tick;
-Ticker encoder1_read_tick;
-Ticker encoder2_read_tick;
+
//Global variables
-const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders???
-const float T2 = 0.1f;
+const float T = 0.02f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders???
+const float T2 = 0.02f;
// Inverse Kinematica variables
const double L1 = 0.208; // Hoogte van tafel tot joint 1
@@ -61,8 +58,8 @@
// Variërende variabelen inverse kinematics:
double q1ref = 0.0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
double q2ref = 0.0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
-double v_x=0.0; // Desired velocity end effector in x direction --> Determined by EMG signals
-double v_y=0.0; // Desired velocity end effector in y direction --> Determined by EMG signals
+double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals
+double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals
double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
@@ -75,7 +72,7 @@
//Variables PID controller
double PI = 3.14159;
-double Kp1 = 1.0; //Motor 1 eerst 17.5 , nu 1
+double Kp1 = 17.5; //Motor 1 eerst 17.5 , nu 1
double Ki1 = 1.02;
double Kd1 = 23.2;
double encoder_radians1=0;
@@ -83,7 +80,7 @@
double err_prev1 = 0;
-double Kp2 = 1.0; //Motor 2 eerst 17.5, nu 1
+double Kp2 = 17.5; //Motor 2 eerst 17.5, nu 1
double Ki2 = 1.02;
double Kd2 = 23.2;
double encoder_radians2=0;
@@ -103,82 +100,84 @@
//---------PID controller motor 1 + motor control 1 & 2-----------------------------------------------------------//
double PID_control(double err, const double Kp, const double Ki, const double Kd, double &err_integral, double &err_prev)
{
- pc.printf("ik doe het, PDI 1\n\r");
-
- err_integral = 0;
- err_prev = err; // initialization with this value only done once!
-
- static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+ pc.printf("ik doe het, PDI \n\r");
- // Proportional part:
- double u_k = Kp * err;
+ err_integral = 0;
+ err_prev = err; // initialization with this value only done once!
+
+ static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
- /* Integral part
- err_integral = err_integral + err * T;
- double u_i = Ki * err_integral;
+ // Proportional part:
+ double u_k = Kp * err;
- // Derivative part
- double err_derivative = (err - err_prev)/T;
- double filtered_err_derivative = LowPassFilter.step(err_derivative);
- double u_d = Kd * filtered_err_derivative;
- err_prev = err;
- */
+ //Integral part
+ err_integral = err_integral + err * T;
+ double u_i = Ki * err_integral;
+
+ // Derivative part
+ double err_derivative = (err - err_prev)/T;
+ double filtered_err_derivative = LowPassFilter.step(err_derivative);
+ double u_d = Kd * filtered_err_derivative;
+ err_prev = err;
+
+
+ // Sum all parts and return it
+ return u_k + u_i + u_d;
+ }
- // Sum all parts and return it
- return u_k; //+ u_i + u_d;
-}
-
-void engine_control1() //Engine 1 is rotational engine, connected with left side pins
+void engine_control1() //Engine 1 is translational engine, connected with left side pins
{
- //while(start_control == 1)
- //{
- pc.printf("ik doe het, engine control 1\n\r");
+ //pc.printf("ik doe het, engine control 1\n\r");
encoder_radians1 = encoder1.getPulses()*(2*PI)/8400.0;
+ pc.printf("encoder1 %d \n\r",encoder1.getPulses());
+ pc.printf("encoder_radians1 %f \n\r",encoder_radians1);
double err1 = q1ref - encoder_radians1;
+ pc.printf("err1 = %f\n\r",err1);
double u1 = PID_control(err1, Kp1, Ki1, Kd1, err_integral1, err_prev1); //PID controller function call
- if(encoder1.getPulses()<5250 && encoder1.getPulses()>-5250) //limits rotation, in counts
+ pc.printf("u1 = %f\n\r",u1);
+
+ if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600
{
- pwmpin1 = 0.5; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
- directionpin1.write(0);
+ pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin1.write(u1<0);
}
else
{
pwmpin1 = 0;
}
-
- // break;
- //}
}
-void engine_control2() //Engine 2 is translational engine, connected with right side wires
+void engine_control2() //Engine 2 is rotational engine, connected with right side wires
{
- pc.printf("ik doe het, engine control 2\n\r");
-
encoder_radians2 = encoder2.getPulses()*(2*PI)/8400.0;
+ pc.printf("encoder2 %f \n\r",encoder2.getPulses());
+ pc.printf("encoder_radians2 %d \n\r",encoder_radians2);
double err2 = q2ref - encoder_radians2;
+ pc.printf("err2 = %f\n\r",err2);
double u2 = PID_control(err2, Kp2, Ki2, Kd2, err_integral2, err_prev2); //PID controller function call
-
- if(encoder2.getPulses()<12600 && encoder2.getPulses()>-1) //limits translation in counts
+ pc.printf("u2 = %f\n\r",u2);
+
+ if(encoder2.getPulses()<5250 && encoder2.getPulses()>-5250) //limits rotation, in counts
{
- pwmpin2 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
- directionpin2.write(0);
+ pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin2.write(u2>0);
}
else
{
pwmpin2 = 0;
}
-
}
//------------------ Inversed Kinematics --------------------------------//
-void inverse_kinematics()
+void inverse_kinematics(double v_x, double v_y)
{
- pc.printf("ik doe het, inverse kinematics\n\r");
+ pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
+
Lq1 = q1ref*r_trans;
Cq2 = q2ref/5.0;
@@ -191,6 +190,10 @@
q1ref = q1_ii;
q2ref = q2_ii;
+
+ pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);
+
+
//start_control = 1;
engine_control1();
engine_control2();
@@ -203,14 +206,17 @@
if(m1>0.5) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
{
- v_x = 0.5f; //beweging in +x direction
+ v_x = 0.5; //beweging in +x direction
+ v_y = 0.0;
ledr = 0; //red
ledb = 1;
ledg = 1;
+
}
else if(m2>0.5) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
{
- v_y = 0.5f; //beweging in +y direction
+ v_y = 0.5; //beweging in +y direction
+ v_x = 0.0;
ledr = 1; //green
ledb = 1;
ledg = 0;
@@ -218,7 +224,7 @@
else if(m1 < -0.5) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
{
- v_x = -0.5f;
+ v_x = -0.5;
v_y = 0;
ledr = 1; //Blue
ledb = 0;
@@ -228,12 +234,13 @@
else if(m2 < -0.5) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
{
v_x = 0;
- v_y = -0.5f;
+ v_y = -0.5;
ledr = 1; //Blue
ledb = 0;
ledg = 1;
}
+
else //If not higher than the threshold, motors will not turn at all
{
v_x = 0;
@@ -244,8 +251,8 @@
//pwmpin1 = 0;
//pwmpin2 = 0;
}
-
- inverse_kinematics(); //Call inverse kinematics function
+
+ inverse_kinematics(v_x, v_y); //Call inverse kinematics function
}
@@ -254,20 +261,22 @@
int main()
{
- wait(1.0f);
pc.baud(115200);
pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
func_tick.attach(&v_des_calculate_qref,T2); //v_des determined every T
+
+
+ while(true)
+ {
//engine_control1_tick.attach(&engine_control1,T2);
//engine_control2_tick.attach(&engine_control2,T2);
// HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
-
- while(true)
- {
- pc.printf("encoder1 %d\n\r",encoder1.getPulses());
- pc.printf("Encoder2 %d\n\r",encoder2.getPulses());
+
+ pc.printf("Encoder engine 1 %d\n\r",encoder2.getPulses());
+ pc.printf("Encoder engine 2 %d\n\r",encoder1.getPulses());
+ wait(0.1f);
}
}