main.cpp

00001 //#define USE_DYNAMIXELS
00002 #define USE_BLUETOOTH
00003 #define USE_SD_CARD
00004 
00005 // We have different modes for different things
00006 #define MODE_MANUAL     1
00007 #define MODE_AUTOMATIC  2
00008 #define MODE_IDLE       3
00009 #define MODE_NULL       0
00010 
00011 // We always want to know if we are closing or opening
00012 #define DIRECTION_CLOSING 1
00013 #define DIRECTION_OPENING 2
00014 #define DIRECTION_SLACK_WATER 3
00015 #define DIRECTION_NULL 0
00016 
00017 // General includes
00018 #include "mbed.h"
00019 #include "ServoRingBuffer.h"
00020 #include "ram_test.h"
00021 #include "Serial_Receive.h"
00022 #include <string>
00023 
00024 // Specific to Dynamixels
00025 #ifdef USE_DYNAMIXELS
00026 #include "MX12.h"
00027 #include "AD7730.h"
00028 #endif
00029 
00030 // Specific to SD Card
00031 #ifdef USE_SD_CARD
00032 #include "SDFileSystem.h"
00033 #endif
00034 
00035 // Everyone should know pi...
00036 #ifndef M_PI
00037     #define M_PI    3.14159265358979323846  /* pi */
00038 #endif
00039 #ifndef M_PI_2
00040     #define M_PI_2  1.57079632679489661923  /* pi/2 */
00041 #endif
00042 
00043 // Create enum for the Jaw state (Closing, hold, opening)
00044 enum jaw_state{
00045     STATE_CLOSING=0,
00046     STATE_CLOSE_HOLD=1,
00047     STATE_OPENING=2,
00048     STATE_OPEN_HOLD=3
00049 };
00050 
00051 
00052 // Define pins and interrupts
00053 Ticker potISR;              //Define a recurring timer interrupt
00054 DigitalOut led1(LED1);      //Led 1 for debugging purposes
00055 DigitalOut led2(LED2);      //Led 2 for debugging purposes
00056 DigitalOut led3(LED3);      //Led 3 for debugging purposes
00057 //DigitalOut led4(LED4);      //Led 4 for debugging purposes
00058 DigitalOut triggerOut(p11);
00059 Serial pc(USBTX, USBRX);    //Set up serial connection to pc
00060 #ifdef USE_BLUETOOTH
00061 Serial bt(p13,p14);         //Set up serial connection to bluetooth adapter
00062 #endif
00063 
00064 AnalogIn AinLeftForce(p16);          //Set up potentiometer on pin 20
00065 AnalogIn AinRightForce(p15);          //Set up potentiometer on pin 20
00066 
00067 #ifdef USE_SD_CARD
00068     // Attach SD card
00069     SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
00070     FILE *fp = NULL;
00071     #define SAMPLES_PER_FILE 10000
00072 #endif
00073 
00074 // Dummy variable for debugging
00075 unsigned int global_count=0;
00076 float max_percent_full=0;
00077 
00078 // Define variables for the program
00079 float servoAngle;                   //This is the desired servo angle based on the scaled potentiometer value
00080 float potData;                      //This is the value of the potentiometer from Ain.read()
00081 bool collect_data = false;          //This is 
00082 
00083 bool keyStrokeFlag = false;         //This is a flag to see if a keystroke has been pressed
00084 char keyStrokeVal;                  //This is a character storing the value of the keystroke
00085 
00086 char g_tissue_type_name[32];
00087 float g_frequency;
00088 int g_max_force;
00089 int g_num_cycles;
00090 float g_current_trajectory_time;
00091 float g_theta;
00092 float g_theta_last=0;
00093 unsigned char g_current_mode=MODE_NULL;
00094 jaw_state g_current_direction=STATE_OPEN_HOLD;
00095 unsigned char g_current_cycle=0;
00096 
00097 // Warning, this buffer is large!
00098 ServoRingBuffer Buffer;
00099 spindleData tempSpindleData;  //For sending to the buffer
00100 
00101 Timer ISRDurationTimer;
00102 Timer AuxSerialTimer;
00103 int worst_latency=0;
00104 int current_latency;
00105 
00106 #ifdef USE_DYNAMIXELS
00107     //Dynamixels can only handle 500Hz for now.  Working on it...
00108     float samplingPeriod = 0.005;       //This is the sampling period for the timer interrupt
00109     #define LEFT_JAW_DYNAMIXEL_ID        3
00110     #define RIGHT_JAW_DYNAMIXEL_ID       4
00111     #define CLOSED_SERVO_ANGLE_LEFT   1121      //This is the closed in encoder counts
00112     #define OPEN_SERVO_ANGLE_LEFT     2783      //This is the open in encoder counts
00113     #define CLOSED_SERVO_ANGLE_RIGHT  3259      //This is the closed in encoder counts
00114     #define OPEN_SERVO_ANGLE_RIGHT    1486      //This is the open in encoder counts
00115     // Dynamixel Object
00116     MX12 mx12_left_jaw  (p28, p27, p30, p29, LEFT_JAW_DYNAMIXEL_ID,  1000000);
00117     MX12 mx12_right_jaw (p28, p27, p30, p29, RIGHT_JAW_DYNAMIXEL_ID, 1000000);
00118     
00119     AD7730 adc(p9, p26, p11, p12, p25);
00120     
00121     /// Set these to inputs so that they don't interfere with the serial communication
00122     DigitalIn nullOut1(p21);
00123     DigitalIn nullOut2(p22);
00124     DigitalIn nullOut3(p23);
00125     DigitalIn nullOut4(p24);
00126 #else
00127     float samplingPeriod = 0.001;       //This is the sampling period for the timer interrupt
00128     #define SERVO_DEGREE_0    900       //This is the pulse width value for HiTEC-422 in microseconds to turn 0 degrees
00129     #define SERVO_DEGREE_180  2100      //This is the pulse width value for HiTEC-422 in microseconds to turn 180 degrees
00130     #define MIN_SERVO_ANGLE   0.0       //This is the minimum servo angle in degrees
00131     #define MAX_SERVO_ANGLE   180.0     //This is the maximum servo angle in degrees
00132     #define MIN_SERVO_ANGLE_Da_VINCI   20.0       //This is the minimum servo angle in degrees
00133     #define MAX_SERVO_ANGLE_Da_VINCI   100.0     //This is the maximum servo angle in degrees
00134     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
00135     const float servoOffset = SERVO_DEGREE_0/1000000.0;                                                                 //This is the pulsewidth value (in seconds) that corresponds to 0 degrees (i.e.-the offset)
00136     
00137     PwmOut myServoLeft(p21);        //Set up servo on pin 21
00138     PwmOut myServoRight(p22);        //Set up servo on pin 22
00139     AnalogIn AinLeftPosition(p20);          //Set up potentiometer on pin 20
00140     AnalogIn AinRightPosition(p19);          //Set up potentiometer on pin 20
00141 
00142 
00143     // Function moveServoTo: Convert a degree value to pulsewidth for Servo
00144     void moveServoTo(float angle) {
00145         // Make sure none of the user input falls outside of min and max angle limits
00146         if(     angle < MIN_SERVO_ANGLE){angle = MIN_SERVO_ANGLE;}
00147         else if(angle > MAX_SERVO_ANGLE){angle = MAX_SERVO_ANGLE;}
00148         myServoLeft.pulsewidth(servoOffset + servoConversion*(180-angle));
00149         myServoRight.pulsewidth(servoOffset + servoConversion*(angle));
00150     }
00151 
00152 #endif
00153 
00154 // Function trapezoidalTrajectory: Function that takes in a time (float in seconds) and outputs a float (0 to 1) that corresponds to a trapezoidal trajectory
00155 float trapezoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) {
00156     // Define variables specific to this function
00157     float y_trapezoid = 0.0;
00158     float timeMod;
00159     float modifiedFrequency = g_frequency/2.0;
00160     float period = 1/modifiedFrequency;
00161     cycle_num=t*modifiedFrequency;
00162     
00163     // Take the time and mod it with the period to be able to break up each cycle into 4 piecewise sections
00164     timeMod = fmodf(t,period);
00165     
00166     //
00167     if (timeMod < period/4.0)
00168     {
00169         y_trapezoid = (-4.0/period)*(timeMod)+1.0;
00170         state = STATE_CLOSING;
00171     }
00172     else if (timeMod >= period/4.0 && timeMod < period/2.0)
00173     {
00174         y_trapezoid = 0.0;
00175         state = STATE_CLOSE_HOLD;
00176     }
00177     else if (timeMod >= period/2.0 && timeMod < 3*period/4.0)
00178     {
00179         y_trapezoid = (4.0/period)*(timeMod)-2;
00180         state = STATE_OPENING;
00181     }
00182     else if (timeMod >= 3*period/4.0)
00183     {
00184         y_trapezoid = 1.0;
00185         state = STATE_OPEN_HOLD;
00186     }
00187 
00188     return y_trapezoid; 
00189 }
00190 
00191 void sinusoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) {
00192     //Fill me with SCIENCE!!!
00193 }
00194 
00195 
00196 // Function timerISRFunction: Timer ISR function to collect data and write to ring buffer
00197 void timerISRFunction() {
00198     if(collect_data){
00199         //led 1 is used as a 'thinking' light, brighter=worse
00200         led1 = 1;
00201         led2 = 0;
00202         triggerOut = 1;
00203         
00204         ISRDurationTimer.reset();
00205         ISRDurationTimer.start();
00206         
00207         // Warning, this calculation is in the ISR and as such is probably slower than we would prefer.  
00208         // @todo The math could certainly be optimized with some precalculated constants.  Lookup tables are faster than sin()
00209         float percent=trapezoidalTrajectory(g_current_trajectory_time,g_current_direction,g_current_cycle);
00210         g_current_trajectory_time+=samplingPeriod;
00211         
00212         
00213         //float angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2;
00214         //g_current_direction=(cos(angle)<0);
00215         //g_current_cycle=(angle+M_PI_2)/(2.0*M_PI);
00216         
00217 
00218         #ifdef USE_DYNAMIXELS
00219             //float percent=(sin(angle)+1)/2.0;
00220             if(adc.isReady()){
00221                 adc.interruptRead();
00222             }
00223             
00224             short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
00225             short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
00226             mx12_right_jaw.coordinated_move(LEFT_JAW_DYNAMIXEL_ID,left_servo, 0, RIGHT_JAW_DYNAMIXEL_ID,right_servo, 0);
00227             
00228 //            tempSpindleData.myServoData[LEFT_SERVO_INDEX].force  = adc.read();
00229 //            tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos    = mx12_left_jaw.GetRawPosition();
00230 //            tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
00231 //            tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos   = mx12_right_jaw.GetRawPosition();
00232 //            tempSpindleData.direction=g_current_direction;
00233 //            tempSpindleData.cycle=g_current_cycle;
00234 //            Buffer.write(tempSpindleData);
00235         #else   
00236             g_theta=(1.0-percent)*(MAX_SERVO_ANGLE_Da_VINCI-MIN_SERVO_ANGLE_Da_VINCI)+MIN_SERVO_ANGLE_Da_VINCI;
00237             tempSpindleData.myServoData[LEFT_SERVO_INDEX].force  = AinLeftForce.read_u16();
00238             tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos    = AinLeftPosition.read_u16();
00239             tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
00240             tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos   = AinRightPosition.read_u16();
00241             tempSpindleData.direction=g_current_direction;
00242             tempSpindleData.cycle=g_current_cycle;
00243             Buffer.write(tempSpindleData);
00244             
00245             
00246             
00247             moveServoTo(g_theta); // in degrees, son
00248         #endif
00249         
00250         //done thinking
00251         led1 = 0;
00252         led2 = 1;
00253         triggerOut = 0;
00254         
00255         ISRDurationTimer.stop();
00256         current_latency=ISRDurationTimer.read_us();
00257         if(current_latency>worst_latency){
00258             worst_latency=current_latency;
00259         }
00260     }
00261 }
00262 
00263 
00264 int main() {
00265     // Crazy fast baud rate!
00266     pc.baud(921600);
00267     
00268     #ifdef USE_BLUETOOTH
00269     bt.baud(9600);
00270     #endif
00271     
00272     // Attach ISR routines
00273     potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod
00274     
00275     // Some debug info:
00276     //DisplayRAMBanks();
00277     //printf ("System clock = %d\r\n", SystemCoreClock);
00278     
00279     pc.printf("\n\n\n");
00280     pc.printf("----------------------------------\n");
00281     pc.printf("|                                |\n");
00282     pc.printf("|     Welcome to our mbed!       |\n");
00283     pc.printf("|                                |\n");
00284     pc.printf("| John and Trevor, Proprietors   |\n");
00285     pc.printf("|                                |\n");
00286     pc.printf("----------------------------------\n");
00287     pc.printf("                ||\n");
00288     pc.printf("                ||\n");
00289     pc.printf("                ||     _\n");
00290     pc.printf("                ||   _( )_\n");
00291     pc.printf("                ||  (_(#)_)\n");
00292     pc.printf("                ||    (_)\\\n");
00293     pc.printf("                ||        | __\n");
00294     pc.printf("   \\    /   |   ||        |/_/\n");
00295     pc.printf(" / | | /  / / | || | \\ \\  |    /  \n");
00296     pc.printf("/  / \\ \\ / / /  || / | /  /  \\ \\ \n");
00297     pc.printf("#################################\n");
00298     pc.printf("#################################\n");
00299     pc.printf("\n\n");
00300     
00301     #ifdef USE_DYNAMIXELS
00302         mx12_left_jaw.Init();
00303         //mx12_left_jaw.SetBaud(3000000);
00304         //mx12_left_jaw.SetBaud(1000000);
00305         //printf("Current Position=%1.3f\n",mx12_left_jaw.GetPosition());
00306         mx12_right_jaw.Set_Return_Delay_Time(0.050);
00307         printf("Current ReturnDelay=%f ms\n",mx12_left_jaw.Get_Return_Delay_Time());
00308         mx12_left_jaw.Set_Return_Delay_Time(0.050);
00309         //mx12_left_jaw.Set_Torque_Limit(99.9);
00310         //mx12_right_jaw.Set_Torque_Limit(99.9);
00311         mx12_left_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
00312         mx12_right_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
00313         mx12_left_jaw.Set_P_Gain(4);
00314         mx12_right_jaw.Set_P_Gain(4);
00315         mx12_left_jaw.Set_I_Gain(8);
00316         mx12_right_jaw.Set_I_Gain(8);
00317         mx12_left_jaw.Set_Alarm_Shutdown(0x04);
00318         mx12_right_jaw.Set_Alarm_Shutdown(0x04);
00319         
00320         mx12_left_jaw.Dump_OD_to_Serial(pc);
00321         mx12_right_jaw.Dump_OD_to_Serial(pc);
00322         
00323         
00324         
00325         adc.setFilter(256 , false, 1);
00326         adc.start();
00327     #else
00328         // Configure Servo for HiTec 422
00329         myServoLeft.period_ms(20);
00330         myServoRight.period_ms(20);
00331     #endif
00332     
00333     printf("Setup Complete.\n");
00334     AuxSerialTimer.start();
00335     
00336     while(1)
00337     {
00338         // spin in a main loop. serialISR will interrupt it to call serialPot
00339         
00340         ///This checks for any new serial bytes, and returns true if
00341         ///we have an entire packet ready.  The new packet will live
00342         ///in newData.
00343         if(
00344         #ifdef USE_BLUETOOTH
00345             receivePacket(bt)
00346         #else
00347             receivePacket(pc)
00348         #endif
00349         )
00350         {
00351             // < Tissue Type (string), Frequency Value (Hz) (int), Force Max (N) (int), # Cycles (in) >
00352             //<date/tissue/time,2,3,4>
00353             //g_tissue_type_name=tissue_type_name;
00354             std::string file_name_in=inString.substr(0, inString.find(","));
00355             g_frequency=newData[1]/10.0; // Since all we send are ints
00356             g_max_force=newData[2];
00357             g_num_cycles=newData[3];
00358             g_current_trajectory_time=0;
00359             g_current_cycle=0;
00360             g_current_mode=MODE_AUTOMATIC;
00361             #ifdef USE_SD_CARD
00362                 int first_slash=file_name_in.find("/");
00363                 std::string new_dir="/sd/"+file_name_in.substr(0, first_slash);
00364                 std::string new_subdir="/sd/"+file_name_in.substr(0, file_name_in.find("/",first_slash+1));
00365                 mkdir(new_dir.c_str(), 0777);
00366                 mkdir(new_subdir.c_str(), 0777);
00367                 std::string file_name="/sd/"+file_name_in+".csv";
00368                 //pc.printf("subdir=\"%s\"\n",file_name.c_str());
00369                 fp = fopen(file_name.c_str(), "w");
00370                 //FILE *fp = fopen("/sd/data/sdtest.txt", "w");
00371                 if(fp == NULL) {
00372                     error("Could not open file for write\n");
00373                 }
00374                 fprintf(fp, "%%Starting New Trajectory\n");
00375                 fprintf(fp, "%%File Name=\"%s\"\n",file_name.c_str());
00376                 fprintf(fp, "%%Current Mode=AUTOMATIC\n");
00377                 fprintf(fp, "%%Trajectory Type=TRAPEZOIDAL\n");
00378                 fprintf(fp, "%%Frequency=%f Hz\n",g_frequency);
00379                 fprintf(fp, "%%Max Force=%f ??\n",g_max_force);
00380                 fprintf(fp, "%%Num Cycles=%d\n",g_num_cycles);
00381                 fprintf(fp, "%%Re. Direction: ,Closing=%d,Opening=%d,Undef=%d\n", DIRECTION_CLOSING , DIRECTION_OPENING , DIRECTION_SLACK_WATER );
00382                 fprintf(fp, "%%PositionLeft,ForceLeft,PositionRight,ForceRight,Time(ms),Direction,CycleNum\n");
00383             #endif
00384             // We are go-times!
00385             collect_data=true;
00386         }
00387         
00388         if( collect_data && g_current_cycle >= g_num_cycles)
00389         {
00390             // STOOOOOOOOOP
00391             collect_data=false;
00392             #ifdef USE_SD_CARD
00393                 // Close the file
00394                 fclose(fp); 
00395                 fp = NULL;
00396             #endif
00397         }
00398         
00399         // This section of code should run whenever there is free time to print to the screen
00400         #ifdef USE_SD_CARD
00401         if(fp != NULL) {
00402             // Only write to SD if there is a valid file handle
00403             led3 = 1;
00404             Buffer.dumpBufferToSD(fp);
00405             led3 = 0;
00406         }
00407         #else
00408             Buffer.dumpBufferToSerial();
00409         #endif
00410         
00411         if(AuxSerialTimer.read_ms()>100 && collect_data){
00412             //Send some extra data for GUI purposes
00413             printf("<%d,%d,%d,%d,%d,%d,%d>  ",tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos,
00414                                               tempSpindleData.myServoData[LEFT_SERVO_INDEX].force,
00415                                               tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos,
00416                                               tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force,
00417                                               tempSpindleData.time,
00418                                               tempSpindleData.direction,
00419                                               tempSpindleData.cycle);
00420             printf("    %dus\n", worst_latency);
00421             worst_latency=0;
00422             AuxSerialTimer.reset();
00423         }
00424         
00425     }
00426 }