Diff: main.cpp

Revision:

4:e44ac08027bd

Parent:

2:dfeadd6c651c

Child:

5:72e92c721cd5

--- a/main.cpp	Mon Jan 26 04:56:08 2015 +0000
+++ b/main.cpp	Mon Apr 06 21:23:36 2015 +0000
@@ -24,6 +24,7 @@
 // Specific to Dynamixels
 #ifdef USE_DYNAMIXELS
 #include "MX12.h"
+#include "AD7730.h"
 #endif
 
 // Specific to SD Card
@@ -39,6 +40,13 @@
     #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
@@ -46,7 +54,7 @@
 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 led4(LED4);      //Led 4 for debugging purposes
 DigitalOut triggerOut(p11);
 Serial pc(USBTX, USBRX);    //Set up serial connection to pc
 #ifdef USE_BLUETOOTH
@@ -83,7 +91,7 @@
 float g_theta;
 float g_theta_last=0;
 unsigned char g_current_mode=MODE_NULL;
-unsigned char g_current_direction=DIRECTION_NULL;
+jaw_state g_current_direction=STATE_OPEN_HOLD;
 unsigned char g_current_cycle=0;
 
 // Warning, this buffer is large!
@@ -97,16 +105,20 @@
 
 #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
+    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 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
+    #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, LEFT_JAW_DYNAMIXEL_ID,  1000000);
-    MX12 mx12_right_jaw (p28, p27, RIGHT_JAW_DYNAMIXEL_ID, 1000000);
+    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);
     
-    /// Set these to outputs so that they don't interfere with the serial communication
+    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);
@@ -139,6 +151,48 @@
 
 #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){
@@ -152,23 +206,34 @@
         
         // 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);
+        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
-            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;
+            //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();
@@ -177,7 +242,6 @@
             tempSpindleData.cycle=g_current_cycle;
             Buffer.write(tempSpindleData);
             
-            g_current_trajectory_time+=samplingPeriod;
             
             
             moveServoTo(g_theta); // in degrees, son
@@ -235,15 +299,31 @@
     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_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);
+        mx12_right_jaw.Set_Return_Delay_Time(0.050);
         printf("Current ReturnDelay=%f ms\n",mx12_left_jaw.Get_Return_Delay_Time());
-        //mx12_right_jaw.SetReturnDelay(0);
+        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);
@@ -276,6 +356,7 @@
             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("/");
@@ -293,6 +374,7 @@
                 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);
@@ -303,7 +385,7 @@
             collect_data=true;
         }
         
-        if( collect_data && g_current_trajectory_time * g_frequency > g_num_cycles)
+        if( collect_data && g_current_cycle >= g_num_cycles)
         {
             // STOOOOOOOOOP
             collect_data=false;
@@ -328,14 +410,14 @@
         
         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,
+            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("The worst time taken was %d microseconds\n", worst_latency);
+            printf("    %dus\n", worst_latency);
             worst_latency=0;
             AuxSerialTimer.reset();
         }