Diff: main.cpp

Revision:

2:4e082e4c255d

Parent:

1:fe4a0b47ff25

Child:

3:587441455259

--- a/main.cpp	Mon Mar 03 21:37:24 2014 +0000
+++ b/main.cpp	Mon Mar 03 22:30:34 2014 +0000
@@ -13,6 +13,17 @@
 #define STORE_POSITION  0
 #define OIL_RIG1        1
 #define OIL_RIG2        2
+#define OIL_RIG3        3
+#define IMG_SHAPE_1     4
+#define IMG_SHAPE_2     5
+#define IMG_SHAPE_3     6
+#define GRASP_SHAPE_1   7
+#define GRASP_SHAPE_2   8
+#define GRASP_SHAPE_3   9
+#define INSERT_TOOL_1   10
+#define INSERT_TOOL_2   11
+#define INSERT_TOOL_3   11
+
 
 void servoBegin(void);
 void initServoDriver(void);
@@ -35,8 +46,13 @@
    // POSITION ODER:
    // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
       
-    {STORE_POSITION, 2300, 500, 600, 2450, 2450, 0, 0}, // storing position
-    {OIL_RIG2, 1200, 1400, 1900, 900, 900, 0, 0} // point laser at oilrig2
+    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
+    {OIL_RIG1, 1500, 1400, 1900, 900, 900, 0, 0}, // point laser at oilrig2
+    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
+    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
+    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
+    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
+    {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
 
 }; 
 
@@ -45,21 +61,28 @@
 /*****************
  INITIALIZATIONS
 *******************/
+//pc.baud(115200); 
+//Laser Range Finder Initialization
 //lrf_baudCalibration();
+ 
+//Servo initialization
 initServoDriver();
-servoBegin();
-//servoPosition(STORE_POSITION);
-//ServoOutputDisable = 0;
+servoBegin(); // initiates servos to start position
+ServoOutputDisable = 0;
+
 while(1){
         if(pc.readable()){
-            //pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
-             pc.scanf("%d %d", &servoNum, &pulseWidth);
+            /*
+             pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
              setServoPulse(servoNum, pulseWidth);
+            */
+            pc.scanf("%d", &posNum);
+            servoPosition(posNum);
+            
             
         }
 }
 
-
 /**************************************************
 *     FIRST STAGE 
 *               
@@ -141,8 +164,8 @@
     
 void servoPosition(int set){   
             //moves to current position
+            setServoPulse(0, Arm_Table[set].base_rotate);
             setServoPulse(1, Arm_Table[set].base_arm);
-            setServoPulse(0, Arm_Table[set].base_rotate);
             setServoPulse(2, Arm_Table[set].lil_arm);
             setServoPulse(3, Arm_Table[set].big_arm);
             setServoPulse(4, Arm_Table[set].claw_arm);