Revision 0:cef87d1a41ad, committed 2015-10-15

Comitter:

pwensing

Date:

Thu Oct 15 22:24:26 2015 +0000

Child:

1:fa2766bcfd50

Commit message:

Inital Commit;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BezierCurve.cpp	Thu Oct 15 22:24:26 2015 +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("\tPt. %d:",i);
+        for( int j = 0 ; j < _dim ; j++) {
+            _pts[i][j] = *p;
+            p++;
+            pc.printf("\t\t%f\n",_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	Thu Oct 15 22:24:26 2015 +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/EthernetInterface.lib	Thu Oct 15 22:24:26 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/EthernetInterface/#2fc406e2553f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ExperimentServer.lib	Thu Oct 15 22:24:26 2015 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/pwensing/code/ExperimentServer/#5a1f3abfca66

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI_pmw.lib	Thu Oct 15 22:24:26 2015 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/pwensing/code/QEI_pmw/#d998cfe55eac

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 15 22:24:26 2015 +0000
@@ -0,0 +1,328 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "EthernetInterface.h"
+#include "ExperimentServer.h"
+#include "QEI.h"
+#include "BezierCurve.h"
+
+#define BEZIER_ORDER_CURRENT 12
+#define BEZIER_ORDER_FOOT    7
+
+#define NUM_INPUTS (21 + 2*(BEZIER_ORDER_FOOT+1) + 2*(BEZIER_ORDER_CURRENT+1) )
+#define NUM_OUTPUTS 19
+#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
+
+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
+
+// Variables for q1
+float current1;
+float current_des1;
+float angle1;
+float angle_des1;
+float velocity1;
+float velocity_des1;
+float duty_factor1;
+float angle1_init;
+
+// Variables for q2
+float current2;
+float current_des2;
+float angle2;
+float angle_des2;
+float velocity2;
+float velocity_des2;
+float duty_factor2;
+float angle2_init;
+
+// Fixed kinematic parameters
+const float l_OA=.010; 
+const float l_OB=.040; 
+const float l_AC=.095; 
+const float l_DE=.095;
+              
+// Timing parameters
+float pwm_period_us;
+float current_control_period_us;
+float impedance_control_period_us;
+float start_period, traj_period, end_period;
+
+// Control parameters
+float K_xx;
+float K_yy;
+float K_xy;
+
+float D_xx;
+float D_xy;
+float D_yy;
+
+float current_gain;
+
+float xDesFoot;
+float yDesFoot;
+
+
+// Model parameters
+float R;
+float k_emf;
+float nu1, nu2;
+float supply_voltage;
+float duty_max;
+
+
+DigitalOut motorFwd1(D7);
+DigitalOut motorRev1(D6);  
+AnalogIn currentSense1(A0);
+PwmOut   pwmOut1(D5);
+
+DigitalOut motorFwd2(D13);
+DigitalOut motorRev2(D12);  
+AnalogIn currentSense2(A1);
+PwmOut   pwmOut2(D11);
+
+QEI encoder1(D3,D4 , NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
+QEI encoder2(D9,D10, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
+Ticker currentLoop;
+
+float prev_current_des1 = 0;
+float prev_current_des2 = 0;
+void CurrentLoop()
+{
+    motorFwd1 = current_des1 >= 0;
+    motorRev1 = current_des1 <  0;
+     
+    current1  = currentSense1.read()*3.3f / 0.14f;   //measure current                
+    if (prev_current_des1 < 0)
+        current1*=-1;    
+        
+    duty_factor1=(current_des1*R + current_gain*(current_des1-current1) + k_emf*velocity1)/supply_voltage;   
+    motorRev1 = duty_factor1< 0;
+    motorFwd1 = duty_factor1>=0;
+    float absDuty1 = abs(duty_factor1);
+    if (absDuty1 > duty_max) {
+        duty_factor1 *= duty_max / absDuty1;
+        absDuty1= duty_max;
+    }
+    pwmOut1.write(absDuty1);
+    prev_current_des1 = current_des1;
+    
+    motorFwd2 = current_des2 >= 0;
+    motorRev2 = current_des2 <  0;
+    
+    current2  = currentSense2.read()*3.3f / 0.14f;   //measure current                
+    if (prev_current_des2 < 0)
+        current2*=-1;    
+        
+    duty_factor2=(current_des2*R + current_gain*(current_des2-current2) + k_emf*velocity2)/supply_voltage;
+        
+    motorRev2 = duty_factor2< 0;
+    motorFwd2 = duty_factor2>=0;
+    float absDuty2 = abs(duty_factor2);
+    if (absDuty2 > duty_max) {
+        duty_factor2 *= duty_max / absDuty2;
+        absDuty2= duty_max;
+    }
+    pwmOut2.write(absDuty2);
+    prev_current_des2 = current_des2;
+}
+
+int main (void) {    
+      
+   BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);     // 7th  order cartesian trajectory
+   BezierCurve ff_current1_bez(1,BEZIER_ORDER_CURRENT); // 12th order feedforward
+   BezierCurve ff_current2_bez(1,BEZIER_ORDER_CURRENT);
+      
+  encoder1.reset();
+  encoder2.reset();
+
+  // 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];
+  while(1) {
+    if (server.getParams(input_params,NUM_INPUTS)) {
+      pwm_period_us               = input_params[0]; // PWM_Period in mirco seconds
+      current_control_period_us   = input_params[1]; // Current control period in micro seconds
+      impedance_control_period_us = input_params[2]; // Impedance control period in microseconds seconds
+      start_period                = input_params[3]; // Experiment time in seconds 
+      traj_period                 = input_params[4]; // Experiment time in seconds
+      end_period                  = input_params[5]; 
+      
+
+      R                        = input_params[6]; // Terminal resistance (Ohms)
+      k_emf                    = input_params[7]; // Back EMF Constant (V / (rad/s))
+      nu1                      = input_params[8]; // Friction coefficienct 1 (Nm / (rad/s))
+      nu2                      = input_params[9]; // Friction coefficienct 1 (Nm / (rad/s))
+      supply_voltage           = input_params[10]; // Power Supply Voltage (V)
+
+      angle1_init              = input_params[11]; // Initial angle for q1 (rad)
+      angle2_init              = input_params[12];// Initial angle for q2 (rad)
+
+      current_gain             = input_params[13]; // Proportional current gain (V/A)
+      K_xx                     = input_params[14]; // Foot stiffness N/m
+      K_yy                     = input_params[15]; // Foot stiffness N/m
+      K_xy                     = input_params[16]; // Foot stiffness N/m
+
+      D_xx                     = input_params[17]; // Foot damping N/(m/s)
+      D_yy                     = input_params[18]; // Foot damping N/(m/s)
+      D_xy                     = input_params[19]; // Foot damping N/(m/s)
+      duty_max                 = input_params[20]; // Maximum duty factor
+      
+      float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
+      for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
+        foot_pts[i] = input_params[21+i];    
+      }
+      rDesFoot_bez.setPoints(foot_pts);
+      
+      float ffcurrent1_pts[BEZIER_ORDER_CURRENT +1], ffcurrent2_pts[BEZIER_ORDER_CURRENT+1];
+      for(int i = 0 ; i <= BEZIER_ORDER_CURRENT ; i++) {
+        ffcurrent1_pts[i] = input_params[21+(2*(BEZIER_ORDER_FOOT+1))+i];
+        ffcurrent2_pts[i] = input_params[21+(2*(BEZIER_ORDER_FOOT+1))+(BEZIER_ORDER_CURRENT+1)+i];    
+      }
+      ff_current1_bez.setPoints(ffcurrent1_pts);
+      ff_current2_bez.setPoints(ffcurrent2_pts);
+      
+      
+      pwmOut1.period_us(pwm_period_us);
+      pwmOut2.period_us(pwm_period_us);
+
+      // Attach current loop!
+      currentLoop.attach_us(CurrentLoop,current_control_period_us);
+
+      // Setup experiment
+      t.reset();
+      t.start();
+
+      motorFwd1 = 1;
+      motorRev1 = 0;
+      pwmOut1.write(0);
+
+      motorFwd2 = 1;
+      motorRev2 = 0;
+      pwmOut2.write(0);
+
+      // Run experiment
+      while( t.read() < start_period + traj_period + end_period) { 
+        // Perform control loop logic
+                   
+        angle1 = encoder1.getPulses() *PULSE_TO_RAD + angle1_init;       
+        velocity1 =encoder1.getVelocity() * PULSE_TO_RAD;
+         
+        angle2 = encoder2.getPulses() * PULSE_TO_RAD + angle2_init;       
+        velocity2 = encoder2.getVelocity() * PULSE_TO_RAD;
+
+        // Control code HERE
+        const float th1 = angle1;
+        const float th2 = angle2;
+        const float dth1= velocity1;
+        const float dth2= velocity2;
+
+        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);
+
+        float xLeg = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
+        float yLeg = - l_AC*cos(th1 + th2) - l_DE*cos(th1) - l_OB*cos(th1);
+
+        float dxLeg = Jx_th1 * dth1 + Jx_th2 * dth2;
+        float dyLeg = Jy_th1 * dth1 + Jy_th2 * dth2;
+
+        
+        float teff  = 0;
+        if( t < start_period) {
+            if (K_xx > 0 || K_yy > 0) {
+                K_xx = 180;
+                K_yy = 180;
+                D_xx = 2;
+                D_yy = 2;
+                K_xy = 0;
+                D_xy = 0;
+            }
+            teff = 0;
+        }
+        else if (t < start_period + traj_period)
+        {
+            K_xx                     = input_params[14]; // Foot stiffness N/m
+            K_yy                     = input_params[15]; // Foot stiffness N/m
+            K_xy                     = input_params[16]; // Foot stiffness N/m
+            D_xx                     = input_params[17]; // Foot damping N/(m/s)
+            D_yy                     = input_params[18]; // Foot damping N/(m/s)
+            D_xy                     = input_params[19]; // Foot damping N/(m/s)
+            teff = (t-start_period);
+        }
+        else
+        {
+            teff = traj_period;    
+        }
+        
+        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;
+
+        float e_x = ( xLeg - rDesFoot[0]);
+        float e_y = ( yLeg - rDesFoot[1]);
+        
+        float de_x = ( dxLeg - vDesFoot[0]);
+        float de_y = ( dyLeg - vDesFoot[1]);
+
+        float fx   = -K_xx * e_x - K_xy * e_y - D_xx * de_x -D_xy * de_y;
+        float fy   = -K_yy * e_y - K_xy * e_x - D_yy * de_y -D_xy * de_x;
+                
+        
+                
+                    
+        float feed_forward_1[1],feed_forward_2[1];                     
+        feed_forward_1[0] = 0;
+        feed_forward_2[0] = 0; 
+        if ( (t >= start_period) && (t <= start_period + traj_period)) {
+            ff_current1_bez.evaluate(teff/traj_period,feed_forward_1);
+            ff_current2_bez.evaluate(teff/traj_period,feed_forward_2);
+        } 
+       
+        current_des1 = (nu1*velocity1 + Jx_th1*fx + Jy_th1*fy)/k_emf+feed_forward_1[0];
+        current_des2 = (nu2*velocity2 + Jx_th2*fx + Jy_th2*fy)/k_emf+feed_forward_2[0];               
+                      
+        // Form output to send to MATLAB     
+        float output_data[NUM_OUTPUTS];
+        output_data[0] = t.read();
+        output_data[1] = angle1;
+        output_data[2] = velocity1;  
+        output_data[3] = current1;
+        output_data[4] = current_des1;
+        output_data[5] = duty_factor1;
+          
+        output_data[6] = angle2;
+        output_data[7] = velocity2;
+        output_data[8] = current2;
+        output_data[9] = current_des2;
+        output_data[10]= duty_factor2;
+
+        output_data[11] = xLeg;
+        output_data[12] = yLeg;
+        output_data[13]= dxLeg;
+        output_data[14]= dyLeg;
+        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();
+      // control.detach();
+      currentLoop.detach();
+      pwmOut1.write(0);
+      pwmOut2.write(0);
+
+    } // end if
+  } // end while
+} // end main
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Thu Oct 15 22:24:26 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#d7bd06319118

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Oct 15 22:24:26 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/34e6b704fe68
\ No newline at end of file