Diff: main.cpp

Revision:

11:53408545bbb4

Parent:

10:5c0c9c647090

Child:

12:560dc69d4ca4

--- a/main.cpp	Tue May 12 20:04:59 2015 +0000
+++ b/main.cpp	Tue May 12 21:37:21 2015 +0000
@@ -128,10 +128,10 @@
                  -0.000248770560431049};
                  
 // Magic numbers to determine if we are grasping (CHANGE TO  A DYNAMIC APPROACH!!!!)  
-float entry_threshold=8.70e6;
+//float entry_threshold=8.70e6;
 float velocity_threshold=-0.01;
 
-float g_thresh_force[NUMBER_OF_TISSUES]={0.16e6,0.12e6};
+float g_thresh_force[NUMBER_OF_TISSUES]={0.12e6,0.08e6};
 ///////////Magic numbers courtesy Rod///////////////
 
 int g_command_corrected;
@@ -738,9 +738,12 @@
                     int tissue_id=0;
                     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;}
-                    
+                    alpha += 0.35;
+                    if (alpha > 1.0){alpha = 1.0;}
                     float force_err=loadcell_offset-g_thresh_force[tissue_id];
-                    float k=20/0.1e6;
+                    float k = 10.0/0.1e6;
+                    if (tissue_id == 0){k=10.0/0.1e6;}
+                    if (tissue_id == 1){k=100.0/0.1e6;}
                     g_command_corrected=(1-alpha)*potread+alpha*(k*force_err+angle);
                 }else{
                     g_command_corrected=potread;