Diff: main.cpp

Revision:

8:cef07a8bdc42

Parent:

7:b4b95e5f0fce

Child:

9:f26a91863e3a

--- a/main.cpp	Wed May 06 20:21:32 2015 +0000
+++ b/main.cpp	Mon May 11 21:39:45 2015 +0000
@@ -93,8 +93,8 @@
 #define NUMBER_OF_TISSUES 2
 float g_error_norm[NUMBER_OF_TISSUES];
 bool g_we_are_grasping=false;
-#define CALIBRATION_READS 5000.0
-float g_calibration_offset;
+#define CALIBRATION_READS 500.0
+float g_calibration_offset = 8600000.0;
 
 
 ///////////Magic numbers courtesy Rod///////////////
@@ -118,6 +118,16 @@
 
 int g_command_corrected;
 
+// These are for load cell initialization of the offset
+long g_loadCellZero = 8600000;
+bool calibrateDone = false;
+long calibrateTotal = 8600;
+int calibCntr = 0;
+int g_loadCellOffset = 100000;
+int g_threshOffset1 = 0.2e6;
+int g_threshOffset2 = 0.15e6;
+
+
 Timer mytimer;
 
 // Warning, this buffer is large!
@@ -287,40 +297,114 @@
 
 // Function timerISRFunction: Timer ISR function to collect data and write to ring buffer
 void timerISRFunction() {
-    //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){
+    if (calibrateDone){
+        //led 1 is used as a 'thinking' light, brighter=worse
+        led1 = 1;
+        led2 = 0;
+        triggerOut = 1;
         
-        // 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()
-        float percent=trapezoidalTrajectory(g_current_trajectory_time,g_current_direction,g_current_cycle);
-        g_current_trajectory_time+=samplingPeriod;
-        
-        
-        //float angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2;
-        //g_current_direction=(cos(angle)<0);
-        //g_current_cycle=(angle+M_PI_2)/(2.0*M_PI);
-        
-
-        #ifdef USE_DYNAMIXELS
-            //float percent=(sin(angle)+1)/2.0;
+        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()
+            float percent=trapezoidalTrajectory(g_current_trajectory_time,g_current_direction,g_current_cycle);
+            g_current_trajectory_time+=samplingPeriod;
+            
+            
+            //float angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2;
+            //g_current_direction=(cos(angle)<0);
+            //g_current_cycle=(angle+M_PI_2)/(2.0*M_PI);
+            
+    
+            #ifdef USE_DYNAMIXELS
+                //float percent=(sin(angle)+1)/2.0;
+                if(adc.isReady()){
+                    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;
+                
+                //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();
+                tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos    = AinLeftPosition.read_u16();
+                tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
+                tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos   = AinRightPosition.read_u16();
+                tempSpindleData.direction=g_current_direction;
+                tempSpindleData.cycle=g_current_cycle;
+                Buffer.write(tempSpindleData);
+                
+                
+                
+                moveServoTo(g_theta); // in degrees, son
+            #endif
+        }else if(g_current_mode==MODE_MANUAL){
+            
+            g_current_trajectory_time+=samplingPeriod;
             if(adc.isReady()){
                 adc.interruptRead();
             }
             
-            while(cm.readable()){
-                led4=1;
+            int im_tired_of_this_game=0;
+            while(cm.readable() && im_tired_of_this_game++<100){
                 serialInterrupt(cm.getc());
             }
-            led4=0;
+            
+    //        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;
             
-            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;
+    //        // 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);
@@ -330,125 +414,57 @@
             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();
-            tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos    = AinLeftPosition.read_u16();
-            tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
-            tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos   = AinRightPosition.read_u16();
-            tempSpindleData.direction=g_current_direction;
-            tempSpindleData.cycle=g_current_cycle;
-            Buffer.write(tempSpindleData);
-            
-            
+            #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
+        }
             
-            moveServoTo(g_theta); // in degrees, son
-        #endif
-    }else if(g_current_mode==MODE_MANUAL){
-        
-        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;
+        //done thinking
+        led1 = 0;
+        led2 = 1;
+        triggerOut = 0;
         
-//        // 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;
+        ISRDurationTimer.stop();
+        current_latency=ISRDurationTimer.read_us();
+        if(current_latency>worst_latency){
+            worst_latency=current_latency;
+        }
     }
 }
 
 // Calibrate Load Cell Function
 void calibrateLoadCell() {
-    int loadCellTotal = 0;
+    long long loadCellTotal = 0;
     for(int ii=0;ii<CALIBRATION_READS;ii++){
-        loadCellTotal=adc.read();
-        pc.printf("Am I crazy?!?!?!?!: %d \n",loadCellTotal);
+        loadCellTotal+= adc.read();
+        wait_ms(2);
         //pc.printf("We are calibrating load cells...\n");
         }
     g_calibration_offset=loadCellTotal/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;
 }
 
 
@@ -464,7 +480,8 @@
     #endif
     
     // Attach ISR routines
-    potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod
+    potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod   
+    
     
     // Some debug info:
     //DisplayRAMBanks();
@@ -492,11 +509,9 @@
     pc.printf("#################################\n");
     pc.printf("\n\n");
     
-
-    
-    
     #ifdef USE_DYNAMIXELS
         adc.setFilter(256 , false, 1);
+        calibrateLoadCell();
         adc.start();
         JoystickLftRt_Zero=0;
         int calib_N=100;
@@ -509,9 +524,7 @@
         myServoRight.period_ms(20);
     #endif
     
-    // Begin Calibration of load cells
-    // Calibrate Load Cells (Does this need to get moved somewhere else?)
-    //calibrateLoadCell();
+
     
     printf("Setup Complete.\n");
     AuxSerialTimer.start();
@@ -520,6 +533,8 @@
     {
         // spin in a main loop. serialISR will interrupt it to call serialPot
         
+   
+        
         #ifdef USE_DYNAMIXELS
         
         #endif
@@ -640,16 +655,30 @@
             //angle=left_servo_measured;
             int loadcell=adc.read();
             
-            
+            //if (!calibrateDone){
+                // 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; 
+                    //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>entry_threshold && angledot<velocty_threshold){
+            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;
                 }
             }
-            if(g_we_are_grasping && angledot>-velocty_threshold){
+            if(g_we_are_grasping && potread > 2400 ) {//angledot>-velocty_threshold){
                 //Grasp is over
                 g_we_are_grasping=false;
             }