Sebastian Uribe
/
pan_flipping
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: MatrixMath Matrix ExperimentServer QEI_pmw MotorShield
Diff: main.cpp
- Revision:
- 39:c8da22fcf4f3
- Parent:
- 38:8ce2f6edba26
diff -r 8ce2f6edba26 -r c8da22fcf4f3 main.cpp
--- a/main.cpp Tue Nov 17 09:11:44 2020 +0000
+++ b/main.cpp Tue Dec 01 04:46:52 2020 +0000
@@ -8,10 +8,10 @@
#include "HardwareSetup.h"
#define BEZIER_ORDER_FOOT 7
-#define BEZIER_ORDER_TORQUE 3 /// if x -> plots x+1 points, so should have x+1 Bezier points for T1 and T2 in MATLAB
+#define BEZIER_ORDER_TORQUE 2 /// if x -> plots x+1 points, so should have x+1 Bezier points for T1 and T2 in MATLAB
//#define NUM_INPUTS (14 + 2*(BEZIER_ORDER_FOOT+1))
-#define NUM_INPUTS (14 + 2*(BEZIER_ORDER_TORQUE+1))
-#define NUM_OUTPUTS 19
+#define NUM_INPUTS (16 + 2*(BEZIER_ORDER_TORQUE+1))
+#define NUM_OUTPUTS 21
#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
@@ -39,6 +39,7 @@
float velocity_des1;
float duty_cycle1;
float angle1_init;
+float shoulder;
// Variables for q2
float current2;
@@ -51,20 +52,15 @@
float velocity_des2;
float duty_cycle2;
float angle2_init;
+float elbow;
-// Fixed kinematic parameters
-//const float l_OA=.011;
-//const float l_OB=.042;
-//const float l_AC=.096;
-//const float l_DE=.091;
-
-float l1; //=0.084125;
-float l2; //=0.084125;
+float l1;
+float l2;
// Timing parameters
float current_control_period_us = 200.0f; // 5kHz current control loop
float impedance_control_period_us = 1000.0f; // 1kHz impedance control loop
-float start_period, traj_period, end_period;
+float pre_buffer_time, traj_period, post_buffer_time;
// Control parameters
float current_Kp = 4.0f;
@@ -156,34 +152,36 @@
// Get inputs from MATLAB
- start_period = input_params[0]; // First buffer time, before trajectory
+ pre_buffer_time = input_params[0]; // First buffer time, before trajectory
traj_period = input_params[1]; // Trajectory time/length
- end_period = input_params[2]; // Second buffer time, after trajectory
+ post_buffer_time = input_params[2]; // Second buffer time, after trajectory
angle1_init = input_params[3]; // Initial angle for q1 (rad)
angle2_init = input_params[4]; // Initial angle for q2 (rad)
+ shoulder = input_params[5];
+ elbow = input_params[6];
- K_xx = input_params[5]; // Foot stiffness N/m
- K_yy = input_params[6]; // Foot stiffness N/m
- K_xy = input_params[7]; // Foot stiffness N/m
- D_xx = input_params[8]; // Foot damping N/(m/s)
- D_yy = input_params[9]; // Foot damping N/(m/s)
- D_xy = input_params[10]; // Foot damping N/(m/s)
- duty_max = input_params[11]; // Maximum duty factor
- l1 = input_params[12]; // Length of 1st link
- l2 = input_params[13]; // Length of 2nd link
+ K_xx = input_params[7]; // Foot stiffness N/m
+ K_yy = input_params[8]; // Foot stiffness N/m
+ K_xy = input_params[9]; // Foot stiffness N/m
+ D_xx = input_params[10]; // Foot damping N/(m/s)
+ D_yy = input_params[11]; // Foot damping N/(m/s)
+ D_xy = input_params[12]; // Foot damping N/(m/s)
+ duty_max = input_params[13]; // Maximum duty factor
+ l1 = input_params[14]; // Length of 1st link
+ l2 = input_params[15]; // Length of 2nd link
// Get foot trajectory points
float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
- foot_pts[i] = input_params[14+i]; // Bezier curve points
+ foot_pts[i] = input_params[16+i]; // Bezier curve points
}
rDesFoot_bez.setPoints(foot_pts);
// Get torque traj points
float torque_pts[2*(BEZIER_ORDER_FOOT+1)];
for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
- torque_pts[i] = input_params[14+i];
+ torque_pts[i] = input_params[16+i];
}
torque_bez.setPoints(torque_pts);
float torque_des[2];
@@ -203,7 +201,7 @@
motorShield.motorBWrite(0, 0); //turn motor B off
// Run experiment
- while( t.read() < start_period + traj_period + end_period) {
+ while( t.read() < pre_buffer_time + traj_period + post_buffer_time) {
// Read encoders to get motor states
angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;
@@ -224,11 +222,7 @@
float Jy_th2 = l2*sin(th1+th2);
- // Calculate the forward kinematics (position and velocity)
-// float xFoot = l_DE*sin(th1)+l_OB*sin(th1)+l_AC*sin(th1+th2);
-// float yFoot = -l_DE*cos(th1)-l_OB*cos(th1)-l_AC*cos(th1+th2);
-// float dxFoot = dth1*(l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1))+dth2*l_AC*cos(th1+th2);
-// float dyFoot = dth1*(l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1))+dth2*l_AC*sin(th1+th2);
+ // Calculate the forward kinematics (position and velocity)
float xFoot = l2*sin(th1+th2) + l1*sin(th1);
float yFoot = -l2*cos(th1+th2) - l1*cos(th1);
float dxFoot = l1*cos(th1)*dth1 + l2*cos(th1+th2)*(dth1+dth2);
@@ -237,11 +231,11 @@
// Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
float teff = 0;
float vMult = 0;
- if( t < start_period) { // impedance operational space to start point
+ if( t < pre_buffer_time) { // impedance operational space to start point
teff = 0.f;
- float th1_d= 0.f;
- float th2_d= -3.1415f/2.f;
+ float th1_d= shoulder;
+ float th2_d= -elbow;
float e_th1= th1_d-th1;
float e_th2= th2_d-th2;
@@ -254,16 +248,16 @@
current_des1 = torque_des[0]/k_t;
current_des2 = torque_des[1]/k_t;
}
- else if (t < start_period + traj_period) // torque control to follow Bezier
+ else if (t < pre_buffer_time + traj_period) // torque control to follow Bezier
{
- K_xx = input_params[5]; // Foot stiffness N/m
- K_yy = input_params[6]; // Foot stiffness N/m
- K_xy = input_params[7]; // Foot stiffness N/m
- D_xx = input_params[8]; // Foot damping N/(m/s)
- D_yy = input_params[9]; // Foot damping N/(m/s)
- D_xy = input_params[10]; // Foot damping N/(m/s)
+ K_xx = input_params[7]; // Foot stiffness N/m
+ K_yy = input_params[8]; // Foot stiffness N/m
+ K_xy = input_params[9]; // Foot stiffness N/m
+ D_xx = input_params[10]; // Foot damping N/(m/s)
+ D_yy = input_params[11]; // Foot damping N/(m/s)
+ D_xy = input_params[12]; // Foot damping N/(m/s)
- teff = (t-start_period);
+ teff = (t-pre_buffer_time);
vMult = 1;
torque_bez.evaluate(teff/traj_period,torque_des);
current_des1 = torque_des[0]/k_t;
@@ -272,48 +266,20 @@
else // impedance operational space to end point
{
teff = 0.f;
- float th1_d= 0.f;
- float th2_d= -3.1415f/2.f;
+ float th1_d= shoulder;
+ float th2_d= -elbow;
float e_th1= th1_d-th1;
float e_th2= th2_d-th2;
float e_dth1= -dth1;
float e_dth2= -dth2;
- torque_des[0] = K_xx*e_th1 + D_xx*e_dth1;
- torque_des[1] = K_yy*e_th2 + D_yy*e_dth2;
+ torque_des[0] = (K_xx*e_th1 + D_xx*e_dth1)/2;
+ torque_des[1] = (K_yy*e_th2 + D_yy*e_dth2)/2;
current_des1 = torque_des[0]/k_t;
current_des2 = torque_des[1]/k_t;
-// teff = 0;
-// float rDesFoot[2] , vDesFoot[2];
-// rDesFoot[0] = l2;
-// rDesFoot[1] = -l1;
-// vDesFoot[0] = 0;
-// vDesFoot[1] =0;
-//
-// // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
-// float xFootd = -rDesFoot[0];
-// float yFootd = rDesFoot[1];
-//
-// // Calculate error variables
-// float e_x = xFootd-xFoot;
-// float e_y = yFootd-yFoot;
-// float de_x = vDesFoot[0]-dxFoot;
-// float de_y = vDesFoot[1]-dyFoot;
-//
-// // Calculate virtual force on foot
-// float fx = K_xx*e_x + K_xy*e_y + D_xx*de_x + D_xy*de_y;
-// float fy = K_xy*e_x + K_yy*e_y + D_xy*de_x + D_yy*de_y;
-//
-// float T1 = Jx_th1*fx+Jy_th1*fy;
-// float T2 = Jx_th2*fx+Jy_th2*fy;
-//
-// // Cartesian impedance
-// // Note: As with the joint space laws, be careful with signs!
-// current_des1 = T1/k_t;
-// current_des2 = T2/k_t;
}
// Form output to send to MATLAB
@@ -341,10 +307,6 @@
output_data[16] = current2*k_t;
output_data[17] = torque_des[0];
output_data[18] = torque_des[1];
-// output_data[15] = rDesFoot[0];
-// output_data[16] = rDesFoot[1];
-// output_data[17] = 0;//vDesFoot[0];
-// output_data[18] = 0;//vDesFoot[1];
// Send data to MATLAB
server.sendData(output_data,NUM_OUTPUTS);
