uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 2:4e082e4c255d
- Parent:
- 1:fe4a0b47ff25
- Child:
- 3:b7b4780a7f6e
diff -r fe4a0b47ff25 -r 4e082e4c255d main.cpp --- 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);