Revision 1:25284247a74c, committed 2021-11-22

Comitter:

sabazerefa

Date:

Mon Nov 22 07:41:36 2021 +0000

Parent:

0:8d3f9fadbecb

Child:

2:4e581e5b39e8

Commit message:

Implemented two motors, only copying desired. not input interface w matlab

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BezierCurve.cpp	Mon Nov 22 07:41:36 2021 +0000
@@ -0,0 +1,82 @@
+#include "BezierCurve.h"
+#include "math.h"
+#include "mbed.h"
+
+extern Serial pc;
+int factorial(int k) {
+    int f = 1;
+    for (int j = 2 ; j<= k ; j++)
+        f*=j; 
+    return f;   
+}
+
+BezierCurve::BezierCurve(int dim, int order) :_dim(dim), _order(order) {
+    _pts = new float*[_order+1];
+    _nck = new int[_order+1];
+    _nck_deriv = new int[_order];
+    int facn = factorial(_order);
+    int facn2= factorial(_order-1);
+    for(int i = 0 ; i <= _order ; i++) {
+        _pts[i] = new float[_dim];
+        _nck[i] = facn / factorial(i) / factorial(_order-i);
+        if (i< _order) {
+            _nck_deriv[i] = facn2 / factorial(i) / factorial(_order-1-i);
+        }
+    }
+}
+
+BezierCurve::~BezierCurve() {
+    for(int i = 0 ; i < _dim ; i++) {
+        delete _pts[i];    
+    }    
+    delete _pts;
+}
+
+void BezierCurve::setPoints(float pts[] ) {
+    pc.printf("Setting Points\n");
+    float * p = pts;
+    for(int i = 0 ; i<=_order ; i++) {
+        pc.printf("\n\r\tPt. %d:",i);
+        for( int j = 0 ; j < _dim ; j++) {
+            _pts[i][j] = *p;
+            p++;
+            pc.printf("\t\t%f",_pts[i][j]);
+        }    
+    }
+}
+
+void BezierCurve::evaluate(float time, float point[]) {
+    //float *_point = new float[_dim];
+    
+    for(int i=0; i< _dim ; i++) {
+        point[i] = 0;
+    }        
+    for(int i=0; i<=_order ; i++) {
+        float mi = pow(time,i)*pow(1-time,_order-i) * _nck[i];
+        for(int j=0 ; j < _dim ; j++) {
+            point[j] += _pts[i][j] * mi;
+        }    
+    }
+    //for(int i=0; i< _dim ; i++) {
+    //    point[i] = _point[i];
+    //} 
+    //delete _point;
+}
+
+void BezierCurve::evaluateDerivative(float time, float point[]) {
+    //double *_point = new double[_dim];
+    for(int i=0; i< _dim ; i++) {
+        point[i] = 0;
+    }    
+    //double dtime = time;    
+    for(int i=0; i<=_order-1 ; i++) {
+        float mi = pow(time,i)*pow(1-time,_order-1-i) * _nck_deriv[i] * _order;
+        for(int j=0 ; j < _dim ; j++) {
+            point[j] += (_pts[i+1][j] - _pts[i][j] ) * mi;
+        }    
+    }
+    //for(int i=0; i< _dim ; i++) {
+    //    point[i] = _point[i];
+    //} 
+    //delete _point;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BezierCurve.h	Mon Nov 22 07:41:36 2021 +0000
@@ -0,0 +1,15 @@
+class BezierCurve
+{
+public:
+    BezierCurve(int dim, int order);
+    ~BezierCurve();
+    void setPoints(float pts[]); 
+    void evaluate(float time, float point[]);
+    void evaluateDerivative(float time, float point[]);
+private:
+    const int _dim;
+    const int _order;
+    float ** _pts;
+    int * _nck;
+    int * _nck_deriv;
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 22 07:41:36 2021 +0000
@@ -0,0 +1,405 @@
+#include "mbed.h"
+#include <stdio.h>      /* printf */
+#include <math.h>       /* cos */
+#include "rtos.h"
+#include "EthernetInterface.h"
+#include "ExperimentServer.h"
+#include "QEI.h"
+#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 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
+
+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)
+QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
+
+
+MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
+Ticker currentLoop;
+
+// Variables for q1
+float current1;
+float current_des1 = 0;
+float prev_current_des1 = 0;
+float current_int1 = 0;
+float angle1;
+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;
+
+// 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;           
+float current_int_max = 3.0f;       
+float duty_max;      
+float K_xx;
+float K_yy;
+float K_xy;
+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.
+    
+    //use the motor shield as follows:
+    //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
+        
+    current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
+    velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
+    float err_c1 = current_des1 - current1;                                             // current errror
+    current_int1 += err_c1;                                                             // integrate error
+    current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max);      // anti-windup
+    float ff1 = R*current_des1 + k_t*velocity1;                                         // feedforward terms
+    duty_cycle1 = (ff1 + current_Kp*err_c1 + current_Ki*current_int1)/supply_voltage;   // PI current controller
+    
+    float absDuty1 = abs(duty_cycle1);
+    if (absDuty1 > duty_max) {
+        duty_cycle1 *= duty_max / absDuty1;
+        absDuty1 = duty_max;
+    }    
+    if (duty_cycle1 < 0) { // backwards
+        motorShield.motorAWrite(absDuty1, 1);
+    } else { // forwards
+        motorShield.motorAWrite(absDuty1, 0);
+    }             
+    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; 
+    
+    
+    current3 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
+    velocity3 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
+    float err_c3 = current_des3 - current3;                                             // current errror
+    current_int3 += err_c3;                                                             // integrate error
+    current_int3 = fmaxf( fminf(current_int3, current_int_max), -current_int_max);      // anti-windup
+    float ff3 = R*current_des3 + k_t*velocity3;                                         // feedforward terms
+    duty_cycle3 = (ff3 + current_Kp*err_c3 + current_Ki*current_int3)/supply_voltage;   // PI current controller
+    
+    float absDuty3 = abs(duty_cycle3);
+    if (absDuty3 > duty_max) {
+        duty_cycle3 *= duty_max / absDuty3;
+        absDuty3 = duty_max;
+    }    
+    if (duty_cycle3 < 0) { // backwards
+        motorShield.motorCWrite(absDuty3, 1);
+    } else { // forwards
+        motorShield.motorCWrite(absDuty3, 0);
+    }             
+    prev_current_des3 = current_des3; 
+    
+    
+    current4 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current
+    velocity4 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
+    float err_c4 = current_des4 - current4;                                             // current errror
+    current_int4 += err_c4;                                                             // integrate error
+    current_int4 = fmaxf( fminf(current_int4, current_int_max), -current_int_max);      // anti-windup
+    float ff4 = R*current_des4 + k_t*velocity4;                                         // feedforward terms
+    duty_cycle4 = (ff4 + current_Kp*err_c4 + current_Ki*current_int4)/supply_voltage;   // PI current controller
+    
+    float absDuty4 = abs(duty_cycle4);
+    if (absDuty4 > duty_max) {
+        duty_cycle4 *= duty_max / absDuty4;
+        absDuty4 = duty_max;
+    }    
+    if (duty_cycle4 < 0) { // backwards
+        motorShield.motorCWrite(absDuty4, 1);
+    } else { // forwards
+        motorShield.motorCWrite(absDuty4, 0);
+    }             
+    prev_current_des4 = current_des4;     
+    
+    
+}
+
+int main (void)
+{
+    
+    // Object for 7th order Cartesian foot trajectory. 
+    
+    //CREATE A TRAJECTORY 
+    BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
+    
+    // Link the terminal with our server and start it up
+    server.attachTerminal(pc);
+    server.init();
+    
+    // Continually get input from MATLAB and run experiments
+    float input_params[NUM_INPUTS];
+    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
+    
+            angle1_init                 = input_params[3];    // Initial angle for q1 (rad)
+            angle2_init                 = input_params[4];    // 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
+          
+            // 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);
+            
+            // Attach current loop interrupt
+            currentLoop.attach_us(CurrentLoop,current_control_period_us);
+                        
+            // Setup experiment
+            t.reset();
+            t.start();
+            encoderA.reset();
+            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;           
+                
+                const float th1 = angle1;
+                const float th2 = angle2;
+                const float dth1= velocity1;
+                const float dth2= velocity2;
+ 
+                // Calculate the Jacobian
+                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 = 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 = Jx_th1*dth1+Jx_th2*dth2;
+                float dyFoot = Jy_th1*dth1+Jy_th2*dth2;       
+
+                // 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_xy = 0;
+                        D_xy = 0;
+                    }
+                    teff = 0;
+                }
+                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)
+                    teff = (t-start_period);
+                    vMult = 1;
+                }
+                else
+                {
+                    teff = traj_period;
+                    vMult = 0;
+                }
+                
+                // 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 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 = K_xx*(rDesFoot[0]-xFoot) +K_xy*(rDesFoot[1]-yFoot)+D_xx*(vDesFoot[0]-dxFoot)+D_xy*(vDesFoot[1]-dyFoot);
+                float fy = K_xy*(rDesFoot[0]-xFoot) + K_yy*(rDesFoot[1]-yFoot) + D_xy*(vDesFoot[0]-xFoot)+D_yy*(vDesFoot[1]-dyFoot);
+                                
+                // Set desired currents             
+                current_des1 = (Jx_th1*fx+Jy_th1*fy)/k_t;          
+                current_des2 = (Jx_th2*fx+Jy_th2*fy)/k_t;   
+                current_des3 = (Jx_th1*fx+Jy_th1*fy)/k_t;   
+                current_des4 = (Jx_th2*fx+Jy_th2*fy)/k_t;          
+       
+
+        
+                // 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)
+                
+                //PART 0A FIRST
+//                float q1_d=0;
+//                float dq1_d=0;
+//                current_des1 = (K_xx*(q1_d-th1) + D_xx*(dq1_d-dth1))/k_t;    
+//                current_des2 = 0;      
+
+//              PART 2   
+//                float q1_d=th1_des;
+//                float dq1_d=dth1_des;                
+//                float q2_d=th2_des;
+//                float dq2_d=dth2_des;
+//                current_des1 = (K_xx*(q1_d-th1) + D_xx*(dq1_d-dth1))/k_t;
+//                current_des2 = (K_yy*(q2_d-th2) + D_yy*(dq2_d-dth2))/k_t;    
+
+
+
+    /*  PART 3!!!!!!!!!!!!!!!!*/     
+                
+                // Cartesian impedance  
+                // Note: As with the joint space laws, be careful with signs!              
+//                current_des1 = 0;          
+//                current_des2 = 0;   
+                
+                
+                // Form output to send to MATLAB     
+                float output_data[NUM_OUTPUTS];
+                // current time
+                output_data[0] = t.read();
+                // motor 1 state
+                output_data[1] = angle1;
+                output_data[2] = velocity1;  
+                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];
+                
+                // Send data to MATLAB
+                server.sendData(output_data,NUM_OUTPUTS);
+
+                wait_us(impedance_control_period_us);   
+            }
+            
+            // Cleanup after experiment
+            server.setExperimentComplete();
+            currentLoop.detach();
+            motorShield.motorAWrite(0, 0); //turn motor A off
+            motorShield.motorBWrite(0, 0); //turn motor B off
+            motorShield.motorCWrite(0,0);
+            motorShield.motorDWrite(0,0);
+        
+        } // end if
+        
+    } // end while
+    
+} // end main
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Mon Nov 22 07:41:36 2021 +0000
@@ -0,0 +1,1 @@
+https://github.com/armmbed/mbed-os/#b6370b4c37f3d4665ed1cdcb1afea85396bba1b3