Marijke Zondag
werkt nog niet
Dependencies: HIDScope MODSERIAL biquadFilter mbed QEI
Fork of
Project_script_union
by 
Marijke Zondag
Diff: main.cpp
- Revision:
- 29:df10cb76ef26
- Parent:
- 28:61d1372349c8
- Child:
- 30:8191e8541a0a
diff -r 61d1372349c8 -r df10cb76ef26 main.cpp
--- a/main.cpp Wed Oct 31 19:14:56 2018 +0000
+++ b/main.cpp Thu Nov 01 08:21:31 2018 +0000
@@ -3,6 +3,7 @@
#include "BiQuad.h"
#include "HIDScope.h"
#include <math.h>
+#include "QEI.h"
//ATTENTION: set mBed to version 151
// set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
@@ -10,8 +11,8 @@
// set HIDScope to version 7
// set biquadFilter to version 7
-//AnalogIn potmeter1 (A0); //First raw EMG signal input
-//AnalogIn potmeter2 (A1); //Second raw EMG signal input
+AnalogIn potmeter1 (A0); //First raw EMG signal input
+AnalogIn potmeter2 (A1); //Second raw EMG signal input
InterruptIn encoderA1 (D9);
InterruptIn encoderB1 (D8);
@@ -33,17 +34,21 @@
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);
//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 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.01f;
+const float T2 = 0.1f;
// Inverse Kinematica variables
const double L1 = 0.208; // Hoogte van tafel tot joint 1
@@ -54,280 +59,122 @@
const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation
// Variërende variabelen inverse kinematics:
-double q1ref = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
-double q2ref = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
-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 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 Lq1; // Translatieafstand als gevolg van motor rotation joint 1
double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
-double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
-double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
+double q1_dot=0.0; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
+double q2_dot=0.0; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
-double q1_ii; // Reference signal for motorangle q1ref
-double q2_ii; // Reference signal for motorangle q2ref
+double q1_ii=0.0; // Reference signal for motorangle q1ref
+double q2_ii=0.0; // Reference signal for motorangle q2ref
//Variables PID controller
double PI = 3.14159;
-double Kp1 = 5.0; //Motor 1 eerst 17.5 , nu 5
+double Kp1 = 1.0; //Motor 1 eerst 17.5 , nu 1
double Ki1 = 1.02;
double Kd1 = 23.2;
-double encoder1 = 0;
double encoder_radians1=0;
+double err_integral1 = 0;
+double err_prev1 = 0;
-double Kp2 = 5.0; //Motor 2 eerst 17.5, nu 5
+
+double Kp2 = 1.0; //Motor 2 eerst 17.5, nu 1
double Ki2 = 1.02;
double Kd2 = 23.2;
-double encoder2 = 0;
double encoder_radians2=0;
+double err_integral2 = 0;
+double err_prev2 = 0;
int start_control = 0;
-
-//double potmeter1s = (potmeter1*2)-1.0f;
-//double potmeter2s = (potmeter2*2)-1.0f;
double emg_cal = 1;
//--------------Functions----------------------------------------------------------------------------------------------------------------------------//
-//------------------ Encoder motor 1 --------------------------------//
-
-void encoderA1_rise()
-{
- if(encoderB1==false)
- {
- encoder1++;
- }
- else
- {
- encoder1--;
- }
-}
-
-void encoderA1_fall()
-{
- if(encoderB1==true)
- {
- encoder1++;
- }
- else
- {
- encoder1--;
- }
-}
-
-void encoderB1_rise()
-{
- if(encoderA1==true)
- {
- encoder1++;
- }
- else
- {
- encoder1--;
- }
-}
-
-void encoderB1_fall()
-{
- if(encoderA1==false)
- {
- encoder1++;
- }
- else
- {
- encoder1--;
- }
-}
-
-void encoder_count1()
-{
- encoderA1.rise(&encoderA1_rise);
- encoderA1.fall(&encoderA1_fall);
- encoderB1.rise(&encoderB1_rise);
- encoderB1.fall(&encoderB1_fall);
-}
-
-//------------------ Encoder motor 2 --------------------------------//
-
-void encoderA2_rise()
-{
- if(encoderB2==false)
- {
- encoder2++;
- }
- else
- {
- encoder2--;
- }
-}
-
-void encoderA2_fall()
-{
- if(encoderB2==true)
- {
- encoder2++;
- }
- else
- {
- encoder2--;
- }
-}
-
-void encoderB2_rise()
-{
- if(encoderA2==true)
- {
- encoder2++;
- }
- else
- {
- encoder2--;
- }
-}
-
-void encoderB2_fall()
-{
- if(encoderA2==false)
- {
- encoder2++;
- }
- else
- {
- encoder2--;
- }
-}
-
-void encoder_count2()
-{
- encoderA2.rise(&encoderA2_rise);
- encoderA2.fall(&encoderA2_fall);
- encoderB2.rise(&encoderB2_rise);
- encoderB2.fall(&encoderB2_fall);
-}
-
//------------------ Filter EMG + Calibration EMG --------------------------------//
-//------------------ Inversed Kinematics --------------------------------//
-
-
-//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
-double PID_controller1(double err1)
+//---------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");
- static double err_integral1 = 0;
- static double err_prev1 = err1; // initialization with this value only done once!
+ err_integral = 0;
+ err_prev = err; // initialization with this value only done once!
- static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+ static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
// Proportional part:
- double u_k1 = Kp1 * err1;
+ double u_k = Kp * err;
/* Integral part
- err_integral1 = err_integral1 + err1 * T;
- double u_i1 = Ki1 * err_integral1;
+ err_integral = err_integral + err * T;
+ double u_i = Ki * err_integral;
// Derivative part
- double err_derivative1 = (err1 - err_prev1)/T;
- double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1);
- double u_d1 = Kd1 * filtered_err_derivative1;
- err_prev1 = err1;
+ 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_k1+0; //+ u_i1 + u_d1;
+ return u_k; //+ u_i + u_d;
}
-void start_your_engines1(double u1)
-{
- pc.printf("ik doe het, engine start 1\n\r");
-
- if(encoder1<5250 && encoder1>-5250) //limits rotation, in counts
- {
- pwmpin1 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
- directionpin1.write(0);
- }
- else
- {
- pwmpin1 = 0;
- }
-}
-
void engine_control1() //Engine 1 is rotational engine, connected with left side pins
{
//while(start_control == 1)
//{
pc.printf("ik doe het, engine control 1\n\r");
- encoder_radians1 = encoder1*(2*PI)/8400;
+ encoder_radians1 = encoder1.getPulses()*(2*PI)/8400.0;
double err1 = q1ref - encoder_radians1;
- double u1 = PID_controller1(err1); //PID controller function call
- start_your_engines1(u1);
+ 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
+ {
+ pwmpin1 = 0.5; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin1.write(0);
+ }
+ else
+ {
+ pwmpin1 = 0;
+ }
// break;
//}
}
-
-
-//---------PID controller motor 2 + start motor 2 -----------------------------------------------------------//
-double PID_controller2(double err2)
-{
- pc.printf("ik doe het, PDI 2\n\r");
-
- static double err_integral2 = 0;
- static double err_prev2 = err2; // initialization with this value only done once!
-
- static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-
- // Proportional part:
- double u_k2 = Kp2 * err2;
-
- /* Integral part
- err_integral2 = err_integral2 + err2 * T;
- double u_i2 = Ki2 * err_integral2;
-
- // Derivative part
- double err_derivative2 = (err2 - err_prev2)/T;
- double filtered_err_derivative2 = LowPassFilter2.step(err_derivative2);
- double u_d2 = Kd2 * filtered_err_derivative2;
- err_prev2 = err2;
- */
-
- // Sum all parts and return it
- return u_k2+0; //+ u_i2 + u_d2;
-}
-
-void start_your_engines2(double u2)
-{
- pc.printf("ik doe het, engine start 2\n\r");
-
- if(encoder2<12600 && encoder2>-1) //limits translation in counts
- {
- pwmpin2 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
- directionpin2.write(0);
- }
- else
- {
- pwmpin2 = 0;
- }
-
-}
-
void engine_control2() //Engine 2 is translational engine, connected with right side wires
{
pc.printf("ik doe het, engine control 2\n\r");
- encoder_radians2 = encoder2*(2*PI)/8400;
+ encoder_radians2 = encoder2.getPulses()*(2*PI)/8400.0;
double err2 = q2ref - encoder_radians2;
- double u2 = PID_controller2(err2); //PID controller function call
- start_your_engines2(u2); //Call start_your_engines function
+ 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
+ {
+ pwmpin2 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+ directionpin2.write(0);
+ }
+ else
+ {
+ pwmpin2 = 0;
+ }
+
}
+//------------------ Inversed Kinematics --------------------------------//
+
+
void inverse_kinematics()
{
@@ -338,43 +185,55 @@
q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));
q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));
- q1_ii = q1ref + q1_dot*T;
- q2_ii = q2ref + q2_dot*T;
+ q1_ii = q1ref + (q1_dot/r_trans)*T;
+ q2_ii = q2ref + (q2_dot*5.0)*T;
q1ref = q1_ii;
q2ref = q2_ii;
//start_control = 1;
- engine_control1();
+ engine_control1();
engine_control2();
}
void v_des_calculate_qref()
{
- if(button1==0) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
+ double m1 = (potmeter1*2.0)-1.0;
+ double m2 = (potmeter2*2.0)-1.0;
+
+ 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
ledr = 0; //red
ledb = 1;
ledg = 1;
}
- else if(button2==0) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
+ 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
ledr = 1; //green
ledb = 1;
ledg = 0;
}
- /*
- else if(button1==0 && button2==0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
+
+ 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_y = 0;
+ ledr = 1; //Blue
+ ledb = 0;
+ ledg = 1;
+ }
+
+ 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;
ledr = 1; //Blue
ledb = 0;
ledg = 1;
- }
- */
+ }
+
else //If not higher than the threshold, motors will not turn at all
{
v_x = 0;
@@ -394,7 +253,8 @@
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
@@ -405,7 +265,9 @@
// 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());
+ }
}