Diff: main.cpp

Revision:

10:5c0c9c647090

Parent:

9:f26a91863e3a

Child:

11:53408545bbb4

diff -r f26a91863e3a -r 5c0c9c647090 main.cpp
--- a/main.cpp	Tue May 12 17:04:31 2015 +0000
+++ b/main.cpp	Tue May 12 20:04:59 2015 +0000
@@ -94,7 +94,7 @@
 #define NUMBER_OF_TISSUES 2
 float g_error_norm[NUMBER_OF_TISSUES];
 bool g_we_are_grasping=false;
-#define CALIBRATION_READS 100.0
+#define CALIBRATION_READS 200.0
 float g_calibration_offset = 8600000.0;
 
 // Values at first touch
@@ -104,23 +104,34 @@
 float g_timeAtFirstTouch;
 
 ///////////Magic numbers courtesy Rod///////////////
-float Phi1[5]={-8.02086003501975e-08,
-                1.55286905458007e-05,
-                0.00795344249784777,
-                8.23733045077119,
-               -0.00299236282304311};
+//float Phi1[5]={-8.02086003501975e-08,
+//                1.55286905458007e-05,
+//                0.00795344249784777,
+//                8.23733045077119,
+//               -0.00299236282304311};
+//float Phi2[5]={-1.24436831310503e-08,
+//                1.23673348605010e-05,
+//                0.00652545188345528,
+//                6.75893262890734,
+//               -0.00228098997419065};
+// Zero-Offset Magic Numbers
+float Phi1[5] = {-1.80212053214826e-08,
+                 1.69579390650964e-06,
+                 0.000592679062823746,
+                 0.624774980172511,
+                 -0.000283294192960159};
 
-float Phi2[5]={-1.24436831310503e-08,
-                1.23673348605010e-05,
-                0.00652545188345528,
-                6.75893262890734,
-               -0.00228098997419065};
-
+float Phi2[5] = {1.00123534796440e-09,
+                 1.42089516619424e-06,
+                 0.000520811899959219,
+                 0.542284752693981,
+                 -0.000248770560431049};
+                 
 // Magic numbers to determine if we are grasping (CHANGE TO  A DYNAMIC APPROACH!!!!)  
 float entry_threshold=8.70e6;
-float velocty_threshold=-0.01;
+float velocity_threshold=-0.01;
 
-float g_thresh_force[NUMBER_OF_TISSUES]={8.80e6,8.75e6};
+float g_thresh_force[NUMBER_OF_TISSUES]={0.16e6,0.12e6};
 ///////////Magic numbers courtesy Rod///////////////
 
 int g_command_corrected;
@@ -128,11 +139,11 @@
 // These are for load cell initialization of the offset
 float g_loadCellZero = 8600000.0;
 bool calibrateDone = false;
-long calibrateTotal = 8600;
+//long calibrateTotal = 8600;
 int calibCntr = 0;
-int g_loadCellOffset = 100000;
-int g_threshOffset1 = 0.2e6;
-int g_threshOffset2 = 0.15e6;
+//int g_loadCellOffset = 100000;
+//int g_threshOffset1 = 0.2e6;
+//int g_threshOffset2 = 0.15e6;
 long long loadCellTotal = 0;
 
 Timer mytimer;
@@ -461,7 +472,7 @@
     //}
 }
 
-// Calibrate Load Cell Function
+// Calibrate Load Cell Function (NOT USING THIS ONE!!!)
 void calibrateLoadCell() {
     long long loadCellTotal = 0;
     int intermediateValue;
@@ -470,12 +481,12 @@
         //pc.printf("Load Cell Read: %i\n",intermediateValue);
         loadCellTotal+= intermediateValue;
         wait_ms(2);
-        pc.printf("We are calibrating load cells...\n");
+        //pc.printf("We are calibrating load cells...\n");
         }
     g_calibration_offset=float(loadCellTotal)/float(CALIBRATION_READS);
     //Update values for thresholding based on calibration
-    g_thresh_force[0]=g_threshOffset1+g_calibration_offset;
-    g_thresh_force[1]=g_threshOffset2+g_calibration_offset;
+    //g_thresh_force[0]=g_threshOffset1+g_calibration_offset;
+    //g_thresh_force[1]=g_threshOffset2+g_calibration_offset;
     calibrateDone = true;
 }
 
@@ -663,37 +674,25 @@
             
             // The 'pulling' of the trigger corresponds to open/closed
             int potread = percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
-        
-            
-            
             
             //angle=left_servo_measured;
+            
+            //if (!calibrateDone){
+            //    wait_ms(2);
+            //}
             int loadcell=adc.read();
-            //pc.printf("Load Cell Read: %d\n",loadcell);
-//            if (!calibrateDone){
-//                calibrateLoadCell();
-//            }
+
             if (!calibrateDone){
                 g_command_corrected = 2800;
                 // Begin Calibration of load cells
-                // Calibrate Load Cells (Does this need to get moved somewhere else?)
-                //calibrateLoadCell();
-                // These are for load cell initialization of the offset
-                //calibrateTotal = (calibrateTotal + loadcell/1000.0)/2.0;
-                
-                //pc.printf("Load Cell Read: %d\n",loadcell);
                 loadCellTotal += (long long) loadcell;
                 calibCntr ++;
                 if (calibCntr == CALIBRATION_READS){
                     g_loadCellZero = float(loadCellTotal)/float(CALIBRATION_READS); 
-                    //Update values for thresholding based on calibration
-                    g_thresh_force[0]=g_threshOffset1+g_loadCellZero;
-                    g_thresh_force[1]=g_threshOffset2+g_loadCellZero;
                     calibrateDone = true;
                 }
             }
 
-
             #ifdef CALIBRATE_TIME_NOW
                 if (calibrateDone){
                     g_command_corrected=potread;
@@ -703,10 +702,14 @@
                     }
                     printf("%6.3f,%0.0f,%2.3f,%2.3f,%d\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell_offset);
             
-                }
+                } 
             #else
+                float loadcell_offset = float(loadcell) - g_loadCellZero;
+                if(loadcell_offset < 0.0){
+                    loadcell_offset = 0.0;
+                }
                 // Code to determine if we should toggle the variable of "g_we_are_grasping"
-                if(!g_we_are_grasping && loadcell>g_loadCellZero+g_forceAboveFirstTouch && angledot<velocty_threshold){
+                if(!g_we_are_grasping && loadcell_offset>g_forceAboveFirstTouch && angledot<velocity_threshold){
                     //Grasp is starting
                     g_we_are_grasping=true;
                     // Set the position and time at first touch
@@ -716,7 +719,7 @@
                         g_error_norm[i]=0.0;
                     }
                 }
-                if(g_we_are_grasping && potread > g_masterPositionFirstTouch && (mytimer.read_us()/1000.0) > (g_timeAtFirstTouch + 200.0)) {//angledot>-velocty_threshold){
+                if(g_we_are_grasping && potread > 2400 && (mytimer.read_us()/1000.0) > (g_timeAtFirstTouch + 200.0)) {//angledot>-velocity_threshold){
                     //Grasp is over
                     g_error_norm[0] = 0;
                     g_error_norm[1] = 0;
@@ -726,8 +729,8 @@
                 std::string sstr;
                 if(g_we_are_grasping){
                     //D_x = [thetadotdot,thetadot,theta,theta^2,theta^3];
-                    g_error_norm[0]+=fabs(float(loadcell)-(Phi1[0]*angledotdot+Phi1[1]*angledot+Phi1[2]*angle+Phi1[3]*angle*angle+Phi1[4]*angle*angle*angle));
-                    g_error_norm[1]+=fabs(float(loadcell)-(Phi2[0]*angledotdot+Phi2[1]*angledot+Phi2[2]*angle+Phi2[3]*angle*angle+Phi2[4]*angle*angle*angle));
+                    g_error_norm[0]+=fabs(float(loadcell_offset)-(Phi1[0]*angledotdot+Phi1[1]*angledot+Phi1[2]*angle+Phi1[3]*angle*angle+Phi1[4]*angle*angle*angle));
+                    g_error_norm[1]+=fabs(float(loadcell_offset )-(Phi2[0]*angledotdot+Phi2[1]*angledot+Phi2[2]*angle+Phi2[3]*angle*angle+Phi2[4]*angle*angle*angle));
                     
                     
                     float offset1 = 100000;
@@ -736,7 +739,7 @@
                     if(g_error_norm[1]>g_error_norm[0]){alpha=(g_error_norm[1]-g_error_norm[0])/(g_error_norm[1]+offset1);sstr="HARD";tissue_id=0;}
                     if(g_error_norm[0]>g_error_norm[1]){alpha=(g_error_norm[0]-g_error_norm[1])/(g_error_norm[0]+offset2);sstr="SOFT";tissue_id=1;}
                     
-                    float force_err=loadcell-g_thresh_force[tissue_id];
+                    float force_err=loadcell_offset-g_thresh_force[tissue_id];
                     float k=20/0.1e6;
                     g_command_corrected=(1-alpha)*potread+alpha*(k*force_err+angle);
                 }else{
@@ -744,15 +747,14 @@
                 }
                 
                 //std::vector<int> data_in=<angle, angledot, angledotdot, loadcell, potread>
-                int loadcell_offset = loadcell - g_loadCellZero;
-                if(loadcell_offset < 0){
-                    loadcell_offset = 0;
-                }
+
                 if (!calibrateDone){
                     g_command_corrected = 2800;
                 }
+                printf("Data:,%6.3f,%6.3f,%2.3f,%s\n",mytimer.read_us()/1000.0,loadcell_offset,alpha,sstr.c_str());
                 
-                printf("Data:,%6.3f,%0.0f,% 2.3f,% 2.3f,%d,%d,%0.0f,%12.3f,%2.3f,%s\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell, potread,g_error_norm[0],g_loadCellZero,alpha,sstr.c_str());
+                //printf("Data:,%6.3f,%0.0f,% 2.3f,% 2.3f,%6.3f,%d,%0.0f,%12.3f,%2.3f,%s\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell_offset, potread,g_error_norm[0],g_loadCellZero,alpha,sstr.c_str());
+                
             #endif
             
             //printf("%s",recent_pos.get_string().c_str());