Diff: initExoVars.cpp

Revision:

2:650a015179b9

Parent:

0:cd5ad8e47eb8

Child:

3:e910d4eb3a36

--- a/initExoVars.cpp	Wed Jun 24 19:24:08 2015 +0000
+++ b/initExoVars.cpp	Thu Jun 25 22:56:53 2015 +0000
@@ -90,30 +90,35 @@
 
 //PARAMETER ADJUSTMENT - Standing Angle
 //Steve = 10
-float stand_adjust=10; //positive is more upright//Steve=10 deg
+float stand_adjust=0; //positive is more upright//Steve=10 deg
 //Person 1
 //const float stand_adjust=10;
 //Person 2
 //const float stand_adjust=10;
- 
+
 //zero_enc_offset_X is the offset for a particular encoder. Must be tuned for an individual encoder based on assembly.
 // Offsets for Exo #1. These are related to individual encoder and should never be changed.
 //const float zero_enc_offset_L = 223.83;
 // zero_enc_offset_R should be negative
 //const float zero_enc_offset_R = -176.4;
 // Offsets for Exo #2
-//const float zero_enc_offset_L = 168.24;
+float zero_enc_offset_L = 168.24;
 // zero_enc_offset_R should be negative
-//const float zero_enc_offset_R = -128.12;
+float zero_enc_offset_R = -128.12;
+
+
 //Offset for Test Rig
-const float zero_enc_offset_L = 205.24;
+//const float zero_enc_offset_L = 205.24;
 // zero_enc_offset_R should be negative
-const float zero_enc_offset_R = -135.17;
- 
+//const float zero_enc_offset_R = -135.17;
+
+
 //zero_ang_X is the zero angle based on the mechanical assembly of the hip and the standing angle parameters of the individual user.
-const float zero_ang_L=zero_enc_offset_L+stand_adjust;
-const float zero_ang_R=zero_enc_offset_R+stand_adjust;
- 
+//const float zero_ang_L=zero_enc_offset_L+stand_adjust;
+//const float zero_ang_R=zero_enc_offset_R+stand_adjust;
+float zero_ang_L;
+float zero_ang_R;
+
 // Upper and lower limit on Encoder
 // TODO(Cashdollar): These may need to be tuned. Window is 160 deg but might not start at 120.
 const float _enc_high=122+stand_adjust; // Hard stop max accepable angle for encoder
@@ -125,13 +130,38 @@
 //MMStance stance(mm_gait_params);
 MMSwing swing;
 MMStance stance;
+MMFSSwing fsSwing;
+MMFSStance fsStance;
+MMFTG ftgSwing;
+MMFTG ftgStance;
+
+void readCalib()
+{
+    char A[10];
+    LocalFileSystem local("local");               // Create the local filesystem under the name "local"
+    FILE *set = fopen("/local/calib.txt", "r");  // Open "setup.txt" on the local file system for read
+    fscanf(set,"%s %f",A,&zero_enc_offset_L); // read offset
+    fscanf(set,"%s %f",A,&zero_enc_offset_R); // read offset
+    fclose(set);
+    wait(1); //TODO(Cashdollar): reduce this time after testing
+    pc.printf("Left Offset is: %f, Right Offset is: %f\r\n", zero_enc_offset_L, zero_enc_offset_R);//, zero_enc_offset_R);
+    wait(5); //TODO(Cashdollar): Delete after testing, this is to make it easier to see what is happening
+    //const float zero_ang_L=zero_enc_offset_L+stand_adjust;
+    //const float zero_ang_R=zero_enc_offset_R+stand_adjust;
+}
 
 void initializeExoIOs()
 {
+
     // Putting various comm pins high
     DB_cs = 1; // Setting SPI CS pin high (so low selects the slave)
-    pc.baud(921600); //pc.baud(115200);//460800
-    //pc.baud(230400);
+
+    pc.baud(921600);
+    pc.printf("starting to read");
+    readCalib();
+    pc.printf("done reading");
+    zero_ang_L=zero_enc_offset_L+stand_adjust;
+    zero_ang_R=zero_enc_offset_R+stand_adjust;
 
     // initialize both knees
     knee_L.init();
@@ -175,3 +205,4 @@
 
 
 }
+