Revision 30:1ccef6a5ba50, committed 2015-05-16

Comitter:

perr1940

Date:

Sat May 16 00:45:11 2015 +0000

Parent:

29:fb369b8e2493

Child:

31:692d08537bb4

Commit message:

publishing code for michael. This should fix the adjusting gait params bug.

Changed in this revision

--- a/FiniteStateMachine/FSM.cpp	Thu May 14 22:52:52 2015 +0000
+++ b/FiniteStateMachine/FSM.cpp	Sat May 16 00:45:11 2015 +0000
@@ -44,7 +44,7 @@
 */
 FSM::FSM():exoState(DoubleStance),PH_UI(1), side(1), nextState(0), currentState(0), timeout(2), t_wait(2), count(0), count_max(1000),
     ptrTraj_L(ref_swing_step), ptrTraj_R(ref_stance_step),t_blend(100),pos_L(encoder_L.angle()), pos_R(encoder_R.angle()),backBias(6), kneeThresh(500),music(hip_L,hip_R), prevUI(0), torso(0),
-    _standup_asst(-0.14), _sitdown_asst(-0.025)
+    _standup_asst(-0.14), _sitdown_asst(-0.025), _left(&stance), _right(&swing)
 {
     time.start();
     if(pos_R>180) {
@@ -237,13 +237,14 @@
 
 int FSM::state(int UI)
 {
-    //pc.printf("%d, ", exoState);
+    pc.printf("%d, ", exoState);
 
     pos_L=encoder_L.angle();
     pos_R=encoder_R.angle();
-    //pc.printf("%6.2f, %6.2f, ", pos_L, pos_R);
+    pc.printf("%6.2f, %6.2f, ", pos_L, pos_R);
     //int x= 0;
     torso=imu.angle_y();
+    pc.printf("%d, %f, ", mm_gait_params.time_steps, backBias);
 
     //pc.printf("%f, ", torso);
 
@@ -387,8 +388,8 @@
         case Step:
             if(count>=count_max) {
                 exoState=DoubleStance;
-                //delete[] ptrBlend_L;
-                //delete[] ptrBlend_R;
+                my_delete(ptrBlend_L);
+                my_delete(ptrBlend_R);
                 time.reset();
             } else {
                 step();
@@ -416,8 +417,8 @@
         case Sit2Bend:
             if(count>=count_max) {
                 exoState=BentForward;
-                delete[] ptrBlend_L;
-                delete[] ptrBlend_R;
+                my_delete(ptrBlend_L);
+                my_delete(ptrBlend_R);
             } else {
                 sit2Bend();
             }
@@ -426,8 +427,8 @@
         case Bend2Sit:
             if(count>=count_max) {
                 exoState=Sitting;
-                delete[] ptrBlend_L;
-                delete[] ptrBlend_R;
+                my_delete(ptrBlend_L);
+                my_delete(ptrBlend_R);
             } else {
                 bend2Sit();
             }
@@ -438,8 +439,8 @@
                 exoState=Standing;
                 hip_L.clear();
                 hip_R.clear();
-                delete[] ptrBlend_L;
-                delete[] ptrBlend_R;
+                my_delete(ptrBlend_L);
+                my_delete(ptrBlend_R);
             } else {
                 home2Standing();
             }
@@ -481,14 +482,14 @@
         case 0:
             hip_L.off();
             hip_R.off();
-            //pc.printf("0, 0\r\n");
+            pc.printf("0, 0\r\n");
             //pc.printf("pos_L:%f, pos_R:%f, 0:%f, hip_L:%f, \r\n", pos_L, pos_R, 0, hip_L.read());
             break;
     }
     return exoState;
 }
 
-/*void FSM::stepInit()
+void FSM::stepInit()
 {
     float value;
     count=0;
@@ -550,16 +551,21 @@
     pc.printf("%6.2f, %6.2f\r\n", left_angle, right_angle);
     count++;
 
-}*/
+}
 
-void FSM::stepInit()
+/*void FSM::stepInit()
 {
+    float value;
     count=0;
     count_max=900;
     t_blend=100;
+    swing.restart();
+    stance.restart();
     if(side==0) { //right foot swing
-        _end_left=stance_calculate(t_blend, pos_L, 0, 1);//calculate the point to blend to
-        _end_right=swing_calculate(t_blend, pos_R, 0, 1);//calculate the point to blend to
+        swing.calculate(t_blend+1,value);
+        swing.init(pos_R,value+backBias,t_blend);
+        stance.calculate(t_blend+1,value);
+        stance.init(pos_L,value+backBias,t_blend);
         hip_L.setGains(Kp_Stance,Kd);
         hip_R.setGains(Kp_Swing,Kd);
         cs_dummy=0;
@@ -568,8 +574,10 @@
         knee_R.unlock();
         knee_L.lock();
     } else if(side==1) { //left foot swing
-        _end_left=swing_calculate(t_blend, pos_L, 0, 1);//calculate the point to blend to
-        _end_right=stance_calculate(t_blend, pos_R, 0, 1);//calculate the point to blend to
+        swing.calculate(t_blend+1,value);
+        swing.init(pos_L,value+backBias,t_blend);
+        stance.calculate(t_blend+1,value);
+        stance.init(pos_R,value+backBias,t_blend);
         hip_R.setGains(Kp_Stance,Kd);
         hip_L.setGains(Kp_Swing,Kd);
         cs_dummy=0;
@@ -578,8 +586,6 @@
         knee_R.lock();
         knee_L.unlock();
     }
-    _start_left=pos_L;
-    _start_right=pos_R;
     mbedLED2=0;
     mbedLED1=1;
 }
@@ -588,11 +594,11 @@
 {
     float left_angle, right_angle;
     if(side==0) { //right foot swing
-        right_angle=swing_calculate(count, _start_right, _end_right, 100);
-        left_angle=stance_calculate(count, _start_left, _end_left, 100);
+        stance.increment(left_angle);
+        swing.increment(right_angle);
     } else { //left foot swing
-        right_angle=stance_calculate(count, _start_right, _end_right, 100);
-        left_angle=swing_calculate(count, _start_left, _end_left, 100);
+        swing.increment(left_angle);
+        stance.increment(right_angle);
     }
     hip_L.PD(left_angle, pos_L);
     hip_R.PD(right_angle,pos_R);
@@ -605,20 +611,17 @@
     }
     //pc.printf("%6.2f, %6.2f\r\n", left_angle, right_angle);
     count++;
-}
+}*/
 
 
 void FSM::doubleStance()
 {
-    //hip_L.P(*(ptrTraj_L+count_max-t_blend-1)+backBias,pos_L);
-    //hip_R.P(*(ptrTraj_R+count_max-t_blend-1)+backBias,pos_R);
-    float left_angle=0, right_angle=0;
-    //_left->calculate(count_max-1,left_angle);
-    //_right->calculate(count_max-1,right_angle);
+    float left_angle, right_angle;
+    _left->calculate(count_max-1,left_angle);
+    _right->calculate(count_max-1,right_angle);
     hip_L.P(left_angle+backBias,pos_L);
     hip_R.P(right_angle+backBias,pos_R);
-    //pc.printf("%f, %f, %f, %f\r\n", pos_L, pos_R, *(ptrTraj_L+count_max-t_blend-1)+backBias, hip_L.read());
-    //pc.printf("%6.2f, %6.2f\r\n", left_angle, right_angle);
+    pc.printf("%6.2f, %6.2f\r\n", left_angle, right_angle);
     mbedLED2=1;
     mbedLED1=0;
 
@@ -710,7 +713,7 @@
         knee_R.lock();
         knee_L.lock();
     }
-    //("\r\n");
+    pc.printf("\r\n");
 }
 
 void FSM::sittingDown()
@@ -751,7 +754,7 @@
     hip_L.P(standingAngle,pos_L);
     hip_R.P(standingAngle,pos_R);
     //pc.printf("%f, %f, %f, %f\r\n", pos_L, pos_R, standingAngle, hip_L.read());
-    //pc.printf("\r\n");
+    pc.printf("\r\n");
 }
 
 void FSM::sitting()

--- a/FiniteStateMachine/FSM.h	Thu May 14 22:52:52 2015 +0000
+++ b/FiniteStateMachine/FSM.h	Sat May 16 00:45:11 2015 +0000
@@ -4,6 +4,7 @@
 #include "mbed.h"
 #include "LinearBlend.h"
 #include "hapticFeedback.h"
+#include "MM_gait.h"
 
 #define BentForward 47
 #define Bend2Sit        49 //12/12/12MM
@@ -111,7 +112,8 @@
     float torso;
     float _standup_asst;
     float _sitdown_asst;
-    float _start_left, _start_right, _end_left, _end_right;
+    TrajectoryGenerator* _left;
+    TrajectoryGenerator* _right;
 };
 
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Gaits/Blend_Generator.lib	Sat May 16 00:45:11 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/perr1940/code/Blend_Generator/#20f5b0dc1d8f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Gaits/Gait_Generator.lib	Sat May 16 00:45:11 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/perr1940/code/Gait_Generator/#5cd85206672d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Gaits/MM_gait.lib	Sat May 16 00:45:11 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/perr1940/code/MM_gait/#b3f112cd34bf

--- a/Knee/knee.cpp	Thu May 14 22:52:52 2015 +0000
+++ b/Knee/knee.cpp	Sat May 16 00:45:11 2015 +0000
@@ -66,17 +66,17 @@
     //int knee_error;
     // Assign status to knee_flag for further analysis
     knee_flag = status();
-    
+
     // Mask against minimum safety requirement
     // Safety requirements gathered from L6482 Stepper Spec, page 58
     int knee_test;
     knee_test = (knee_flag & 0x2600);
 
-       // Confirm that safety requirements are met
+    // Confirm that safety requirements are met
     // 0 is safe
     //1 is unsafe
     knee_error = !(knee_test == 0x2600);
-    
+
     return knee_error;
 }
 

--- a/MODSERIAL.lib	Thu May 14 22:52:52 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/AjK/code/MODSERIAL/#ae0408ebdd68

--- a/dataComm.lib	Thu May 14 22:52:52 2015 +0000
+++ b/dataComm.lib	Sat May 16 00:45:11 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/perr1940/code/dataComm_Brad/#78435e587de1
+http://developer.mbed.org/users/perr1940/code/dataComm_Brad/#7486fb079085

--- a/initExoVars/initExoVars.cpp	Thu May 14 22:52:52 2015 +0000
+++ b/initExoVars/initExoVars.cpp	Sat May 16 00:45:11 2015 +0000
@@ -5,10 +5,9 @@
 #include "knee.h"
 #include "FSM.h"
 #include "AvailableMemory.h"
+//#include "MM_gait.h"
 //#include "constants.h"
 
-
-
 /********************************
 *  Initializing various I/O pins!
 ********************************/
@@ -20,8 +19,7 @@
 SPI dataBedSPI(p11, p12, p13); // mosi, miso, sclk
 DigitalOut DB_cs(p14);
 
-//Serial pc(USBTX, USBRX);
-MODSERIAL pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX);
 //StepperDriver Stepper(p18,p5,p6,p7);
 
 // Setup stuff
@@ -87,50 +85,12 @@
 const float _enc_high=120; // Hard stop max accepable angle for encoder
 const float _enc_low=-40; // Hard stop min accepable angle for encoder
 
-float params_time_steps=900;
-float params_peak_time=416;
-float params_walking_angle=10;
-float params_end_angle=-22;
-float params_max_angle=30;
 
-//bool swing_calculate(int time, float &value_, float start, float end, float blend_steps)
-float swing_calculate(int time, float start, float end, float blend_steps)
-{
-    float _slope=(end-start)/blend_steps;
-    float value_;
-    if (time<=blend_steps) {
-        value_=_slope*time+start;
-    } else if (time <= params_peak_time) {
-        value_=(params_end_angle-params_max_angle)*pow(time/params_peak_time-1,2)+params_max_angle;
-    } else {
-        value_=(params_walking_angle-params_max_angle)*pow((time-params_peak_time)/(params_time_steps-params_peak_time),2)+params_max_angle;
-    }
-    return value_;
-    /*if(time>params_time_steps) {
-        return 1;
-    } else {
-        return 0;
-    }*/
-};
-
-//bool stance_calculate(int time, float &value_, float start, float end, float blend_steps)
-float stance_calculate(int time, float start, float end, float blend_steps)
-{
-    float value_;
-    float _slope=(end-start)/blend_steps;
-    if (time<=blend_steps) {
-        value_=_slope*time+start;
-    } else {
-        value_ = (-params_walking_angle+params_end_angle)/(params_time_steps-1)*time+params_walking_angle;
-    }
-    return value_;
-    /*if(time>params_time_steps) {
-        return 1;
-    } else {
-        return 0;
-    }*/
-
-};
+MMgait_t mm_gait_params;
+//MMSwing swing(mm_gait_params);
+//MMStance stance(mm_gait_params);
+MMSwing swing;
+MMStance stance;
 
 void initializeExoIOs()
 {
@@ -163,8 +123,21 @@
 
     dataBedSPI.frequency(10000000);
 
+
+    mm_gait_params.time_steps=900;
+    mm_gait_params.peak_time=416;
+    mm_gait_params.walking_angle=10;
+    mm_gait_params.end_angle=-22;
+    mm_gait_params.max_angle=30;
+
+    swing.set(mm_gait_params);
+    stance.set(mm_gait_params);
+    //pc.printf("%Mem: %d \r\n", AvailableMemory());
+
+
     // print each knee status
 
     pc.printf("knee_L: %x, knee_R:%x\r\n,", knee_L.status(), knee_R.status());
 
+
 }

--- a/initExoVars/initExoVars.h	Thu May 14 22:52:52 2015 +0000
+++ b/initExoVars/initExoVars.h	Sat May 16 00:45:11 2015 +0000
@@ -8,9 +8,10 @@
 #include "HipControl.h"
 #include "knee.h"
 #include "FSM.h"
+#include "MM_gait.h"
 #include "dataComm.h"
-#include "MODSERIAL.h"
 #define PI (3.141592653589793)
+#define my_delete(x) {delete x; x = NULL;}
 
 //#define SAMPLE_TIME .01
 #define SAMPLE_TIME .001
@@ -29,8 +30,7 @@
 extern DigitalOut DB_cs;
 extern SPI IMUspi;
 extern DigitalOut IMUcs;
-//extern Serial pc;
-extern MODSERIAL pc;
+extern Serial pc;
 
 // Setup stuff
 // absolutes
@@ -59,10 +59,11 @@
 
 extern FSM fsm;
 
-//extern bool swing_calculate(int time, float &value_, float start, float end, float blend_steps);
-//extern bool stance_calculate(int time, float &value_, float start, float end, float blend_steps);
-extern float swing_calculate(int time, float start, float end, float blend_steps);
-extern float stance_calculate(int time, float start, float end, float blend_steps);
+extern struct MMgait_t mm_gait_params;
+extern MMSwing swing;
+extern MMStance stance;
+extern GaitGenerator mm_gait;
+
 
 /*******************************
 * END OF I/O Initialization
@@ -106,12 +107,6 @@
 extern const float zero_ang_L;
 extern const float zero_ang_R;
 
-extern float params_time_steps;
-extern float params_peak_time;
-extern float params_walking_angle;
-extern float params_end_angle;
-extern float params_max_angle;
-
 #endif
 
 extern void initializeExoIOs();

--- a/main.cpp	Thu May 14 22:52:52 2015 +0000
+++ b/main.cpp	Sat May 16 00:45:11 2015 +0000
@@ -60,7 +60,7 @@
     
     // Run state change/analysis in FSM
     int exoState=fsm.state(dataIn[1]);
-    float temp=dbg.read_us();
+    //float temp=dbg.read_us();
     //pc.printf("%f\r\n", temp);
     
 }
@@ -71,7 +71,7 @@
     pc.printf("\r\nExoStart \r\n");
     wait(1);
     initializeExoIOs();
-    //pc.printf("Test\r\n"); // keep for debugging compile errors
+    pc.printf("Test\r\n"); // keep for debugging compile errors
     mbedLED1 = 1;
     pc.printf("Starting exo...\n\r");
     //If desired, a startup sound can be played. This function is defined in the DatabedCode, because it will command a sound to be played once it detects a heartbeat from ControlBed