Diff: main.cpp

Revision:

7:b4b95e5f0fce

Parent:

6:b4347c69816a

Child:

8:cef07a8bdc42

--- a/main.cpp	Tue May 05 18:53:06 2015 +0000
+++ b/main.cpp	Wed May 06 20:21:32 2015 +0000
@@ -93,6 +93,8 @@
 #define NUMBER_OF_TISSUES 2
 float g_error_norm[NUMBER_OF_TISSUES];
 bool g_we_are_grasping=false;
+#define CALIBRATION_READS 5000.0
+float g_calibration_offset;
 
 
 ///////////Magic numbers courtesy Rod///////////////
@@ -438,6 +440,17 @@
     }
 }
 
+// Calibrate Load Cell Function
+void calibrateLoadCell() {
+    int loadCellTotal = 0;
+    for(int ii=0;ii<CALIBRATION_READS;ii++){
+        loadCellTotal=adc.read();
+        pc.printf("Am I crazy?!?!?!?!: %d \n",loadCellTotal);
+        //pc.printf("We are calibrating load cells...\n");
+        }
+    g_calibration_offset=loadCellTotal/CALIBRATION_READS;
+}
+
 
 int main() {
     // Crazy fast baud rate!
@@ -479,10 +492,12 @@
     pc.printf("#################################\n");
     pc.printf("\n\n");
     
+
+    
+    
     #ifdef USE_DYNAMIXELS
         adc.setFilter(256 , false, 1);
         adc.start();
-        
         JoystickLftRt_Zero=0;
         int calib_N=100;
         for(int ii=0;ii<calib_N;ii++){JoystickLftRt_Zero+=AinJoystickLftRt.read();}
@@ -494,6 +509,10 @@
         myServoRight.period_ms(20);
     #endif
     
+    // Begin Calibration of load cells
+    // Calibrate Load Cells (Does this need to get moved somewhere else?)
+    //calibrateLoadCell();
+    
     printf("Setup Complete.\n");
     AuxSerialTimer.start();
     mytimer.start();
@@ -656,7 +675,7 @@
             }
             
             //std::vector<int> data_in=<angle, angledot, angledotdot, loadcell, potread>
-            printf("Data:,%6.3f,%0.0f,% 2.3f,% 2.3f,%d,%d,%0.0f,%0.0f,%0.3f,%s\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell, potread,g_error_norm[1]*angle,g_error_norm[1],alpha,sstr.c_str());
+            printf("Data:,%6.3f,%0.0f,% 2.3f,% 2.3f,%d,%d,%0.0f,%0.0f,%12.3f,%s\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell, potread,g_error_norm[1]*angle,g_error_norm[1],g_calibration_offset,sstr.c_str());
             //printf("%s",recent_pos.get_string().c_str());
 //            printf("<%d,%d,%d,%d,%d,%d,%d>  ",tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos,
 //                                              tempSpindleData.myServoData[LEFT_SERVO_INDEX].force,