Diff: main.cpp

Revision:

12:560dc69d4ca4

Parent:

11:53408545bbb4

Child:

13:6a0a7a04fd91

--- a/main.cpp	Tue May 12 21:37:21 2015 +0000
+++ b/main.cpp	Mon Jul 20 17:12:36 2015 +0000
@@ -59,8 +59,8 @@
 DigitalOut led4(LED4);      //Led 4 for debugging purposes
 DigitalOut triggerOut(p11);
 Serial pc(USBTX, USBRX);    //Set up serial connection to pc
-AnalogIn AinLeftForce(p16);     //Set up potentiometer on pin 20
-AnalogIn AinRightForce(p15);    //Set up potentiometer on pin 20
+AnalogIn AinLeftForce(p16); //Set up left load cell on pin 16
+AnalogIn AinRightForce(p15);//Set up right load cell on pin 15
 
 // Specific to bluetooth
 #ifdef USE_BLUETOOTH
@@ -96,6 +96,7 @@
 bool g_we_are_grasping=false;
 #define CALIBRATION_READS 200.0
 float g_calibration_offset = 8600000.0;
+float integral_err = 0.0;
 
 // Values at first touch
 int g_masterPositionFirstTouch = 2400;
@@ -114,7 +115,8 @@
 //                0.00652545188345528,
 //                6.75893262890734,
 //               -0.00228098997419065};
-// Zero-Offset Magic Numbers
+
+/////////// Zero-Offset Magic Numbers courtesy Rod//////////////
 float Phi1[5] = {-1.80212053214826e-08,
                  1.69579390650964e-06,
                  0.000592679062823746,
@@ -131,7 +133,7 @@
 //float entry_threshold=8.70e6;
 float velocity_threshold=-0.01;
 
-float g_thresh_force[NUMBER_OF_TISSUES]={0.12e6,0.08e6};
+float g_thresh_force[NUMBER_OF_TISSUES]={0.16e6,0.06e6};
 ///////////Magic numbers courtesy Rod///////////////
 
 int g_command_corrected;
@@ -406,6 +408,9 @@
     //        short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
     //        
     //        g_input_pot_1=left_servo;
+            // Set limits on the max and min value of the grasper (for safety reasons)
+            if (g_command_corrected < 1900){g_command_corrected = 1900;};
+            if (g_command_corrected > 2900){g_command_corrected = 2900;};
             short left_servo =g_command_corrected;
             short right_servo=g_command_corrected;
             
@@ -554,7 +559,7 @@
     
     
     //wait_ms(1000);
-    //calibrateLoadCell();
+    //calibrateLoadCell(); // This function did not work as well as having it in the main loop
     while(1)
     {
         // spin in a main loop. serialISR will interrupt it to call serialPot
@@ -723,6 +728,7 @@
                     //Grasp is over
                     g_error_norm[0] = 0;
                     g_error_norm[1] = 0;
+                    integral_err = 0.0;
                     g_we_are_grasping=false;
                 }
                 float alpha=0;
@@ -738,13 +744,22 @@
                     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;
+                    alpha += 0.15;
                     if (alpha > 1.0){alpha = 1.0;}
                     float force_err=loadcell_offset-g_thresh_force[tissue_id];
-                    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);
+                    integral_err = integral_err + force_err;
+                    float current_time = mytimer.read_us()/1000.0;
+                    float dt = 39.0; //Change to a real approach later
+                    float kp = 10.0/0.1e6;
+                    float ki = 10.0/0.1e8;
+                    if (tissue_id == 0){kp=10.0/0.1e6;}
+                    if (tissue_id == 1){kp=100.0/0.1e6;}
+                    // Collaborative Mode
+                    g_command_corrected=(1-alpha)*potread+alpha*(kp*force_err+ki*integral_err*dt+angle);
+                    // Fully Autonomous Mode
+                    //g_command_corrected=(kp*force_err+angle);
+                    // Fully Manual Mode
+                    //g_command_corrected = potread;
                 }else{
                     g_command_corrected=potread;
                 }