Diff: main.cpp

Revision:

5:72e92c721cd5

Parent:

4:e44ac08027bd

Child:

6:b4347c69816a

--- a/main.cpp	Mon Apr 06 21:23:36 2015 +0000
+++ b/main.cpp	Wed Apr 29 21:52:37 2015 +0000
@@ -1,6 +1,7 @@
 #define USE_DYNAMIXELS
 //#define USE_BLUETOOTH
 #define USE_SD_CARD
+#define ROD_IS_RIGHT
 
 // We have different modes for different things
 #define MODE_MANUAL     1
@@ -19,11 +20,11 @@
 #include "ServoRingBuffer.h"
 #include "ram_test.h"
 #include "Serial_Receive.h"
+#include "data_set.h"
 #include <string>
 
 // Specific to Dynamixels
 #ifdef USE_DYNAMIXELS
-#include "MX12.h"
 #include "AD7730.h"
 #endif
 
@@ -54,15 +55,16 @@
 DigitalOut led1(LED1);      //Led 1 for debugging purposes
 DigitalOut led2(LED2);      //Led 2 for debugging purposes
 DigitalOut led3(LED3);      //Led 3 for debugging purposes
-//DigitalOut led4(LED4);      //Led 4 for debugging purposes
+DigitalOut led4(LED4);      //Led 4 for debugging purposes
 DigitalOut triggerOut(p11);
 Serial pc(USBTX, USBRX);    //Set up serial connection to pc
 #ifdef USE_BLUETOOTH
 Serial bt(p13,p14);         //Set up serial connection to bluetooth adapter
 #endif
 
-AnalogIn AinLeftForce(p16);          //Set up potentiometer on pin 20
-AnalogIn AinRightForce(p15);          //Set up potentiometer on pin 20
+
+AnalogIn AinLeftForce(p16);     //Set up potentiometer on pin 20
+AnalogIn AinRightForce(p15);    //Set up potentiometer on pin 20
 
 #ifdef USE_SD_CARD
     // Attach SD card
@@ -76,12 +78,6 @@
 float max_percent_full=0;
 
 // Define variables for the program
-float servoAngle;                   //This is the desired servo angle based on the scaled potentiometer value
-float potData;                      //This is the value of the potentiometer from Ain.read()
-bool collect_data = false;          //This is 
-
-bool keyStrokeFlag = false;         //This is a flag to see if a keystroke has been pressed
-char keyStrokeVal;                  //This is a character storing the value of the keystroke
 
 char g_tissue_type_name[32];
 float g_frequency;
@@ -93,10 +89,54 @@
 unsigned char g_current_mode=MODE_NULL;
 jaw_state g_current_direction=STATE_OPEN_HOLD;
 unsigned char g_current_cycle=0;
+int g_input_pot_1;
+#define NUMBER_OF_TISSUES 2
+float g_error_norm[NUMBER_OF_TISSUES];
+bool g_we_are_grasping=false;
+
+
+///////////Magic numbers courtesy Rod///////////////
+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};
+
+float entry_threshold=8.70e6;
+float velocty_threshold=-0.01;
+
+float g_thresh_force[NUMBER_OF_TISSUES]={8.80e6,8.75e6};
+///////////Magic numbers courtesy Rod///////////////
+
+int g_command_corrected;
+
+Timer mytimer;
 
 // Warning, this buffer is large!
 ServoRingBuffer Buffer;
 spindleData tempSpindleData;  //For sending to the buffer
+#ifdef ROD_IS_RIGHT
+    int angle_sum=0;
+    int angle_count=0;
+    int angledot_sum=0;
+    int angledot_count=0;
+    int last_angle=0;
+    int angledotdot_sum=0;
+    int angledotdot_count=0;
+    int last_angledot=0;
+#else
+    data_set recent_pos(30,"Data","Time (us)","Position (encoder counts)");
+    float coeffs[4];
+    bool crunching_data_flag;
+#endif
+
+int bits_received;
 
 Timer ISRDurationTimer;
 Timer AuxSerialTimer;
@@ -104,25 +144,40 @@
 int current_latency;
 
 #ifdef USE_DYNAMIXELS
-    //Dynamixels can only handle 500Hz for now.  Working on it...
-    float samplingPeriod = 0.005;       //This is the sampling period for the timer interrupt
+    Serial cm(p28,p27);         //Set up serial connection to OpenCM 9.04
+    unsigned short left_servo_measured=0;
+    unsigned short right_servo_measured=0;
+    char input_buffer[16];
+    int  input_buffer_location=0;
+
+    float samplingPeriod = 0.001;       //This is the sampling period for the timer interrupt
     #define LEFT_JAW_DYNAMIXEL_ID        3
     #define RIGHT_JAW_DYNAMIXEL_ID       4
-    #define CLOSED_SERVO_ANGLE_LEFT   1121      //This is the closed in encoder counts
-    #define OPEN_SERVO_ANGLE_LEFT     2783      //This is the open in encoder counts
-    #define CLOSED_SERVO_ANGLE_RIGHT  3259      //This is the closed in encoder counts
-    #define OPEN_SERVO_ANGLE_RIGHT    1486      //This is the open in encoder counts
-    // Dynamixel Object
-    MX12 mx12_left_jaw  (p28, p27, p30, p29, LEFT_JAW_DYNAMIXEL_ID,  1000000);
-    MX12 mx12_right_jaw (p28, p27, p30, p29, RIGHT_JAW_DYNAMIXEL_ID, 1000000);
+//    #define CLOSED_SERVO_ANGLE_LEFT   1001      //This is the closed in encoder counts
+//    #define OPEN_SERVO_ANGLE_LEFT     2663      //This is the open in encoder counts
+//    #define CLOSED_SERVO_ANGLE_RIGHT  3259      //This is the closed in encoder counts
+//    #define OPEN_SERVO_ANGLE_RIGHT    1486      //This is the open in encoder counts
+    #define CLOSED_SERVO_ANGLE_LEFT   1975      //This is the closed in encoder counts
+    #define OPEN_SERVO_ANGLE_LEFT     2560      //This is the open in encoder counts
+    #define CLOSED_SERVO_ANGLE_RIGHT  1975      //This is the closed in encoder counts
+    #define OPEN_SERVO_ANGLE_RIGHT    2560      //This is the open in encoder counts
+    //AD7730( mosi, miso, sclk, ready, cs)
+    AD7730 adc(p11, p12, p13, p14, p15);
+    //AD7730 adc2(p11, p12, p13, p18, p19);
     
-    AD7730 adc(p9, p26, p11, p12, p25);
+    
+    AnalogIn AinJoystickFwdBk(p17);          //Set up potentiometer on pin 16
+    AnalogIn AinJoystickLftRt(p16);          //Set up potentiometer on pin 17
+    float JoystickFwdBk_Zero=0.5;
+    float JoystickLftRt_Zero=0.5;
     
     /// Set these to inputs so that they don't interfere with the serial communication
     DigitalIn nullOut1(p21);
     DigitalIn nullOut2(p22);
     DigitalIn nullOut3(p23);
     DigitalIn nullOut4(p24);
+    /// This one is in the way of the SMD pads
+    DigitalIn nullOut5(p20);
 #else
     float samplingPeriod = 0.001;       //This is the sampling period for the timer interrupt
     #define SERVO_DEGREE_0    900       //This is the pulse width value for HiTEC-422 in microseconds to turn 0 degrees
@@ -151,6 +206,41 @@
 
 #endif
 
+void serialInterrupt(char buffer){
+  input_buffer[input_buffer_location]=buffer;
+  input_buffer_location++;
+  bits_received+=8;
+  //pc.printf("RC:%d\n",buffer);
+  
+  
+  //Is the packet looking good so far??
+  if(input_buffer[0]==0xFF){
+    //Is the packet looking good so far??????
+    if(input_buffer[1]==0xFF || input_buffer_location ==1){
+      //Do we have a complete packet??
+      if(input_buffer_location>=6){ //This is 6 because we already incremented
+        //We do!  Extract the jucy datas
+        left_servo_measured = ( input_buffer[2] << 8 ) | input_buffer[3];
+        right_servo_measured = ( input_buffer[4] << 8 ) | input_buffer[5];
+        //pc.printf("RP:%d,%d\n",left_servo_measured,right_servo_measured);
+        //Reset the buffer location so we can start over
+        input_buffer_location=0;
+      }
+    }else{
+      //Something is wrong.  We may just not be at the correct location in the packet
+      //Reset the buffer location so we can start over
+      input_buffer_location=0;
+      //printf("Error, Byte 2 not what was expected: 0xFF!=0x%02x\n",input_buffer[1]);
+    }
+  }else{
+    //Something is wrong.  We may just not be at the correct location in the packet
+    //Reset the buffer location so we can start over
+    input_buffer_location=0;
+    //printf("Error, Byte 1 not what was expected: 0xFF!=0x%02x\n",input_buffer[0]);
+  }
+}
+
+
 // Function trapezoidalTrajectory: Function that takes in a time (float in seconds) and outputs a float (0 to 1) that corresponds to a trapezoidal trajectory
 float trapezoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) {
     // Define variables specific to this function
@@ -195,14 +285,14 @@
 
 // Function timerISRFunction: Timer ISR function to collect data and write to ring buffer
 void timerISRFunction() {
-    if(collect_data){
-        //led 1 is used as a 'thinking' light, brighter=worse
-        led1 = 1;
-        led2 = 0;
-        triggerOut = 1;
-        
-        ISRDurationTimer.reset();
-        ISRDurationTimer.start();
+    //led 1 is used as a 'thinking' light, brighter=worse
+    led1 = 1;
+    led2 = 0;
+    triggerOut = 1;
+    
+    ISRDurationTimer.reset();
+    ISRDurationTimer.start();
+    if(g_current_mode==MODE_AUTOMATIC){
         
         // Warning, this calculation is in the ISR and as such is probably slower than we would prefer.  
         // @todo The math could certainly be optimized with some precalculated constants.  Lookup tables are faster than sin()
@@ -221,17 +311,30 @@
                 adc.interruptRead();
             }
             
+            while(cm.readable()){
+                led4=1;
+                serialInterrupt(cm.getc());
+            }
+            led4=0;
+            
             short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
             short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
-            mx12_right_jaw.coordinated_move(LEFT_JAW_DYNAMIXEL_ID,left_servo, 0, RIGHT_JAW_DYNAMIXEL_ID,right_servo, 0);
             
-//            tempSpindleData.myServoData[LEFT_SERVO_INDEX].force  = adc.read();
-//            tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos    = mx12_left_jaw.GetRawPosition();
-//            tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
-//            tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos   = mx12_right_jaw.GetRawPosition();
-//            tempSpindleData.direction=g_current_direction;
-//            tempSpindleData.cycle=g_current_cycle;
-//            Buffer.write(tempSpindleData);
+            //Send the commanded position to the OpenCM board
+            cm.putc(0xFF);
+            cm.putc(0xFF);
+            cm.putc(left_servo  >> 8);   //Top 4 bits
+            cm.putc(left_servo  & 0xFF); //Bottom 8 bits
+            cm.putc(right_servo >> 8);   //Top 4 bits
+            cm.putc(right_servo & 0xFF); //Bottom 8 bits
+            
+            tempSpindleData.myServoData[LEFT_SERVO_INDEX].force  = adc.read();
+            tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos    = left_servo_measured;
+            tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = 0;
+            tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos   = right_servo_measured;
+            tempSpindleData.direction=g_current_direction;
+            tempSpindleData.cycle=g_current_cycle;
+            Buffer.write(tempSpindleData);
         #else   
             g_theta=(1.0-percent)*(MAX_SERVO_ANGLE_Da_VINCI-MIN_SERVO_ANGLE_Da_VINCI)+MIN_SERVO_ANGLE_Da_VINCI;
             tempSpindleData.myServoData[LEFT_SERVO_INDEX].force  = AinLeftForce.read_u16();
@@ -246,24 +349,102 @@
             
             moveServoTo(g_theta); // in degrees, son
         #endif
+    }else if(g_current_mode==MODE_MANUAL){
         
-        //done thinking
-        led1 = 0;
-        led2 = 1;
-        triggerOut = 0;
+        g_current_trajectory_time+=samplingPeriod;
+        if(adc.isReady()){
+            adc.interruptRead();
+        }
+        
+        int im_tired_of_this_game=0;
+        while(cm.readable() && im_tired_of_this_game++<100){
+            serialInterrupt(cm.getc());
+        }
+        
+//        float pot_open=0.75;
+//        float pot_closed=0.48;
+//        
+//        float percent=AinJoystickFwdBk.read();
+//        percent=(pot_open-percent)/(pot_open-pot_closed);
+//        float skew   =0;//AinJoystickLftRt.read()-JoystickLftRt_Zero;
+//        
+//        // The 'pulling' of the trigger corresponds to open/closed
+//        short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
+//        short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
+//        
+//        g_input_pot_1=left_servo;
+        short left_servo =g_command_corrected;
+        short right_servo=g_command_corrected;
         
-        ISRDurationTimer.stop();
-        current_latency=ISRDurationTimer.read_us();
-        if(current_latency>worst_latency){
-            worst_latency=current_latency;
-        }
+//        // The 'skewing' of the trigger corresponds to yawing the jaws left or right, while maintaining open/closed
+//        left_servo +=skew*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT );
+//        right_servo-=skew*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT);
+//        
+//        /// Todo: There is undoubtedly a cleaner way to do this.
+//        if(OPEN_SERVO_ANGLE_LEFT  < CLOSED_SERVO_ANGLE_LEFT   && left_servo  < OPEN_SERVO_ANGLE_LEFT ){left_servo =OPEN_SERVO_ANGLE_LEFT; }
+//        if(OPEN_SERVO_ANGLE_LEFT  > CLOSED_SERVO_ANGLE_LEFT   && left_servo  > OPEN_SERVO_ANGLE_LEFT ){left_servo =OPEN_SERVO_ANGLE_LEFT; }
+//        if(OPEN_SERVO_ANGLE_RIGHT < CLOSED_SERVO_ANGLE_RIGHT  && right_servo < OPEN_SERVO_ANGLE_RIGHT){right_servo=OPEN_SERVO_ANGLE_RIGHT;}
+//        if(OPEN_SERVO_ANGLE_RIGHT > CLOSED_SERVO_ANGLE_RIGHT  && right_servo > OPEN_SERVO_ANGLE_RIGHT){right_servo=OPEN_SERVO_ANGLE_RIGHT;}
+//        
+//        // Just to make sure
+//        // left_servo=(left_servo+4096)%4096;
+//        if(left_servo <   0){left_servo =   0;}
+//        if(left_servo >4095){left_servo =4095;}
+//        if(right_servo<   0){right_servo=   0;}
+//        if(right_servo>4095){right_servo=4095;}
+        
+        //Send the commanded position to the OpenCM board
+        cm.putc(0xFF);
+        cm.putc(0xFF);
+        cm.putc(left_servo  >> 8);   //Top 4 bits
+        cm.putc(left_servo  & 0xFF); //Bottom 8 bits
+        cm.putc(right_servo >> 8);   //Top 4 bits
+        cm.putc(right_servo & 0xFF); //Bottom 8 bits
+        
+        #ifdef ROD_IS_RIGHT
+            //Famous Rod's Magical World of Good Ideas Part 1
+            int angle=left_servo_measured;
+            angle_sum+=angle;
+            if(angle_count!=0){
+                int angledot=(angle-last_angle);
+                angledot_sum+=angledot;
+                if(angledot_count!=0){
+                    int angledotdot=(angledot-last_angledot);
+                    angledotdot_sum+=angledotdot;
+                    angledotdot_count++;
+                }
+                angledot_count++;
+                last_angledot=angledot;
+            }
+            angle_count++;
+            last_angle=angle;
+        #else
+            // John's worse than Rod ideas of terribleness
+            if(!crunching_data_flag){ // Only add data if no one is using it
+                recent_pos.add_data(left_servo_measured,g_current_trajectory_time);
+            }
+        #endif
+    }
+        
+    //done thinking
+    led1 = 0;
+    led2 = 1;
+    triggerOut = 0;
+    
+    ISRDurationTimer.stop();
+    current_latency=ISRDurationTimer.read_us();
+    if(current_latency>worst_latency){
+        worst_latency=current_latency;
     }
 }
 
 
 int main() {
     // Crazy fast baud rate!
-    pc.baud(921600);
+    pc.baud(115200);
+    
+    // For communication with OpenCM 9.04 Board
+    cm.baud(1000000);
     
     #ifdef USE_BLUETOOTH
     bt.baud(9600);
@@ -299,31 +480,14 @@
     pc.printf("\n\n");
     
     #ifdef USE_DYNAMIXELS
-        mx12_left_jaw.Init();
-        //mx12_left_jaw.SetBaud(3000000);
-        //mx12_left_jaw.SetBaud(1000000);
-        //printf("Current Position=%1.3f\n",mx12_left_jaw.GetPosition());
-        mx12_right_jaw.Set_Return_Delay_Time(0.050);
-        printf("Current ReturnDelay=%f ms\n",mx12_left_jaw.Get_Return_Delay_Time());
-        mx12_left_jaw.Set_Return_Delay_Time(0.050);
-        //mx12_left_jaw.Set_Torque_Limit(99.9);
-        //mx12_right_jaw.Set_Torque_Limit(99.9);
-        mx12_left_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
-        mx12_right_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
-        mx12_left_jaw.Set_P_Gain(4);
-        mx12_right_jaw.Set_P_Gain(4);
-        mx12_left_jaw.Set_I_Gain(8);
-        mx12_right_jaw.Set_I_Gain(8);
-        mx12_left_jaw.Set_Alarm_Shutdown(0x04);
-        mx12_right_jaw.Set_Alarm_Shutdown(0x04);
-        
-        mx12_left_jaw.Dump_OD_to_Serial(pc);
-        mx12_right_jaw.Dump_OD_to_Serial(pc);
-        
-        
-        
         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();}
+        JoystickLftRt_Zero=JoystickLftRt_Zero/calib_N;
+        g_current_mode=MODE_MANUAL;
     #else
         // Configure Servo for HiTec 422
         myServoLeft.period_ms(20);
@@ -332,11 +496,15 @@
     
     printf("Setup Complete.\n");
     AuxSerialTimer.start();
-    
+    mytimer.start();
     while(1)
     {
         // spin in a main loop. serialISR will interrupt it to call serialPot
         
+        #ifdef USE_DYNAMIXELS
+        
+        #endif
+        
         ///This checks for any new serial bytes, and returns true if
         ///we have an entire packet ready.  The new packet will live
         ///in newData.
@@ -357,7 +525,6 @@
             g_num_cycles=newData[3];
             g_current_trajectory_time=0;
             g_current_cycle=0;
-            g_current_mode=MODE_AUTOMATIC;
             #ifdef USE_SD_CARD
                 int first_slash=file_name_in.find("/");
                 std::string new_dir="/sd/"+file_name_in.substr(0, first_slash);
@@ -382,13 +549,13 @@
                 fprintf(fp, "%%PositionLeft,ForceLeft,PositionRight,ForceRight,Time(ms),Direction,CycleNum\n");
             #endif
             // We are go-times!
-            collect_data=true;
+            g_current_mode=MODE_AUTOMATIC;
         }
         
-        if( collect_data && g_current_cycle >= g_num_cycles)
+        if( g_current_mode==MODE_AUTOMATIC && g_current_cycle >= g_num_cycles)
         {
             // STOOOOOOOOOP
-            collect_data=false;
+            g_current_mode=MODE_NULL;
             #ifdef USE_SD_CARD
                 // Close the file
                 fclose(fp); 
@@ -396,7 +563,7 @@
             #endif
         }
         
-        // This section of code should run whenever there is free time to print to the screen
+        // This section of code should run whenever there is free time to save datas
         #ifdef USE_SD_CARD
         if(fp != NULL) {
             // Only write to SD if there is a valid file handle
@@ -405,20 +572,101 @@
             led3 = 0;
         }
         #else
+            //Warning, this is a big fat stream of data, 1 minute is approx 3MB
+            //I certainly recommend using the SD card.
             Buffer.dumpBufferToSerial();
         #endif
         
-        if(AuxSerialTimer.read_ms()>100 && collect_data){
+        if(g_current_mode!=MODE_NULL && AuxSerialTimer.read_us()>30000){
             //Send some extra data for GUI purposes
-            printf("<%d,%d,%d,%d,%d,%d,%d>  ",tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos,
-                                              tempSpindleData.myServoData[LEFT_SERVO_INDEX].force,
-                                              tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos,
-                                              tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force,
-                                              tempSpindleData.time,
-                                              tempSpindleData.direction,
-                                              tempSpindleData.cycle);
-            printf("    %dus\n", worst_latency);
-            worst_latency=0;
+            #ifdef ROD_IS_RIGHT
+                float angle=0,angledot=0,angledotdot=0;
+                if(angledotdot_count>0){
+                    angle      =float(angle_sum      )/angle_count;
+                    angledot   =float(angledot_sum   )/angledot_count;
+                    angledotdot=float(angledotdot_sum)/angledotdot_count;
+                }
+                angle_sum=0;angle_count=0;
+                angledot_sum=0;angledot_count=0;
+                angledotdot_sum=0;angledotdot_count=0;
+            #else
+                crunching_data_flag=true;
+                recent_pos.least_squares_3rd(coeffs);
+                crunching_data_flag=false;
+                float x=g_current_trajectory_time;
+                int angle      =coeffs[0]+coeffs[1]*x+coeffs[2]*x*x+coeffs[3]*x*x*x;
+                int angledot   =          coeffs[1]+2*coeffs[2]*x+3*coeffs[3]*x*x  ;
+                int angledotdot=                    2*coeffs[2]  +6*coeffs[3]*x    ;
+            #endif
+            
+            
+            
+            float pot_open=0.75;
+            float pot_closed=0.48;
+            
+            float percent=0.0;
+            //Average the pot a bunch, it's NOISY
+            for(int i=0;i<100;i++){
+                percent+=AinJoystickFwdBk.read();
+            }
+            percent=percent/100.0;
+            percent=(pot_open-percent)/(pot_open-pot_closed);
+            
+            // 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;
+            int loadcell=adc.read();
+            
+            if(!g_we_are_grasping && loadcell>entry_threshold && angledot<velocty_threshold){
+                //Grasp is starting
+                g_we_are_grasping=true;
+                for(int i=0;i<NUMBER_OF_TISSUES;i++){
+                    g_error_norm[i]=0.0;
+                }
+            }
+            if(g_we_are_grasping && angledot>-velocty_threshold){
+                //Grasp is over
+                g_we_are_grasping=false;
+            }
+            float alpha=0;
+            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));
+                
+                
+                float offset1 = 100000;
+                float offset2 = 300000;
+                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;}
+                
+                float force_err=loadcell-g_thresh_force[tissue_id];
+                float k=20/0.1e6;
+                g_command_corrected=(1-alpha)*potread+alpha*(k*force_err+angle);
+            }else{
+                g_command_corrected=potread;
+            }
+            
+            //std::vector<int> data_in=<angle, angledot, angledotdot, loadcell, potread>
+            printf("%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("%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,
+//                                              tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos,
+//                                              tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force,
+//                                              tempSpindleData.time,
+//                                              tempSpindleData.direction,
+//                                              tempSpindleData.cycle);
+//            printf("    %dus", worst_latency);
+//            worst_latency=0;
+//            printf("    %0.0fbps\n", bits_received*1000.0/AuxSerialTimer.read_ms());
+//            bits_received=0;
             AuxSerialTimer.reset();
         }