File content as of revision 13:6a0a7a04fd91:

// 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
#define MODE_AUTOMATIC  2
#define MODE_IDLE       3
#define MODE_NULL       0

// We always want to know if we are closing or opening
#define DIRECTION_CLOSING 1
#define DIRECTION_OPENING 2
#define DIRECTION_SLACK_WATER 3
#define DIRECTION_NULL 0

// General includes
#include "mbed.h"
#include "ServoRingBuffer.h"
#include "ram_test.h"
#include "Serial_Receive.h"
#include "data_set.h"
#include <string>

// Specific to Dynamixels
#ifdef USE_DYNAMIXELS
#include "AD7730.h"
#endif

// Specific to SD Card
#ifdef USE_SD_CARD
#include "SDFileSystem.h"
#endif

// Everyone should know pi...
#ifndef M_PI
    #define M_PI    3.14159265358979323846  /* pi */
#endif
#ifndef M_PI_2
    #define M_PI_2  1.57079632679489661923  /* pi/2 */
#endif

// Create enum for the Jaw state (Closing, hold, opening)
enum jaw_state{
    STATE_CLOSING=0,
    STATE_CLOSE_HOLD=1,
    STATE_OPENING=2,
    STATE_OPEN_HOLD=3
};

// 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
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
DigitalOut failSafePowerSwitch(p29);       //Set up pin 28 to send HIGH to OpenCM when MBED has power

// Specific to bluetooth
#ifdef USE_BLUETOOTH
Serial bt(p13,p14);         //Set up serial connection to bluetooth adapter
#endif

// 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
    FILE *fp = NULL;
    #define SAMPLES_PER_FILE 10000
#endif

// Dummy variable for debugging
unsigned int global_count=0;
float max_percent_full=0;

// Define variables for the program
char g_tissue_type_name[32];
float g_frequency;
int g_max_force;
int g_num_cycles;
float g_current_trajectory_time;
float g_theta;
float g_theta_last=0;
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;
#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 = 3.5;
float g_timeAtFirstTouch;

///////////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};

/////////// Zero-Offset Magic Numbers courtesy Rod//////////////
float Phi1[5] = {-1.80212053214826e-08,
                 1.69579390650964e-06,
                 0.000592679062823746,
                 0.624774980172511,
                 -0.000283294192960159};

float Phi2[5] = {1.00123534796440e-09,
                 1.42089516619424e-06,
                 0.000520811899959219,
                 0.542284752693981,
                 -0.000248770560431049};
                 
// Magic numbers to determine if we are grasping (CHANGE TO  A DYNAMIC APPROACH!!!!)  
//float entry_threshold=8.70e6;
float velocity_threshold=-0.01;

float g_thresh_force[NUMBER_OF_TISSUES]={0.16e6,0.06e6};
///////////Magic numbers courtesy Rod///////////////

int g_command_corrected;

// These are for load cell initialization of the offset
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;

// 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;
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;
    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   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);
    
    
    AnalogIn AinJoystickFwdBk(p17);          //Set up potentiometer on pin 17
    AnalogIn AinJoystickLftRt(p16);          //Set up potentiometer on pin 16
    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
    #define SERVO_DEGREE_180  2100      //This is the pulse width value for HiTEC-422 in microseconds to turn 180 degrees
    #define MIN_SERVO_ANGLE   0.0       //This is the minimum servo angle in degrees
    #define MAX_SERVO_ANGLE   180.0     //This is the maximum servo angle in degrees
    #define MIN_SERVO_ANGLE_Da_VINCI   20.0       //This is the minimum servo angle in degrees
    #define MAX_SERVO_ANGLE_Da_VINCI   100.0     //This is the maximum servo angle in degrees
    const float servoConversion = ((SERVO_DEGREE_180-SERVO_DEGREE_0)/(MAX_SERVO_ANGLE - MIN_SERVO_ANGLE))/1000000.0;    //This is the interpolation between min and max servo values
    const float servoOffset = SERVO_DEGREE_0/1000000.0;                                                                 //This is the pulsewidth value (in seconds) that corresponds to 0 degrees (i.e.-the offset)
    
    PwmOut myServoLeft(p21);        //Set up servo on pin 21
    PwmOut myServoRight(p22);        //Set up servo on pin 22
    AnalogIn AinLeftPosition(p20);          //Set up potentiometer on pin 20
    AnalogIn AinRightPosition(p19);          //Set up potentiometer on pin 20


    // Function moveServoTo: Convert a degree value to pulsewidth for Servo
    void moveServoTo(float angle) {
        // Make sure none of the user input falls outside of min and max angle limits
        if(     angle < MIN_SERVO_ANGLE){angle = MIN_SERVO_ANGLE;}
        else if(angle > MAX_SERVO_ANGLE){angle = MAX_SERVO_ANGLE;}
        myServoLeft.pulsewidth(servoOffset + servoConversion*(180-angle));
        myServoRight.pulsewidth(servoOffset + servoConversion*(angle));
    }

#endif

// Serial interrupt function  
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 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);
        //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
    float y_trapezoid = 0.0;
    float timeMod;
    float modifiedFrequency = g_frequency/2.0;
    float period = 1/modifiedFrequency;
    cycle_num=t*modifiedFrequency;
    
    // 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;
        state = STATE_CLOSING;
    }
    else if (timeMod >= period/4.0 && timeMod < period/2.0)
    {
        y_trapezoid = 0.0;
        state = STATE_CLOSE_HOLD;
    }
    else if (timeMod >= period/2.0 && timeMod < 3*period/4.0)
    {
        y_trapezoid = (4.0/period)*(timeMod)-2;
        state = STATE_OPENING;
    }
    else if (timeMod >= 3*period/4.0)
    {
        y_trapezoid = 1.0;
        state = STATE_OPEN_HOLD;
    }

    return y_trapezoid; 
}

void sinusoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) {
    //Fill me with SCIENCE!!!
}


// Function timerISRFunction: Timer ISR function to collect data and write to ring buffer
void timerISRFunction() {
    //if (calibrateDone){
        //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()
            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();
            }
            
            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;
            // 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;
            
    //        // 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;
        }
    //}
}

// Calibrate Load Cell Function (NOT USING THIS ONE!!!)
void calibrateLoadCell() {
    long long loadCellTotal = 0;
    int intermediateValue;
    for(int ii=0;ii<CALIBRATION_READS;ii++){
        intermediateValue = adc.read();
        //pc.printf("Load Cell Read: %i\n",intermediateValue);
        loadCellTotal+= intermediateValue;
        wait_ms(2);
        //pc.printf("We are calibrating load cells...\n");
        }
    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*8);
    
    // For communication with OpenCM 9.04 Board
    cm.baud(1000000);
    
    #ifdef USE_BLUETOOTH
    bt.baud(9600);
    #endif
    
    // 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();
    //printf ("System clock = %d\r\n", SystemCoreClock);
    
    pc.printf("\n\n\n");
    pc.printf("----------------------------------\n");
    pc.printf("|                                |\n");
    pc.printf("|     Welcome to our mbed!       |\n");
    pc.printf("|                                |\n");
    pc.printf("| John and Trevor, Proprietors   |\n");
    pc.printf("|                                |\n");
    pc.printf("----------------------------------\n");
    pc.printf("                ||\n");
    pc.printf("                ||\n");
    pc.printf("                ||     _\n");
    pc.printf("                ||   _( )_\n");
    pc.printf("                ||  (_(#)_)\n");
    pc.printf("                ||    (_)\\\n");
    pc.printf("                ||        | __\n");
    pc.printf("   \\    /   |   ||        |/_/\n");
    pc.printf(" / | | /  / / | || | \\ \\  |    /  \n");
    pc.printf("/  / \\ \\ / / /  || / | /  /  \\ \\ \n");
    pc.printf("#################################\n");
    pc.printf("#################################\n");
    pc.printf("\n\n");
    
    #ifdef USE_DYNAMIXELS
        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);
        myServoRight.period_ms(20);
    #endif
    

    
    printf("Setup Complete.\n");
    AuxSerialTimer.start();
    mytimer.start();
    
    
    //wait_ms(1000);
    //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
        
   
        
        #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.
        if(
        #ifdef USE_BLUETOOTH
            receivePacket(bt)
        #else
            receivePacket(pc)
        #endif
        )
        {
            // < Tissue Type (string), Frequency Value (Hz) (int), Force Max (N) (int), # Cycles (in) >
            //<date/tissue/time,2,3,4>
            //g_tissue_type_name=tissue_type_name;
            std::string file_name_in=inString.substr(0, inString.find(","));
            g_frequency=newData[1]/10.0; // Since all we send are ints
            g_max_force=newData[2];
            g_num_cycles=newData[3];
            g_current_trajectory_time=0;
            g_current_cycle=0;
            #ifdef USE_SD_CARD
                int first_slash=file_name_in.find("/");
                std::string new_dir="/sd/"+file_name_in.substr(0, first_slash);
                std::string new_subdir="/sd/"+file_name_in.substr(0, file_name_in.find("/",first_slash+1));
                mkdir(new_dir.c_str(), 0777);
                mkdir(new_subdir.c_str(), 0777);
                std::string file_name="/sd/"+file_name_in+".csv";
                //pc.printf("subdir=\"%s\"\n",file_name.c_str());
                fp = fopen(file_name.c_str(), "w");
                //FILE *fp = fopen("/sd/data/sdtest.txt", "w");
                if(fp == NULL) {
                    error("Could not open file for write\n");
                }
                fprintf(fp, "%%Starting New Trajectory\n");
                fprintf(fp, "%%File Name=\"%s\"\n",file_name.c_str());
                fprintf(fp, "%%Current Mode=AUTOMATIC\n");
                fprintf(fp, "%%Trajectory Type=TRAPEZOIDAL\n");
                fprintf(fp, "%%Frequency=%f Hz\n",g_frequency);
                fprintf(fp, "%%Max Force=%f ??\n",g_max_force);
                fprintf(fp, "%%Num Cycles=%d\n",g_num_cycles);
                fprintf(fp, "%%Re. Direction: ,Closing=%d,Opening=%d,Undef=%d\n", DIRECTION_CLOSING , DIRECTION_OPENING , DIRECTION_SLACK_WATER );
                fprintf(fp, "%%PositionLeft,ForceLeft,PositionRight,ForceRight,Time(ms),Direction,CycleNum\n");
            #endif
            // We are go-times!
            g_current_mode=MODE_AUTOMATIC;
        }
        
        if( g_current_mode==MODE_AUTOMATIC && g_current_cycle >= g_num_cycles)
        {
            // STOOOOOOOOOP
            g_current_mode=MODE_NULL;
            #ifdef USE_SD_CARD
                // Close the file
                fclose(fp); 
                fp = NULL;
            #endif
        }
        
        // 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
            led3 = 1;
            Buffer.dumpBufferToSD(fp);
            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(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;
                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;
            
            //if (!calibrateDone){
            //    wait_ms(2);
            //}
            int loadcell=adc.read();

            if (!calibrateDone){
                g_command_corrected = 2800;
                // Begin Calibration of load cells
                loadCellTotal += (long long) loadcell;
                calibCntr ++;
                if (calibCntr == CALIBRATION_READS){
                    g_loadCellZero = float(loadCellTotal)/float(CALIBRATION_READS); 
                    calibrateDone = true;
                }
            }
            

            #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
                float loadcell_offset = float(loadcell) - g_loadCellZero;
                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 && 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;
                    }
                }
                if(g_we_are_grasping && potread > 2400 && (mytimer.read_us()/1000.0) > (g_timeAtFirstTouch + 200.0)) {//angledot>-velocity_threshold){
                    //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;
                std::string sstr;
                if(g_we_are_grasping){
                    //D_x = [thetadotdot,thetadot,theta,theta^2,theta^3];
                    g_error_norm[0]+=fabs(float(loadcell_offset)-(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_offset )-(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;}
                    alpha += 0.15;
                    if (alpha > 1.0){alpha = 1.0;}
                    float force_err=loadcell_offset-g_thresh_force[tissue_id];
                    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;
                }
                
                //std::vector<int> data_in=<angle, angledot, angledotdot, loadcell, potread>

                if (!calibrateDone){
                    g_command_corrected = 2800;
                }
                //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,%6.3f,%3.4f,%d\n",mytimer.read_us()/1000.0,g_loadCellZero,forceInNewtons,adc.read());
                
                
            #endif
            
            //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();
        }
        
    }
}