Thomas Ashworth
For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
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