Diff: main.cpp

Revision:

10:1a1d52207f59

Parent:

9:1b29cd9ed1ba

Child:

11:8d2455e383ce

--- a/main.cpp	Fri Mar 28 15:32:16 2014 +0000
+++ b/main.cpp	Sat Mar 29 21:22:14 2014 +0000
@@ -180,9 +180,9 @@
 //increase in number 5 rotates gripper
 
     {STORE_POSITION, 85, 10, 0, 175, 100, 0},              // storing position
-    {OIL_RIG1, 164, 90, 90, 52, 100, 0},                 // point laser at oilrig1
-    {OIL_RIG2, 145, 90, 90, 51, 100, 0},                // point laser at oilrig2
-    {OIL_RIG3, 130, 90, 90, 50, 100, 0},                // point laser at oilrig2
+    {OIL_RIG1, 161, 90, 90, 47, 100, 0},                 // point laser at oilrig1
+    {OIL_RIG2, 140, 90, 90, 44, 100, 0},                // point laser at oilrig2
+    {OIL_RIG3, 130, 90, 90, 57, 100, 0},                // point laser at oilrig2
     {DRIVE_POSITION_NOTOOL, 85, 10, 0, 175, 100, 0},    // Drive through course
     {TOOL_1, 95, 64, 97, 79, 0, 0},                  // Look over first tool
     {TOOL_2, 75, 70, 102, 74, 0, 0},                  // Look over second tool
@@ -210,7 +210,7 @@
     /*****************
      INITIALIZATIONS
     *******************/
-    float location[3], current=0;
+    float location[3], current=6;
     int direction[3];
     double distance;
 
@@ -223,12 +223,14 @@
     //Servo initialization
     initServoDriver();
     servoBegin();   //initiates servos to start position
-    //ServoOutputDisable = 0;
+    //servoPosition(0);
+    ServoOutputDisable = 0;
 
     /*******************
     WHILE LOOP FOR TESTING
     ********************/
-    /*while(1) {
+    while(1) {
+        
         pc.scanf("%d %d", &servoNum, &servoAngle);
         if(servoAngle > 175) {
             servoAngle = 175;
@@ -244,20 +246,20 @@
         //pc.printf("Distance %d", distLaser);
         //ledtoggle();
 
-        pc.scanf("%d", &posNum);
-        servoPosition(posNum);
-        wait(5);
+        //pc.scanf("%d", &posNum);
+        //servoPosition(posNum);
+        //wait(5);
         //shape_detected = shapeDetection();
         distLaser = getDistance();
         pc.printf("Distance %d", distLaser);
 
-    }*/
+    }
 
     /********************************
     MAIN WHILE LOOP FOR COMPETITION
     *********************************/
     while(1) {
-        
+
         switch (state) {
 
                 /**************************************************
@@ -268,12 +270,11 @@
                 **************************************************/
             case START :
                 myled1 = 1;
-                wait(5);
+                wait(1);
                 myled1 = 0;
                 state = OILRIG1_POS;
                 break;
 
-
                 /**************************************************
                 *           STAGE 1
                 *
@@ -281,10 +282,19 @@
                 *
                 **************************************************/
             case OILRIG1_POS:                   //aims arm at square oil rig
-                servoPosition(OIL_RIG1);        //position arm to point at first oilrig
-                wait(5);                        //wait for servos to settle
+
+                setServoPulse(3, Arm_Table[OIL_RIG1].claw_arm);
+                setServoPulse(2, 50);
+                setServoPulse(1, Arm_Table[OIL_RIG1].base_arm);
+                setServoPulse(2, Arm_Table[OIL_RIG1].big_arm);
+                setServoPulse(0, Arm_Table[OIL_RIG1].base_rotate);
+                setServoPulse(4, Arm_Table[OIL_RIG1].claw_rotate);
+                setServoPulse(5, Arm_Table[OIL_RIG1].claw_open);
+
+                wait(3);                        //wait for servos to settle
+                
                 fire = fire_checker(OIL_RIG1);  //determines if oil rig is on fire
-
+                pc.printf("FIRE: %d", fire);
                 //determines what tool is needed
                 if (fire == 1) {
                     tool_needed = SQUARE;
@@ -292,13 +302,14 @@
                 } else {
                     state = OILRIG2_POS;
                 }
-                
+
                 break;
 
             case OILRIG2_POS:
                 servoPosition(OIL_RIG2);    //position arm to point at first oilrig
                 wait(1);                    //wait for servos to settle
                 fire = fire_checker(OIL_RIG2);
+                pc.printf("FIRE: %d", fire);
                 if (fire == 1) {
                     tool_needed = TRIANGLE;
                     state = GOTO_TOOLS;
@@ -316,8 +327,15 @@
                 *
                 **************************************************/
             case GOTO_TOOLS:
-                servoPosition(DRIVE_POSITION_NOTOOL);   //drive position without a tool
-                wait(2);                                //wait for servos to settle
+            
+                setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
+                setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
+                setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
+                setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
+                setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
+                setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
+
+                wait(5);                                //wait for servos to settle
 
                 to_tools_section(location, current);
 
@@ -546,15 +564,10 @@
 {
     pc.printf("Setting Initial Position\n\r");
     setServoPulseNo_delay(3, 175);
-    wait(2);
     setServoPulseNo_delay(2, 0);
-    wait(2);
-    setServoPulseNo_delay(1, 10);
-    wait(2);
+    setServoPulseNo_delay(1, 2);
     setServoPulseNo_delay(0, 85);
-    wait(1);
     setServoPulseNo_delay(4, 100);
-    wait(1);
     setServoPulseNo_delay(5, 0);
     setGripper(1);
 }
@@ -600,7 +613,7 @@
         case 1:
             for (int i = 0; i<5; i++) {
                 distLaser = getDistance();
-                pc.printf("L DISTANCE: %d", distLaser);
+                pc.printf("L DISTANCE: %d \n\r", distLaser);
                 if ((distLaser < OILRIG1_MAX)
                         && (distLaser > OILRIG1_MIN)) {
                     fire_detected++;
@@ -612,7 +625,7 @@
         case  2:
             for (int i = 0; i<5; i++) {
                 distLaser = getDistance();
-                pc.printf("L DISTANCE: %d", distLaser);
+                pc.printf("L DISTANCE: %d \n\r", distLaser);
                 if ((distLaser < OILRIG2_MAX)
                         && (distLaser > OILRIG2_MIN)) {
                     fire_detected++;
@@ -632,12 +645,12 @@
                 }
             }
             break;
-        
+
         default:
             for (int i = 0; i<5; i++) {
                 distLaser = getDistance();
                 if ((distLaser < OILRIG1_MAX)
-                        || (distLaser > OILRIG1_MIN)) {
+                        && (distLaser > OILRIG1_MIN)) {
                     fire_detected++;
                 } else {
                     fire_not_detected++;
@@ -683,7 +696,7 @@
 
     location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
-    while(location< 68) {
+    while(location< 70) {
         location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
 
         pid1.setInputLimits(0, set);
@@ -1005,14 +1018,10 @@
     // current position in reference to the starting position
     current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
     pc.printf("current %f \r\n",current);
-     
+
     motors.stopBothMotors();
 
     wait(2);
-    
-    motors.setMotor0Speed(.2*127); //right
-    motors.setMotor1Speed(.2*127); //left
-    while(IR.getIRDistance()>16);
 
     motors.setMotor0Speed(-.2*127); //right
     motors.setMotor1Speed(-.2*127); //left