Diff: main.cpp

Revision:

13:6a0a7a04fd91

Parent:

12:560dc69d4ca4

Child:

14:7c5beaa9fb01

--- a/main.cpp	Mon Jul 20 17:12:36 2015 +0000
+++ b/main.cpp	Thu Aug 13 17:26:23 2015 +0000
@@ -1,7 +1,7 @@
 // Different modes to select
 #define USE_DYNAMIXELS
 //#define USE_BLUETOOTH
-#define USE_SD_CARD
+//#define USE_SD_CARD
 #define ROD_IS_RIGHT
 //#define CALIBRATE_TIME_NOW
 
@@ -52,15 +52,16 @@
 };
 
 // Define pins and interrupts
-Ticker potISR;              //Define a recurring timer interrupt
-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
+Ticker potISR;                             //Define a recurring timer interrupt
+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 triggerOut(p11);
-Serial pc(USBTX, USBRX);    //Set up serial connection to pc
-AnalogIn AinLeftForce(p16); //Set up left load cell on pin 16
-AnalogIn AinRightForce(p15);//Set up right load cell on pin 15
+Serial pc(USBTX, USBRX);                   //Set up serial connection to pc
+AnalogIn AinLeftForce(p16);                //Set up left load cell on pin 16
+AnalogIn AinRightForce(p15);               //Set up right load cell on pin 15
+DigitalOut failSafePowerSwitch(p29);       //Set up pin 28 to send HIGH to OpenCM when MBED has power
 
 // Specific to bluetooth
 #ifdef USE_BLUETOOTH
@@ -97,20 +98,23 @@
 #define CALIBRATION_READS 200.0
 float g_calibration_offset = 8600000.0;
 float integral_err = 0.0;
+float g_loadCellSlopeCalibration = 2.1e-5;
+float forceInNewtons = 0.0;
+float g_gravity = 9.81;
 
 // Values at first touch
 int g_masterPositionFirstTouch = 2400;
 int g_slavePositionFirstTouch = 2400;
-float g_forceAboveFirstTouch = 30000.0;
+float g_forceAboveFirstTouch = 3.5;
 float g_timeAtFirstTouch;
 
 ///////////Magic numbers courtesy Rod///////////////
 //float Phi1[5]={-8.02086003501975e-08,
 //                1.55286905458007e-05,
-//                0.00795344249784777,
+//                0.00795344249784777, 
 //                8.23733045077119,
 //               -0.00299236282304311};
-//float Phi2[5]={-1.24436831310503e-08,
+//float Phi2[5]={-1.24436831310503e-08,                                  
 //                1.23673348605010e-05,
 //                0.00652545188345528,
 //                6.75893262890734,
@@ -497,7 +501,7 @@
 
 int main() {
     // Crazy fast baud rate!
-    pc.baud(115200);
+    pc.baud(115200*8);
     
     // For communication with OpenCM 9.04 Board
     cm.baud(1000000);
@@ -509,6 +513,8 @@
     // Attach ISR routines
     potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod   
     
+    // Turn p28 to HIGH signaling the OpenCM to take commands from MBED
+    failSafePowerSwitch = 1;
     
     // Some debug info:
     //DisplayRAMBanks();
@@ -642,7 +648,7 @@
             Buffer.dumpBufferToSerial();
         #endif
         
-        if(g_current_mode!=MODE_NULL && AuxSerialTimer.read_us()>30000){
+        if(g_current_mode!=MODE_NULL && AuxSerialTimer.read_us()>1000){
             //Send some extra data for GUI purposes
             #ifdef ROD_IS_RIGHT
                 float angle=0,angledot=0,angledotdot=0;
@@ -697,6 +703,7 @@
                     calibrateDone = true;
                 }
             }
+            
 
             #ifdef CALIBRATE_TIME_NOW
                 if (calibrateDone){
@@ -713,12 +720,15 @@
                 if(loadcell_offset < 0.0){
                     loadcell_offset = 0.0;
                 }
+                // Convert Load cell read into Newtons
+                forceInNewtons = ((loadcell_offset)*g_loadCellSlopeCalibration)*g_gravity;
                 // Code to determine if we should toggle the variable of "g_we_are_grasping"
-                if(!g_we_are_grasping && loadcell_offset>g_forceAboveFirstTouch && angledot<velocity_threshold){
+                if(!g_we_are_grasping && forceInNewtons>g_forceAboveFirstTouch && angledot<velocity_threshold){
                     //Grasp is starting
                     g_we_are_grasping=true;
                     // Set the position and time at first touch
                     g_masterPositionFirstTouch = potread;
+                    g_slavePositionFirstTouch = angle;  
                     g_timeAtFirstTouch = mytimer.read_us()/1000.0;
                     for(int i=0;i<NUMBER_OF_TISSUES;i++){
                         g_error_norm[i]=0.0;
@@ -755,11 +765,11 @@
                     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);
+                    //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;
+                    g_command_corrected = potread;
                 }else{
                     g_command_corrected=potread;
                 }
@@ -769,9 +779,12 @@
                 if (!calibrateDone){
                     g_command_corrected = 2800;
                 }
-                printf("Data:,%6.3f,%6.3f,%2.3f,%s\n",mytimer.read_us()/1000.0,loadcell_offset,alpha,sstr.c_str());
+                //printf("Data:,%6.3f,%6.3f,%3.4f,%6.3f\n",(float(g_slavePositionFirstTouch)-angle),forceInNewtons,float(g_slavePositionFirstTouch),forceInNewtons);
+                printf("Data:,%6.3f,%6.3f,%6.3f,%6.3f,%6.3f\n",mytimer.read_us()/1000.0,forceInNewtons,(float(g_slavePositionFirstTouch)-angle),angledot,angledotdot);
                 
-                //printf("Data:,%6.3f,%0.0f,% 2.3f,% 2.3f,%6.3f,%d,%0.0f,%12.3f,%2.3f,%s\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell_offset, potread,g_error_norm[0],g_loadCellZero,alpha,sstr.c_str());
+                
+                //printf("Data:,%6.3f,%6.3f,%3.4f,%d\n",mytimer.read_us()/1000.0,g_loadCellZero,forceInNewtons,adc.read());
+                
                 
             #endif