Erina Yamaguchi
/
jumping_leg_clicky
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: ExperimentServer MotorShield QEI_pmw
Diff: main.cpp
- Revision:
- 27:9f2dad72971f
- Parent:
- 26:0fc6264f5ef5
- Child:
- 28:925794e4178b
diff -r 0fc6264f5ef5 -r 9f2dad72971f main.cpp
--- a/main.cpp Sun Oct 02 22:20:03 2022 +0000
+++ b/main.cpp Fri Nov 18 19:38:21 2022 +0000
@@ -6,18 +6,21 @@
#include "BezierCurve.h"
#include "MotorShield.h"
#include "HardwareSetup.h"
-
-#define BEZIER_ORDER_FOOT 7
-#define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
-#define NUM_OUTPUTS 19
-
+
+//#define BEZIER_ORDER_FOOT 7
+#define NUM_INPUTS 12
+#define NUM_OUTPUTS 6
+
#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
-
+
// Initializations
Serial pc(USBTX, USBRX); // USB Serial Terminal
ExperimentServer server; // Object that lets us communicate with MATLAB
Timer t; // Timer to measure elapsed time of experiment
+Timer hold; // Timer to measure how long to hold clicked position
+DigitalIn clicker(PB_8);
+
QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING); // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING); // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING); // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
@@ -32,31 +35,20 @@
float prev_current_des1 = 0;
float current_int1 = 0;
float angle1;
+float angle1_des;
float velocity1;
float duty_cycle1;
float angle1_init;
-
-// Variables for q2
-float current2;
-float current_des2 = 0;
-float prev_current_des2 = 0;
-float current_int2 = 0;
-float angle2;
-float velocity2;
-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 l_1=.25;
+const float l_OB=.5;
// 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;
-
+
// Control parameters
float current_Kp = 4.0f;
float current_Ki = 0.4f;
@@ -68,17 +60,17 @@
float D_xx;
float D_xy;
float D_yy;
-
+
// Model parameters
float supply_voltage = 12; // motor supply voltage
float R = 2.0f; // motor resistance
float k_t = 0.18f; // motor torque constant
float nu = 0.0005; // motor viscous friction
-
+
// Current control interrupt function
void CurrentLoop()
{
- // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
+ // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
//use the motor shield as follows:
//motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
@@ -103,33 +95,13 @@
}
prev_current_des1 = current_des1;
- current2 = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f); // measure current
- velocity2 = encoderB.getVelocity() * PULSE_TO_RAD; // measure velocity
- float err_c2 = current_des2 - current2; // current error
- current_int2 += err_c2; // integrate error
- current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max); // anti-windup
- float ff2 = R*current_des2 + k_t*velocity2; // feedforward terms
- duty_cycle2 = (ff2 + current_Kp*err_c2 + current_Ki*current_int2)/supply_voltage; // PI current controller
-
- float absDuty2 = abs(duty_cycle2);
- if (absDuty2 > duty_max) {
- duty_cycle2 *= duty_max / absDuty2;
- absDuty2 = duty_max;
- }
- if (duty_cycle2 < 0) { // backwards
- motorShield.motorBWrite(absDuty2, 1);
- } else { // forwards
- 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);
+ // Object for 7th order Cartesian foo//t trajectory
+// BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
// Link the terminal with our server and start it up
server.attachTerminal(pc);
@@ -151,8 +123,8 @@
end_period = 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)
-
+ angle1_des = input_params[4];
+
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
@@ -160,13 +132,15 @@
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
+
+ float th1_des = angle1_init;
// 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[12+i];
- }
- rDesFoot_bez.setPoints(foot_pts);
+// float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
+// for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
+// foot_pts[i] = input_params[12+i];
+// }
+// rDesFoot_bez.setPoints(foot_pts);
// Attach current loop interrupt
currentLoop.attach_us(CurrentLoop,current_control_period_us);
@@ -178,46 +152,40 @@
encoderB.reset();
encoderC.reset();
encoderD.reset();
-
+
motorShield.motorAWrite(0, 0); //turn motor A off
- motorShield.motorBWrite(0, 0); //turn motor B off
// Run experiment
while( t.read() < start_period + traj_period + end_period) {
// Read encoders to get motor states
angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;
- velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
-
- angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;
- velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;
+ velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
const float th1 = angle1;
- const float th2 = angle2;
const float dth1= velocity1;
- const float dth2= velocity2;
-
+
// Calculate the Jacobian
- float Jx_th1 = 0;
- float Jx_th2 = 0;
- float Jy_th1 = 0;
- float Jy_th2 = 0;
+// float Jx_th1 = l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1);
+// float Jx_th2 = l_AC*cos(th1+th2);
+// float Jy_th1 = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
+// float Jy_th2 = l_AC*sin(th1+th2);
// Calculate the forward kinematics (position and velocity)
- float xFoot = 0;
- float yFoot = 0;
- float dxFoot = 0;
- float dyFoot = 0;
+ // float xFoot = l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1);
+// float yFoot = -l_AC*cos(th1+th2)-l_DE*cos(th1)-l_OB*cos(th1);
+// float dxFoot = velocity1*(l_AC*cos(th1+th2)+l_DE*cos(th1)+l_OB*cos(th1))+velocity2*l_AC*cos(th1+th2);
+// float dyFoot = velocity1*(l_AC*sin(th1+th2)+l_DE*sin(th1)+l_OB*sin(th1))+velocity2*l_AC*sin(th1+th2);
// 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) {
if (K_xx > 0 || K_yy > 0) {
- K_xx = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
- K_yy = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
- D_xx = 0.1; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
- D_yy = 0.1; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
+ K_xx = 50; // for joint space control, set this to 1; for Cartesian space control, set this to 50
+ K_yy = 50; // for joint space control, set this to 1; for Cartesian space control, set this to 50
+ D_xx = 2; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
+ D_yy = 2; // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
K_xy = 0;
D_xy = 0;
}
@@ -241,51 +209,49 @@
}
// Get desired foot positions and velocities
- float rDesFoot[2] , vDesFoot[2];
- rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
- rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
- vDesFoot[0]/=traj_period;
- vDesFoot[1]/=traj_period;
- vDesFoot[0]*=vMult;
- vDesFoot[1]*=vMult;
-
- // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
- float xFoot_inv = -rDesFoot[0];
- float yFoot_inv = rDesFoot[1];
- float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
- float alpha = abs(acos( (pow(l_OE,2) - pow(l_AC,2) - pow((l_OB+l_DE),2))/(-2.0f*l_AC*(l_OB+l_DE)) ));
- float th2_des = -(3.14159f - alpha);
- float th1_des = -((3.14159f/2.0f) + atan2(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
+// float rDesFoot[2] , vDesFoot[2];
+// rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
+// rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
+// vDesFoot[0]/=traj_period;
+// vDesFoot[1]/=traj_period;
+// vDesFoot[0]*=vMult;
+// vDesFoot[1]*=vMult;
+//
+ // // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles
+// float xFoot_inv = -rDesFoot[0];
+// float yFoot_inv = rDesFoot[1];
+// float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
+// float alpha = abs(acos( (pow(l_OE,2) - pow(l_AC,2) - pow((l_OB+l_DE),2))/(-2.0f*l_AC*(l_OB+l_DE)) ));
+// float th2_des = -(3.14159f - alpha);
+// float th1_des = -((3.14159f/2.0f) + atan2(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
- float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
- float dth1_des = (1.0f/dd) * ( Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
- float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
+// float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
+// float dth1_des = (1.0f/dd) * ( Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
+// float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
- // Calculate error variables
- float e_x = 0;
- float e_y = 0;
- float de_x = 0;
- float de_y = 0;
-
- // Calculate virtual force on foot
- float fx = 0;
- float fy = 0;
-
- // Set desired currents
- current_des1 = 0;
- current_des2 = 0;
-
+
+// Set desired currents
+// current_des1 = (-K_xx*(angle1)-D_xx*(velocity1))/k_t;
+ while (hold.read() == 0 | hold.read() > 2){
+ if(clicker.read() == 0) {
+ th1_des = angle1_init;
+ }
+ else {
+ th1_des = angle1_des;
+ hold.start();
+ }
+ }
+
// Joint impedance
// sub Kxx for K1, Dxx for D1, Kyy for K2, Dyy for D2
// Note: Be careful with signs now that you have non-zero desired angles!
// Your equations should be of the form i_d = K1*(q1_d - q1) + D1*(dq1_d - dq1)
-// current_des1 = 0;
-// current_des2 = 0;
+ current_des1 = (K_xx*(th1_des-angle1))/k_t;
+// current_des1 = (K_xx*(th1_des-angle1)+D_xx*(dth1_des-velocity1))/k_t;
// Cartesian impedance
- // Note: As with the joint space laws, be careful with signs!
-// current_des1 = 0;
-// current_des2 = 0;
+ // Note: As with the joint space laws, be careful with signs!
+// current_des1 = t1/k_t;
// Form output to send to MATLAB
@@ -298,21 +264,16 @@
output_data[3] = current1;
output_data[4] = current_des1;
output_data[5] = duty_cycle1;
- // motor 2 state
- output_data[6] = angle2;
- output_data[7] = velocity2;
- output_data[8] = current2;
- output_data[9] = current_des2;
- output_data[10]= duty_cycle2;
+
// foot state
- output_data[11] = xFoot;
- output_data[12] = yFoot;
- output_data[13] = dxFoot;
- output_data[14] = dyFoot;
- output_data[15] = rDesFoot[0];
- output_data[16] = rDesFoot[1];
- output_data[17] = vDesFoot[0];
- output_data[18] = vDesFoot[1];
+// output_data[11] = xFoot;
+// output_data[12] = yFoot;
+// output_data[13] = dxFoot;
+// output_data[14] = dyFoot;
+// output_data[15] = rDesFoot[0];
+// output_data[16] = rDesFoot[1];
+// output_data[17] = vDesFoot[0];
+// output_data[18] = vDesFoot[1];
// Send data to MATLAB
server.sendData(output_data,NUM_OUTPUTS);
@@ -324,7 +285,6 @@
server.setExperimentComplete();
currentLoop.detach();
motorShield.motorAWrite(0, 0); //turn motor A off
- motorShield.motorBWrite(0, 0); //turn motor B off
} // end if