Revision 27:9f2dad72971f, committed 2022-11-18

Comitter:

erinay

Date:

Fri Nov 18 19:38:21 2022 +0000

Parent:

26:0fc6264f5ef5

Child:

28:925794e4178b

Commit message:

Initial tests to move foot position after force applied to clicker

Changed in this revision

--- 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