Patrick Wensing / Mbed 2 deprecated Bezier_Trajectory_Follower

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:   EthernetInterface ExperimentServer QEI_pmw mbed-rtos mbed

Diff: main.cpp

Revision:
1:fa2766bcfd50
Parent:
0:cef87d1a41ad
diff -r cef87d1a41ad -r fa2766bcfd50 main.cpp
--- 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]);