File content as of revision 4:e44ac08027bd:

#define USE_DYNAMIXELS
//#define USE_BLUETOOTH
#define USE_SD_CARD

// 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 <string>

// Specific to Dynamixels
#ifdef USE_DYNAMIXELS
#include "MX12.h"
#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
#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

#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
float servoAngle;                   //This is the desired servo angle based on the scaled potentiometer value
float potData;                      //This is the value of the potentiometer from Ain.read()
bool collect_data = false;          //This is 

bool keyStrokeFlag = false;         //This is a flag to see if a keystroke has been pressed
char keyStrokeVal;                  //This is a character storing the value of the keystroke

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;

// Warning, this buffer is large!
ServoRingBuffer Buffer;
spindleData tempSpindleData;  //For sending to the buffer

Timer ISRDurationTimer;
Timer AuxSerialTimer;
int worst_latency=0;
int current_latency;

#ifdef USE_DYNAMIXELS
    //Dynamixels can only handle 500Hz for now.  Working on it...
    float samplingPeriod = 0.005;       //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   1121      //This is the closed in encoder counts
    #define OPEN_SERVO_ANGLE_LEFT     2783      //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
    // Dynamixel Object
    MX12 mx12_left_jaw  (p28, p27, p30, p29, LEFT_JAW_DYNAMIXEL_ID,  1000000);
    MX12 mx12_right_jaw (p28, p27, p30, p29, RIGHT_JAW_DYNAMIXEL_ID, 1000000);
    
    AD7730 adc(p9, p26, p11, p12, p25);
    
    /// 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);
#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

// 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);
    
    //
    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(collect_data){
        //led 1 is used as a 'thinking' light, brighter=worse
        led1 = 1;
        led2 = 0;
        triggerOut = 1;
        
        ISRDurationTimer.reset();
        ISRDurationTimer.start();
        
        // 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();
            }
            
            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;
            mx12_right_jaw.coordinated_move(LEFT_JAW_DYNAMIXEL_ID,left_servo, 0, RIGHT_JAW_DYNAMIXEL_ID,right_servo, 0);
            
//            tempSpindleData.myServoData[LEFT_SERVO_INDEX].force  = adc.read();
//            tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos    = mx12_left_jaw.GetRawPosition();
//            tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
//            tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos   = mx12_right_jaw.GetRawPosition();
//            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
        
        //done thinking
        led1 = 0;
        led2 = 1;
        triggerOut = 0;
        
        ISRDurationTimer.stop();
        current_latency=ISRDurationTimer.read_us();
        if(current_latency>worst_latency){
            worst_latency=current_latency;
        }
    }
}


int main() {
    // Crazy fast baud rate!
    pc.baud(921600);
    
    #ifdef USE_BLUETOOTH
    bt.baud(9600);
    #endif
    
    // Attach ISR routines
    potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod
    
    // 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
        mx12_left_jaw.Init();
        //mx12_left_jaw.SetBaud(3000000);
        //mx12_left_jaw.SetBaud(1000000);
        //printf("Current Position=%1.3f\n",mx12_left_jaw.GetPosition());
        mx12_right_jaw.Set_Return_Delay_Time(0.050);
        printf("Current ReturnDelay=%f ms\n",mx12_left_jaw.Get_Return_Delay_Time());
        mx12_left_jaw.Set_Return_Delay_Time(0.050);
        //mx12_left_jaw.Set_Torque_Limit(99.9);
        //mx12_right_jaw.Set_Torque_Limit(99.9);
        mx12_left_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
        mx12_right_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
        mx12_left_jaw.Set_P_Gain(4);
        mx12_right_jaw.Set_P_Gain(4);
        mx12_left_jaw.Set_I_Gain(8);
        mx12_right_jaw.Set_I_Gain(8);
        mx12_left_jaw.Set_Alarm_Shutdown(0x04);
        mx12_right_jaw.Set_Alarm_Shutdown(0x04);
        
        mx12_left_jaw.Dump_OD_to_Serial(pc);
        mx12_right_jaw.Dump_OD_to_Serial(pc);
        
        
        
        adc.setFilter(256 , false, 1);
        adc.start();
    #else
        // Configure Servo for HiTec 422
        myServoLeft.period_ms(20);
        myServoRight.period_ms(20);
    #endif
    
    printf("Setup Complete.\n");
    AuxSerialTimer.start();
    
    while(1)
    {
        // spin in a main loop. serialISR will interrupt it to call serialPot
        
        ///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;
            g_current_mode=MODE_AUTOMATIC;
            #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!
            collect_data=true;
        }
        
        if( collect_data && g_current_cycle >= g_num_cycles)
        {
            // STOOOOOOOOOP
            collect_data=false;
            #ifdef USE_SD_CARD
                // Close the file
                fclose(fp); 
                fp = NULL;
            #endif
        }
        
        // This section of code should run whenever there is free time to print to the screen
        #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
            Buffer.dumpBufferToSerial();
        #endif
        
        if(AuxSerialTimer.read_ms()>100 && collect_data){
            //Send some extra data for GUI purposes
            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\n", worst_latency);
            worst_latency=0;
            AuxSerialTimer.reset();
        }
        
    }
}