Patrick Wensing / Mbed 2 deprecated Bezier_Trajectory_Follower

Bezier trajectory tracking controller for iterative feedforward learning

Dependencies:   EthernetInterface ExperimentServer QEI_pmw mbed-rtos mbed

Files at this revision

API Documentation at this revision

Revision 1:fa2766bcfd50, committed 2015-10-19

Comitter:
pwensing
Date:
Mon Oct 19 17:23:10 2015 +0000
Parent:
0:cef87d1a41ad
Commit message:
Reduced bezier order

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Thu Oct 15 22:24:26 2015 +0000
+++ b/main.cpp	Mon Oct 19 17:23:10 2015 +0000
@@ -6,7 +6,7 @@
 #include "BezierCurve.h"
 
 #define BEZIER_ORDER_CURRENT 12
-#define BEZIER_ORDER_FOOT    7
+#define BEZIER_ORDER_FOOT    3
 
 #define NUM_INPUTS (21 + 2*(BEZIER_ORDER_FOOT+1) + 2*(BEZIER_ORDER_CURRENT+1) )
 #define NUM_OUTPUTS 19
@@ -233,6 +233,7 @@
 
         
         float teff  = 0;
+        float vMult = 0;
         if( t < start_period) {
             if (K_xx > 0 || K_yy > 0) {
                 K_xx = 180;
@@ -253,6 +254,7 @@
             D_yy                     = input_params[18]; // Foot damping N/(m/s)
             D_xy                     = input_params[19]; // Foot damping N/(m/s)
             teff = (t-start_period);
+            vMult = 1;
         }
         else
         {
@@ -264,6 +266,8 @@
         rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
         vDesFoot[0]/=traj_period;
         vDesFoot[1]/=traj_period;
+        vDesFoot[0]*=vMult;
+        vDesFoot[1]*=vMult;
 
         float e_x = ( xLeg - rDesFoot[0]);
         float e_y = ( yLeg - rDesFoot[1]);