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:
- 32:c60a5d33cd79
- Parent:
- 31:4424902a0fd0
- Child:
- 34:9a24d0f718ac
diff -r 4424902a0fd0 -r c60a5d33cd79 main.cpp
--- a/main.cpp Mon Nov 02 19:41:45 2020 +0000
+++ b/main.cpp Mon Nov 02 21:26:44 2020 +0000
@@ -9,8 +9,6 @@
#include "Matrix.h"
#include "MatrixMath.h"
-int doni = 50;
-
#define BEZIER_ORDER_FOOT 7
#define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
#define NUM_OUTPUTS 19
@@ -57,31 +55,28 @@
float duty_cycle2;
float angle2_init;
-// Fixed kinematic parameters
-const float l_OA=.011;
-const float l_OB=.042;
-const float l_AC=.096;
-const float l_DE=.091;
-const float m1 =.0393 + .2;
-const float m2 =.0368;
-const float m3 = .00783;
-const float m4 = .0155;
-const float I1 = 0.0000251; //25.1 * 10^-6;
-const float I2 = 0.0000535; //53.5 * 10^-6;
-const float I3 = 0.00000925; //9.25 * 10^-6;
-const float I4 = 0.0000222; //22.176 * 10^-6;
-const float l_O_m1=0.032;
-const float l_B_m2=0.0344;
-const float l_A_m3=0.0622;
-const float l_C_m4=0.0610;
-const float N = 18.75;
-const float Ir = 0.0035/pow(N,2);
+
// 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;
+// Hardware kinematic parameters -- NEED PAN PARAMETERS
+const float l_c1; //upper arm center of mass
+const float l_B; //upper arm length
+const float r_c2; //lower arm center of mass
+const float l_C; //lower arm length
+
+const float m1; //mass of upper arm
+const float m2; //mass of lower arm
+
+const float I1; //upper arm interia
+const float I2; //lower arm inertia
+
+const float N; //gear ratio
+const float Ir; //motor inertia
+
// Control parameters
float current_Kp = 4.0f;
float current_Ki = 0.4f;
@@ -147,12 +142,10 @@
motorShield.motorBWrite(absDuty2, 0);
}
prev_current_des2 = current_des2;
-
}
int main (void)
{
-
// Object for 7th order Cartesian foot trajectory
BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
@@ -165,26 +158,37 @@
pc.printf("%f",input_params[0]);
while(1) {
-
// If there are new inputs, this code will run
if (server.getParams(input_params,NUM_INPUTS)) {
+ // Get inputs from MATLAB
+ start_period = 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
-
- // Get inputs from MATLAB
- start_period = 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
+ l_c1 = input_params[3]; //upper arm center of mass
+ l_B = input_params[4]; //upper arm length
+ l_c2 = input_params[5]; //lower arm center of mass
+ l_C = input_params[6]; //lower arm length
+
+ m1 = input_params[7]; //mass of upper arm
+ m2 = input_params[8]; //mass of lower arm
+
+ I1 = input_params[9]; //upper arm interia
+ I2 = input_params[10]; //lower arm inertia
+
+ N = input_params[11]; //gear ratio
+ Ir = input_params[12]; //motor inertia
- angle1_init = input_params[3]; // Initial angle for q1 (rad)
- angle2_init = input_params[4]; // Initial angle for q2 (rad)
+ angle1_init = input_params[13]; // Initial angle for q1 (rad)
+ angle2_init = input_params[14]; // Initial angle for q2 (rad)
- 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
+ K_xx = input_params[15]; // Foot stiffness N/m
+ K_yy = input_params[16]; // Foot stiffness N/m
+ K_xy = input_params[17]; // Foot stiffness N/m
+ D_xx = input_params[18]; // Foot damping N/(m/s)
+ D_yy = input_params[19]; // Foot damping N/(m/s)
+ D_xy = input_params[20]; // Foot damping N/(m/s)
+ duty_max = input_params[21]; // Maximum duty factor
// Get foot trajectory points
float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
@@ -251,12 +255,12 @@
}
else if (t < start_period + traj_period)
{
- 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[15]; // Foot stiffness N/m
+ K_yy = input_params[16]; // Foot stiffness N/m
+ K_xy = input_params[17]; // Foot stiffness N/m
+ D_xx = input_params[18]; // Foot damping N/(m/s)
+ D_yy = input_params[19]; // Foot damping N/(m/s)
+ D_xy = input_params[20]; // Foot damping N/(m/s)
teff = (t-start_period);
vMult = 1;
}
@@ -305,16 +309,6 @@
float t2 = Jx_th2*fx + Jy_th2*fy;
// Calculate mass matrix elements
-// float M11 = I1+I2+I3+I4+Ir+Ir*pow(N,2)+pow(l_AC,2)*m4+pow(l_A_m3,2)*m3
-// +pow(l_B_m2,2)*m2+pow(l_C_m4,2)*m4+pow(l_OA,2)*m3+pow(l_OB,2)*m2+pow(l_OA,2)*m4
-// +pow(l_O_m1,2)*m1+2*l_C_m4*l_OA*m4+2*l_AC*l_C_m4*m4*cos(th2)+2*l_AC*l_OA*m4*cos(th2)
-// +2*l_A_m3*l_OA*m3*cos(th2)+2*l_B_m2*l_OB*m2*cos(th2);
-//
-// float M12 = I2+I3+pow(l_AC,2)*m4+pow(l_A_m3,2)*m3+pow(l_B_m2,2)*m2+Ir*N+l_AC*l_C_m4*m4*cos(th2)
-// +l_AC*l_OA*m4*cos(th2)+l_A_m3*l_OA*m3*cos(th2)+l_B_m2*l_OB*m2*cos(th2);
-//
-// float M22 = pow(Ir*N,2)+m4*pow(l_AC,2)+m3*pow(l_A_m3,2)+m2*pow(l_B_m2,2)+I2+I3;
-
float M11 = I1 + I2 + I3 + I4 + Ir + Ir*N*N + l_AC*l_AC*m4 + l_A_m3*l_A_m3*m3 + l_B_m2*l_B_m2*m2 + l_C_m4*l_C_m4*m4 + l_OA*l_OA*m3 + l_OB*l_OB*m2 + l_OA*l_OA*m4 + l_O_m1*l_O_m1*m1 + 2*l_C_m4*l_OA*m4 + 2*l_AC*l_C_m4*m4*cos(th2) + 2*l_AC*l_OA*m4*cos(th2) + 2*l_A_m3*l_OA*m3*cos(th2) + 2*l_B_m2*l_OB*m2*cos(th2);
float M12 = I2 + I3 + l_AC*l_AC*m4 + l_A_m3*l_A_m3*m3 + l_B_m2*l_B_m2*m2 + Ir*N + l_AC*l_C_m4*m4*cos(th2) + l_AC*l_OA*m4*cos(th2) + l_A_m3*l_OA*m3*cos(th2) + l_B_m2*l_OB*m2*cos(th2);
float M22 = Ir*N*N + m4*l_AC*l_AC + m3*l_A_m3*l_A_m3 + m2*l_B_m2*l_B_m2 + I2 + I3;
@@ -340,12 +334,8 @@
float L12 = Lambda.getNumber(1,2);
float L21 = Lambda.getNumber(2,1);
float L22 = Lambda.getNumber(2,2);
-// float L11 = 1;
-// float L12 = 0;
-// float L21 = 0;
-// float L22 = 1;
- // Calculate desired motor torqu
+ // Calculate desired motor torque
float t1_op = (Jx_th1*L11+Jy_th1*L21)*fx + (Jx_th1*L12+Jy_th1*L22)*fy;
float t2_op = (Jx_th2*L11+Jy_th2*L21)*fx + (Jx_th2*L12+Jy_th2*L22)*fy;
@@ -390,10 +380,6 @@
currentLoop.detach();
motorShield.motorAWrite(0, 0); //turn motor A off
motorShield.motorBWrite(0, 0); //turn motor B off
-
} // end if
-
} // end while
-
-} // end main
-
+} // end main
\ No newline at end of file
