Biorobotics Project
not working yet
Dependencies: Motor_with_encoder MODSERIAL mbed HIDScope
Diff: main.cpp
- Revision:
- 15:207d01972e0b
- Parent:
- 14:7a95e57b5364
- Child:
- 16:1dd69f935b80
--- a/main.cpp Mon Oct 30 16:35:20 2017 +0000
+++ b/main.cpp Tue Oct 31 11:07:42 2017 +0000
@@ -19,7 +19,7 @@
//HIDScope scope (2);
EMG_filter emg1(A0);
EMG_filter emg2(A1);
-float EMG_threshold = 0.2; // Threshold for the EMG signal
+float EMG_threshold = 0.25; // Threshold for the EMG signal
// ------------------------
// ---- Motor input and outputs ----
@@ -44,6 +44,7 @@
// --- Define Ticker and Timeout ---
Ticker mainticker; //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_calibration
Timeout calibrationgo; // Timeout that will determine the duration of the calibration.
+Timer duration; // Timer to see how long the maintickerfunction takes to compute
// ---------------------------------
@@ -53,9 +54,9 @@
volatile double Pos2; // Encoder readout of motor 2 [counts]
volatile int count = 0; // Loop counts
-double Kp = 250;// you can set these constants however you like depending on trial & error
-double Ki = 100;
-double Kd = 0;
+double Kp = 25.0;// you can set these constants however you like depending on trial & error
+double Ki = 10.0;
+double Kd = 10.0;
float last_error = 0;
float error1 = 0;
@@ -93,12 +94,12 @@
volatile float W2; // Desired velocity of joint 2 [rad/s];
volatile float W3; // Desired velocity of joint 3 [rad/s];
-volatile float Vx_des; // Desired velocity end-effector in x-direction [m/s]
-volatile float Vy_des; // Desired velocity end-effector in y-direction [m/s]
-volatile float Vz_des; // Desired velocity end-effector in z-direction [m/s]
+float Vx_des; // Desired velocity end-effector in x-direction [m/s]
+float Vy_des; // Desired velocity end-effector in y-direction [m/s]
+float Vz_des; // Desired velocity end-effector in z-direction [m/s]
const float T = 0.002; // Time interval of the readout of the EMG --> Check this with the ticker, etc!!!
-
+const float SampleTime = 0.002; // Time interval of the mainticker
// --- Booleans for the maintickerfunction ---
bool go_calibration; // Boolean to put on/off the calibration of the EMG
@@ -181,7 +182,6 @@
ye = HE0(2,4); // New y-coordinate of the end-effector
ze = HE0(3,4); // New z-coordinate of the end-effector
-
}
void Jacobian(float vx_des, float vy_des, float vz_des) // Function to determine the velocities with desired velocity as input
@@ -193,7 +193,6 @@
Q2 = Pos2; // Position of joint 2 [rad]
Q3 = 0; // Position of joint 3 [rad] --> need this from Servo!
- //pc.printf("\r\nq1 = %.3f, q2 = %.3f, q3 = %.3f\r\n", q1, q2, q3);
Brockett(Q1,Q2,Q3); // Start with conducting Brockett to obtain the end-effector position
@@ -263,17 +262,18 @@
W2 = Q_dot(2,1);
W3 = Q_dot(3,1);
- // pc.printf("\r\nv1 = %.3f, w2 = %.3f, w3 = %.3f\r\n", V1,W2,W3);
-
-// Eva - not sure if we need this... --> I think for the setpoint of the PID controller
+
Q1 += V1*T; // Predicted value for q1 [m]
Q2 += W2*T; // Predicted value for q2 [rad]
Q3 += W3*T; // Predicted value for q3 [rad]
+/*
if (count == 100) {
- //pc.printf("\r\n Q1 = %.3f, Q2 = %.3f, Q3 = %.3f\r\n", Q1,Q2,Q3);
+
+ pc.printf("Velocities v1 = %.3f, w2 = %.3f, w3 = %.3f\r\n", V1,W2,W3);
+ pc.printf("After velocities Q1 = %.3f, Q2 = %.3f, Q3 = %.3f\r\n", Q1,Q2,Q3);
}
-
+*/
}
//--- State switch function -----
@@ -307,7 +307,7 @@
}
// --- Determine the setpoints of the joint velocities
-void setpointreadout(float v_des) // something goes wrong here
+void setpointreadout(float& v_des) // something goes wrong here
{
/*
potvalue = pot.read();
@@ -316,32 +316,33 @@
potvalue2 = pot2.read();
setpoint2 = emg2.normalized;
*/
+ Pos1 = motor1.getPosition()/16800*d*pi; // Position of link 1 [m]
+ Pos2 = motor2.getPosition()/16800*2*pi; // Position of link 2 [m]
- Pos1 = motor1.getPosition()/16800*d*pi; // Position of link 1 [m]
- Pos2 = motor2.getPosition()/16800*2*pi; // Position of link 2 [m]
-
+ /*
+ if (Pos2 >= EMG_threshold) // (pot.read()>= EMG_threshold)
+ {
+ v_des = 0.1; // pot.read();
+
+ }
- if(pot.read()>= EMG_threshold)
- {
- v_des = pot.read();
- pc.printf("potvalue is %f\r\n", v_des);
- }
if (pot2.read() >= EMG_threshold)
{
- v_des = -pot2.read();
+ v_des = -0.1; // -pot2.read();
}
-/*
+ */
+
if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Positive direction
- v_des = emg1.normalized;
+ v_des = emg1.normalized;
+
}
+
+ if (emg2.normalized >= EMG_threshold && emg1.normalized <= EMG_threshold) { // Negative direction
+ v_des = -emg2.normalized;
+ }
+}
- if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Negative direction
- v_des = -emg2.normalized;
- }
- */
-//pc.printf("\r\nv_des in setpoint function= %f\r\n", v_des);
-}
void PIDcalculation(float q1, float q2, float q3) // inputs: potvalue, motor#, setpoint
{
@@ -352,14 +353,15 @@
error1 = q1 - Pos1; // Position error of link 1 [m]
error2 = q2 - Pos2; // Position error of link 2 [m]
- changeError = (error1 - last_error)/0.001f; // derivative term
- totalError += error1*0.001f; //accumalate errors to find integral term
+
+ changeError = (error1 - last_error)/SampleTime; // derivative term
+ totalError += error1*SampleTime; //accumalate errors to find integral term
pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
pidTerm = pidTerm;
- if (pidTerm >= 1000) {
- pidTerm = 1000;
- } else if (pidTerm <= -1000) {
- pidTerm = -1000;
+ if (pidTerm >= 10) {
+ pidTerm = 10;
+ } else if (pidTerm <= -10) {
+ pidTerm = -10;
} else {
pidTerm = pidTerm;
}
@@ -369,19 +371,19 @@
} else {
dir1 = 1;//Reverse motion
}
- pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
+ pidTerm_scaled = abs(pidTerm); ///10.0f; //make sure it's a positive value
if(pidTerm_scaled >= 0.55f) {
pidTerm_scaled = 0.55f;
}
- changeError2 = (error2 - last_error2)/0.001f; // derivative term
- totalError2 += error2*0.001f; //accumalate errors to find integral term
+ changeError2 = (error2 - last_error2)/SampleTime; // derivative term
+ totalError2 += error2*SampleTime; //accumalate errors to find integral term
pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
pidTerm2 = pidTerm2;
- if (pidTerm2 >= 1000) {
- pidTerm2 = 1000;
- } else if (pidTerm2 <= -1000) {
- pidTerm2 = -1000;
+ if (pidTerm2 >= 10) {
+ pidTerm2 = 10;
+ } else if (pidTerm2 <= -10) {
+ pidTerm2 = -10;
} else {
pidTerm2 = pidTerm2;
}
@@ -391,11 +393,14 @@
} else {
dir2 = 0;//Reverse motion
}
- pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value
+ pidTerm_scaled2 = abs(pidTerm2); ///10.0f; //make sure it's a positive value
if(pidTerm_scaled2 >= 0.55f) {
pidTerm_scaled2 = 0.55f;
}
+if (count%100 == 0){
+ //pc.printf("pidterm = %f, pidterm2 = %f\r\n", pidTerm, pidTerm2);}
+
last_error = error1;
speed1 = pidTerm_scaled+0.45f;
//speedservo1 = speed1;
@@ -408,49 +413,47 @@
void r_movehorizontal()
{
setpointreadout(Vx_des); // Start with obtaining the position of the robot and the desired velocities
- if (count == 100) {
- //pc.printf("\r\n Vx_des outside setpoint function = %f\r\n", Vx_des);
- }
Jacobian(Vx_des, 0, 0); // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
PIDcalculation(Q1, Q2, Q3); // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.
-
+ if (count%100 == 0)[
+ pc.printf("Q1 = %f, Q2= %f, Q3 = %f\r\n", Q1, Q2, Q3);}
}
void r_movevertical()
{
setpointreadout(Vy_des); // Start with obtaining the position of the robot and the desired velocities
- if (count == 100) {
- // pc.printf("\r\n Vy_des outside setpoint function = %f\r\n", Vy_des);
- }
Jacobian(0, Vy_des, 0); // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
PIDcalculation(Q1, Q2, Q3); // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.
-
+if (count%100 == 0)[
+ pc.printf("Q1 = %f, Q2= %f, Q3 = %f\r\n", Q1, Q2, Q3);}
}
void r_movebase()
{
setpointreadout(Vz_des); // Start with obtaining the position of the robot and the desired velocities
- if (count == 100) {
- pc.printf("\r\n Vz_des outside setpoint function = %f\r\n", Vz_des);
- }
Jacobian(0, 0, Vz_des); // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
PIDcalculation(Q1, Q2, Q3); // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.
+if (count%100 == 0)[
+ pc.printf("Q1 = %f, Q2= %f, Q3 = %f\r\n", Q1, Q2, Q3);}
}
//--------------------------------
void maintickerfunction()
{
+ duration.reset();
+ duration.start();
count++;
+
+
+ //if (count%2 == 0){ // Sample at 500 Hz
+ processEMG();
+ //}
+
if (go_switch) {
r_processStateSwitch();
}
- //if(1) { // Sample EMG with 500 Hz
- processEMG();
-
- //}
-
if (go_move) {
switch(state) {
case R_HORIZONTAL:
@@ -465,7 +468,8 @@
}
}
-
+ duration.stop();
+ //pc.printf("duration = %f\r\n", duration.read());
}
int main()
@@ -476,34 +480,34 @@
go_move = true;
speed1.period(PwmPeriod);
speed2.period(PwmPeriod);
- state = R_BASE;
+ state = R_VERTICAL; // Initialize the state of the robot
calibrationgo.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
- mainticker.attach(&calibrationEMG, 0.001f); // Attach ticker to the calibration function
+ mainticker.attach(&calibrationEMG, SampleTime); // Attach ticker to the calibration function
wait(5.0f);
- mainticker.attach(&maintickerfunction,0.002f);
+ mainticker.attach(&maintickerfunction,SampleTime);
while(true) {
-
+
ledstatedef = 1;
- if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
+ if(emg1.normalized >= 0.6 && emg2.normalized >= 0.6) {
ledstateswitch = 1;
ledstatedef = 0;
go_switch = true;
go_move = false;
- }
-
+ }
-
- if(count == 100) {
- count = 0;
+ if(count%100 == 0) {
+
+ //pc.printf("MVC1 = %f, MVC2 = %f\r\n", emg1.MVC, emg2.MVC);
//pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
//pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
//pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
}
- wait(0.001f);
+ wait(SampleTime);
+
}
}