Diff: main.cpp

Revision:

9:f26a91863e3a

Parent:

8:cef07a8bdc42

Child:

10:5c0c9c647090

--- a/main.cpp	Mon May 11 21:39:45 2015 +0000
+++ b/main.cpp	Tue May 12 17:04:31 2015 +0000
@@ -1,7 +1,9 @@
+// Different modes to select
 #define USE_DYNAMIXELS
 //#define USE_BLUETOOTH
 #define USE_SD_CARD
 #define ROD_IS_RIGHT
+//#define CALIBRATE_TIME_NOW
 
 // We have different modes for different things
 #define MODE_MANUAL     1
@@ -49,7 +51,6 @@
     STATE_OPEN_HOLD=3
 };
 
-
 // Define pins and interrupts
 Ticker potISR;              //Define a recurring timer interrupt
 DigitalOut led1(LED1);      //Led 1 for debugging purposes
@@ -58,14 +59,15 @@
 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
+
+// Specific to bluetooth
 #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
-
+// Specific to SD Card
 #ifdef USE_SD_CARD
     // Attach SD card
     SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
@@ -78,7 +80,6 @@
 float max_percent_full=0;
 
 // Define variables for the program
-
 char g_tissue_type_name[32];
 float g_frequency;
 int g_max_force;
@@ -93,9 +94,14 @@
 #define NUMBER_OF_TISSUES 2
 float g_error_norm[NUMBER_OF_TISSUES];
 bool g_we_are_grasping=false;
-#define CALIBRATION_READS 500.0
+#define CALIBRATION_READS 100.0
 float g_calibration_offset = 8600000.0;
 
+// Values at first touch
+int g_masterPositionFirstTouch = 2400;
+int g_slavePositionFirstTouch = 2400;
+float g_forceAboveFirstTouch = 30000.0;
+float g_timeAtFirstTouch;
 
 ///////////Magic numbers courtesy Rod///////////////
 float Phi1[5]={-8.02086003501975e-08,
@@ -110,6 +116,7 @@
                 6.75893262890734,
                -0.00228098997419065};
 
+// Magic numbers to determine if we are grasping (CHANGE TO  A DYNAMIC APPROACH!!!!)  
 float entry_threshold=8.70e6;
 float velocty_threshold=-0.01;
 
@@ -119,14 +126,14 @@
 int g_command_corrected;
 
 // These are for load cell initialization of the offset
-long g_loadCellZero = 8600000;
+float g_loadCellZero = 8600000.0;
 bool calibrateDone = false;
 long calibrateTotal = 8600;
 int calibCntr = 0;
 int g_loadCellOffset = 100000;
 int g_threshOffset1 = 0.2e6;
 int g_threshOffset2 = 0.15e6;
-
+long long loadCellTotal = 0;
 
 Timer mytimer;
 
@@ -155,6 +162,7 @@
 int worst_latency=0;
 int current_latency;
 
+// Specific to Dynamixels
 #ifdef USE_DYNAMIXELS
     Serial cm(p28,p27);         //Set up serial connection to OpenCM 9.04
     unsigned short left_servo_measured=0;
@@ -218,6 +226,7 @@
 
 #endif
 
+// Serial interrupt function  
 void serialInterrupt(char buffer){
   input_buffer[input_buffer_location]=buffer;
   input_buffer_location++;
@@ -231,7 +240,7 @@
     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
+        //We do!  Extract the juicy 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);
@@ -265,7 +274,7 @@
     // Take the time and mod it with the period to be able to break up each cycle into 4 piecewise sections
     timeMod = fmodf(t,period);
     
-    //
+    // Determining trajectory
     if (timeMod < period/4.0)
     {
         y_trapezoid = (-4.0/period)*(timeMod)+1.0;
@@ -297,7 +306,7 @@
 
 // Function timerISRFunction: Timer ISR function to collect data and write to ring buffer
 void timerISRFunction() {
-    if (calibrateDone){
+    //if (calibrateDone){
         //led 1 is used as a 'thinking' light, brighter=worse
         led1 = 1;
         led2 = 0;
@@ -449,25 +458,27 @@
         if(current_latency>worst_latency){
             worst_latency=current_latency;
         }
-    }
+    //}
 }
 
 // Calibrate Load Cell Function
 void calibrateLoadCell() {
     long long loadCellTotal = 0;
+    int intermediateValue;
     for(int ii=0;ii<CALIBRATION_READS;ii++){
-        loadCellTotal+= adc.read();
+        intermediateValue = adc.read();
+        //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=loadCellTotal/CALIBRATION_READS;
+    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;
     calibrateDone = true;
 }
 
-
 int main() {
     // Crazy fast baud rate!
     pc.baud(115200);
@@ -511,8 +522,8 @@
     
     #ifdef USE_DYNAMIXELS
         adc.setFilter(256 , false, 1);
-        calibrateLoadCell();
         adc.start();
+        
         JoystickLftRt_Zero=0;
         int calib_N=100;
         for(int ii=0;ii<calib_N;ii++){JoystickLftRt_Zero+=AinJoystickLftRt.read();}
@@ -529,6 +540,10 @@
     printf("Setup Complete.\n");
     AuxSerialTimer.start();
     mytimer.start();
+    
+    
+    //wait_ms(1000);
+    //calibrateLoadCell();
     while(1)
     {
         // spin in a main loop. serialISR will interrupt it to call serialPot
@@ -647,64 +662,99 @@
             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 ;
+            int potread = percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
         
             
             
             
             //angle=left_servo_measured;
             int loadcell=adc.read();
-            
-            //if (!calibrateDone){
+            //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;
-                //calibCntr ++;
-                //if (calibCntr == CALIBRATION_READS){
-                    //g_loadCellZero = calibrateTotal; 
+                
+                //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;
-                //}
-            //}
-            // Code to determine if we should toggle the variable of "g_we_are_grasping"
-            if(!g_we_are_grasping && loadcell>g_loadCellZero+g_loadCellOffset && 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;
+                    g_thresh_force[0]=g_threshOffset1+g_loadCellZero;
+                    g_thresh_force[1]=g_threshOffset2+g_loadCellZero;
+                    calibrateDone = true;
                 }
             }
-            if(g_we_are_grasping && potread > 2400 ) {//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));
-                
+
+
+            #ifdef CALIBRATE_TIME_NOW
+                if (calibrateDone){
+                    g_command_corrected=potread;
+                    int loadcell_offset = loadcell - g_loadCellZero;
+                    if(loadcell_offset < 0){
+                        loadcell_offset = 0;
+                    }
+                    printf("%6.3f,%0.0f,%2.3f,%2.3f,%d\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell_offset);
+            
+                }
+            #else
+                // 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){
+                    //Grasp is starting
+                    g_we_are_grasping=true;
+                    // Set the position and time at first touch
+                    g_masterPositionFirstTouch = potread;
+                    g_timeAtFirstTouch = mytimer.read_us()/1000.0;
+                    for(int i=0;i<NUMBER_OF_TISSUES;i++){
+                        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){
+                    //Grasp is over
+                    g_error_norm[0] = 0;
+                    g_error_norm[1] = 0;
+                    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;
+                }
                 
-                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;}
+                //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;
+                }
                 
-                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;
-            }
+                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());
+            #endif
             
-            //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,%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,
@@ -713,7 +763,7 @@
 //                                              tempSpindleData.time,
 //                                              tempSpindleData.direction,
 //                                              tempSpindleData.cycle);
-//            printf("    %dus", worst_latency);
+//            printf("    %dus", worst_latency);  
 //            worst_latency=0;
 //            printf("    %0.0fbps\n", bits_received*1000.0/AuxSerialTimer.read_ms());
 //            bits_received=0;