Diff: main.cpp

Revision:

2:dfeadd6c651c

Parent:

0:8a555873b7d3

Child:

4:e44ac08027bd

--- a/main.cpp	Sun Sep 21 18:19:20 2014 +0000
+++ b/main.cpp	Mon Jan 26 04:38:06 2015 +0000
@@ -1,20 +1,344 @@
+#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"
- 
-Timeout flipper;
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
- 
-void flip() {
-    led2 = !led2;
+#include "ServoRingBuffer.h"
+#include "ram_test.h"
+#include "Serial_Receive.h"
+#include <string>
+
+// Specific to Dynamixels
+#ifdef USE_DYNAMIXELS
+#include "MX12.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
+
+
+
+// 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;
+unsigned char g_current_direction=DIRECTION_NULL;
+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.002;       //This is the sampling period for the timer interrupt
+    #define LEFT_JAW_DYNAMIXEL_ID        3
+    #define RIGHT_JAW_DYNAMIXEL_ID       4
+    #define MIN_SERVO_ANGLE_Da_VINCI  1600      //This is the open in encoder counts
+    #define MAX_SERVO_ANGLE_Da_VINCI  3200      //This is the closed in encoder counts
+    // Dynamixel Object
+    MX12 mx12_left_jaw  (p28, p27, LEFT_JAW_DYNAMIXEL_ID,  1000000);
+    MX12 mx12_right_jaw (p28, p27, RIGHT_JAW_DYNAMIXEL_ID, 1000000);
+    
+    /// Set these to outputs 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 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 angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2;
+        g_theta=(sin(angle)+1)/2.0*(MAX_SERVO_ANGLE_Da_VINCI-MIN_SERVO_ANGLE_Da_VINCI)+MIN_SERVO_ANGLE_Da_VINCI;
+        g_current_direction=(cos(angle)<0);
+        g_current_cycle=(angle+M_PI_2)/(2.0*M_PI);
+
+        #ifdef USE_DYNAMIXELS
+            tempSpindleData.myServoData[LEFT_SERVO_INDEX].force  = AinLeftForce.read_u16();
+            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);
+            //mx12_right_jaw.write_short(MX12_REG_GOAL_POSITION,short(g_theta));
+            //mx12_right_jaw.coordinated_move(LEFT_JAW_DYNAMIXEL_ID,(short)g_theta, 175, RIGHT_JAW_DYNAMIXEL_ID,(short)g_theta, 175);
+            g_current_trajectory_time+=samplingPeriod;
+        #else   
+            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);
+            
+            g_current_trajectory_time+=samplingPeriod;
+            
+            
+            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() {
-    led2 = 1;
-    flipper.attach(&flip, 2.0); // setup flipper to call flip after 2 seconds
- 
-    // spin in a main loop. flipper will interrupt it to call flip
-    while(1) {
-        led1 = !led1;
-        wait(0.2);
+    // 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.SetBaud(3000000);
+        //mx12_left_jaw.SetBaud(1000000);
+        //printf("Current Position=%1.3f\n",mx12_left_jaw.GetPosition());
+        mx12_left_jaw.Set_Return_Delay_Time(0.002);
+        mx12_right_jaw.Set_Return_Delay_Time(0.002);
+        mx12_left_jaw.Set_Torque_Limit(30);
+        mx12_right_jaw.Set_Torque_Limit(30);
+        printf("Current ReturnDelay=%f ms\n",mx12_left_jaw.Get_Return_Delay_Time());
+        //mx12_right_jaw.SetReturnDelay(0);
+    #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_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, "%%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_trajectory_time * g_frequency > 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>\n",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("The worst time taken was %d microseconds\n", worst_latency);
+            worst_latency=0;
+            AuxSerialTimer.reset();
+        }
+        
     }
 }
\ No newline at end of file