Revision 24:2e56d3bebb24, committed 2015-05-06

Comitter:

mzling

Date:

Wed May 06 17:14:00 2015 +0000

Parent:

23:e0923403be2f

Child:

25:1292b886b8d2

Commit message:

Added gaitEquations

Changed in this revision

--- a/FiniteStateMachine/FSM.cpp	Mon Apr 27 21:57:54 2015 +0000
+++ b/FiniteStateMachine/FSM.cpp	Wed May 06 17:14:00 2015 +0000
@@ -10,6 +10,8 @@
 #include "hapticFeedback.h"
 #include "MPU6000.h"
 
+#include "gaitEquations.h"
+
 SPI dummy(p5,p6,p7);
 DigitalOut cs_dummy(p15);
 //DigitalOut test(p7);
@@ -63,31 +65,65 @@
     float gyro = imu.read_rot(1);
 }
 
+/** 
+* Sets the backbias parameter
+* @param f new float value for backbias
+* @author Michael Ling
+* @date 4/30/15
+*/
 void FSM::set_backbias(float f)
 {
     backBias = f;
 }
 
+
+/** 
+* Returns the current value of backbias
+* @author Michael Ling
+* @date 4/30/15
+*/
 float FSM::get_backbias()
 {
     return backBias;
 }
 
+/** 
+* Sets the standup assistance parameter
+* @param f new float value for standup assistance
+* @author Michael Ling
+* @date 4/30/15
+*/
 void FSM::set_standup_asst(float f)
 {
     _standup_asst = f;
 }
 
+/** 
+* Returns the current value of standup assistance
+* @author Michael Ling
+* @date 4/30/15
+*/
 float FSM::get_standup_asst()
 {
     return _standup_asst;
 }
 
+/** 
+* Sets the sitdown assistance parameter
+* @param f new float value for sitdown assistance
+* @author Michael Ling
+* @date 4/30/15
+*/
 void FSM::set_sitdown_asst(float f)
 {
     _sitdown_asst = f;
 }
 
+/** 
+* Returns the current value of sitdown assistance
+* @author Michael Ling
+* @date 4/30/15
+*/
 float FSM::get_sitdown_asst()
 {
     return _sitdown_asst;
@@ -198,9 +234,11 @@
 int FSM::state(int UI)
 {
     //pc.printf("EX: %d, ", exoState);
-    
+     
     pos_L=encoder_L.angle();
     pos_R=encoder_R.angle();
+    pc.printf("L: %6.2f, R: %6.2f\r\n", pos_L, pos_R);
+    //int x= 0;
     torso=imu.angle_y();
     
     //pc.printf("%f, ", torso);

--- a/FiniteStateMachine/FSM.h	Mon Apr 27 21:57:54 2015 +0000
+++ b/FiniteStateMachine/FSM.h	Wed May 06 17:14:00 2015 +0000
@@ -60,7 +60,7 @@
     * error_byte != 0x0 is unsafe.
     */
     short int error_byte;
-    
+
     /** Create instance of safety_flag
     * safety_flag is an analysis of error_byte that checks for safety
     * safety_flag = 0 is safe.

--- a/dataComm.lib	Mon Apr 27 21:57:54 2015 +0000
+++ b/dataComm.lib	Wed May 06 17:14:00 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/mzling/code/dataComm/#86d9e33652b6
+http://developer.mbed.org/users/mzling/code/dataComm/#c95a0006ba68

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gaitEquations/gaitEquations.cpp	Wed May 06 17:14:00 2015 +0000
@@ -0,0 +1,26 @@
+#include <math.h>
+
+void theta_swing(float endTime, float tHipMaxPercent, int startAngle, int endAngle, int maxAmplitude, float* arr)
+{
+    int timeSteps = endTime * 1000;
+    //float theta_swing[timeSteps];
+    float h = endTime * tHipMaxPercent;
+    for (int i = 0; i < timeSteps; i++) {
+        float f = endTime/(timeSteps-1)*i;
+        if (f <= h) {
+           arr[i] = (startAngle-maxAmplitude)/pow(h,2)*pow(f-h,2)+maxAmplitude;
+        } else {
+           arr[i] =  (endAngle-maxAmplitude)/pow(endTime-h,2)*pow(f-h,2)+maxAmplitude;
+        }
+    }
+    //return theta_swing;
+}
+
+void theta_stance(float endTime, float tHipMaxPercent, int startAngle, int endAngle, int maxAmplitude, float* arr)
+{
+    int timeSteps = endTime * 1000;
+    for (int i = 0; i < timeSteps; i++) {
+        arr[i] = ((startAngle-endAngle)/endTime)*endTime/(timeSteps-1)*i+endAngle;
+    }
+}
+        
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gaitEquations/gaitEquations.h	Wed May 06 17:14:00 2015 +0000
@@ -0,0 +1,7 @@
+//#ifndef GAITEQUATIONS_H
+//#define GAITEQUATIONS_H
+//#endif
+
+extern void theta_swing(float endTime, float tHipMaxPercent, int startAngle, int endAngle, int maxAmplitude, float* arr);
+
+extern void theta_stance(float endTime, float tHipMaxPercent, int startAngle, int endAngle, int maxAmplitude, float* arr);

--- a/initExoVars/initExoVars.cpp	Mon Apr 27 21:57:54 2015 +0000
+++ b/initExoVars/initExoVars.cpp	Wed May 06 17:14:00 2015 +0000
@@ -74,8 +74,12 @@
 float stand_adjust= 10; //positive is more upright.Default 10
 //const float zero_ang_L=220+10;//approximate point where the pilot should be standing straight
 //const float zero_ang_R=40+10;//approximate point where the pilot should be standing straight
-const float zero_ang_L=190;//approximate point where the pilot should be standing straight
-const float zero_ang_R=-176;//approximate point where the pilot should be standing straight
+
+/** VERIFY THESE VALUES BEFORE RUNNING CODE ON EXO
+* USING ERRONEOUS OFFSETS MAY CAUSE PERMANENT DAMAGE!!! */
+const float zero_ang_L=223.83;//approximate point where the pilot should be standing straight
+const float zero_ang_R=-176.03;//approximate point where the pilot should be standing straight
+
 const float _enc_high=120; // Hard stop max accepable angle for encoder
 const float _enc_low=-40; // Hard stop min accepable angle for encoder
 

--- a/main.cpp	Mon Apr 27 21:57:54 2015 +0000
+++ b/main.cpp	Wed May 06 17:14:00 2015 +0000
@@ -16,6 +16,8 @@
 #include "dataBedComm.h"
 #include "dataComm.h"
 short dataIn[7];
+/** DataOut: Indices 0,1,2,and 8 are reserved. 0 is start byte, 8 is end byte, 1 and 2 are error codes.
+Other indices can be used for read angles */
 short dataOut[]={0xFF,30,31,0, 0, 0, 0, 0, 0xFE};
 dataComm dc = dataComm();
 Timer dbg;
@@ -25,43 +27,30 @@
 // periodicFcns runs at the start of every control loop cycle
 // It initiates communication with dataBed, checks for errors/safety, and starts the FSM
 
-union float_to_short {
-    float f;
-    uint32_t u;
-} q;
+
+
 void periodicFcns()
 {
     dbg.reset();
-//    dataOut[1]=encoder_L.readRaw();
+    //dataOut[1]=encoder_L.readRaw();
     float f1 = encoder_L.read_angle();
     float f2 = fsm.read_angle_y();
-    //float f2 = -87.43423;
+  
+    /** Multiplying by 91 allows casting float to short without losing too much precision
+    * Assuming angles range from -360 to 360, 91 is the max factor that guarantees we will not overflow
+    */
     short s1 = (short)(f1*91);
     short s2 = (short)(f2*91);
-    dataOut[1] = s1;
-    dataOut[2] = s2;
-    pc.printf("first is %f %d\r\n", f1, s1);
-    pc.printf("second is %f %d\r\n", f2, s2);
-    //dataOut[3] = (short)f1;
-    //dataOut[4] = (short)f2;
-    //dataOut[5] = (short)f2;
-    //pc.printf("%f\r\n", f1);
-    //pc.printf("data1: %d\r\n", dataOut[1]);
-    //pc.printf("%f\r\n", f2);
-    //pc.printf("Size %d\r\n", sizeof dataOut[2]);
-    //pc.printf("data2: %d\r\n", dataOut[2]);
-    //for (int i = 2; i < 4; i++) {
-      //  pc.printf("data%d is %x\r\n", i, dataOut[i]);
-    //}
+    dataOut[3] = s1;
+    dataOut[4] = s2;
+    //pc.printf("1:%f %d\r\n", f1, dataOut[3]);
+    //pc.printf("2:%f %d\r\n", f2, dataOut[4]);
     //dataOut[2]=fsm.error();
     short* ptr=dataIn;
     ptr=sendData(dataOut, 9, dataIn);
-    //for (int i = 0; i < 7; i++) {
-        if (dataIn[1] != 0) {   
-    pc.printf("UI: %d\r\n", dataIn[1]);
+    if (dataIn[1] != 0) {   
+        pc.printf("UI: %d\r\n", dataIn[1]);
     }
-    //}
-    //pc.printf("\r\n");*/
     dc.process_write(dataIn+2, 7);
     //pc.printf("%d, %d, %d, %d,", dataIn[0], dataIn[1], dataIn[2], dataIn[3]);