UH Robotics
uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of
theRobot
by 
Thomas Ashworth
Diff: main.cpp
- Revision:
- 9:1b29cd9ed1ba
- Parent:
- 8:77a57909aa15
- Child:
- 10:1a1d52207f59
--- a/main.cpp Thu Mar 27 23:16:02 2014 +0000
+++ b/main.cpp Fri Mar 28 15:32:16 2014 +0000
@@ -75,9 +75,9 @@
//Oil Rig distance thresholds
#define OILRIG1_MAX 3000
#define OILRIG1_MIN 1000
-#define OILRIG2_MAX 3000
+#define OILRIG2_MAX 5000
#define OILRIG2_MIN 1000
-#define OILRIG3_MAX 3000
+#define OILRIG3_MAX 5000
#define OILRIG3_MIN 1000
//for servo normalization
@@ -179,11 +179,11 @@
//increase in number 5 rotates gripper
- {STORE_POSITION, 85, 5, 5, 175, 100, 0}, // storing position
- {OIL_RIG1, 163, 90, 90, 48, 100, 0}, // point laser at oilrig1
- {OIL_RIG2, 144, 90, 90, 47, 100, 0}, // point laser at oilrig2
+ {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
- {DRIVE_POSITION_NOTOOL, 55, 70, 102, 74, 0, 0}, // Drive through course
+ {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
{TOOL_3, 55, 70, 102, 74, 0, 0}, // Look over third tool
@@ -214,6 +214,7 @@
int direction[3];
double distance;
+
pc.baud(115200);
//Laser Range Finder Initialization
lrf_baudCalibration();
@@ -227,7 +228,7 @@
/*******************
WHILE LOOP FOR TESTING
********************/
- while(1) {
+ /*while(1) {
pc.scanf("%d %d", &servoNum, &servoAngle);
if(servoAngle > 175) {
servoAngle = 175;
@@ -237,22 +238,26 @@
servoAngle = 90;
}
setServoPulse(servoNum, servoAngle);
-
- //ledtoggle();
- //pc.scanf("%d", &posNum);
- //servoPosition(posNum);
- //wait(3);
//shape_detected = shapeDetection();
//distLaser = getDistance();
//pc.printf("Distance %d", distLaser);
+ //ledtoggle();
- }
+ 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) {
/**************************************************
@@ -277,7 +282,7 @@
**************************************************/
case OILRIG1_POS: //aims arm at square oil rig
servoPosition(OIL_RIG1); //position arm to point at first oilrig
- wait(2); //wait for servos to settle
+ wait(5); //wait for servos to settle
fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
//determines what tool is needed
@@ -287,6 +292,7 @@
} else {
state = OILRIG2_POS;
}
+
break;
case OILRIG2_POS:
@@ -300,6 +306,7 @@
tool_needed = CIRCLE;
state = GOTO_TOOLS;
}
+ pc.printf("tool needed: %d", tool_needed);
break;
/**************************************************
@@ -310,15 +317,14 @@
**************************************************/
case GOTO_TOOLS:
servoPosition(DRIVE_POSITION_NOTOOL); //drive position without a tool
- wait(1); //wait for servos to settle
-
-//****************************************************//
+ wait(2); //wait for servos to settle
to_tools_section(location, current);
state = IDENTIFY_TOOLS;
+ pc.printf("YES!!!!!");
+ while(1);
break;
- while(1) {}
/**************************************************
* STAGE 3
*
@@ -594,33 +600,39 @@
case 1:
for (int i = 0; i<5; i++) {
distLaser = getDistance();
+ pc.printf("L DISTANCE: %d", distLaser);
if ((distLaser < OILRIG1_MAX)
- || (distLaser > OILRIG1_MIN)) {
+ && (distLaser > OILRIG1_MIN)) {
fire_detected++;
} else {
fire_not_detected++;
}
}
+ break;
case 2:
for (int i = 0; i<5; i++) {
distLaser = getDistance();
+ pc.printf("L DISTANCE: %d", distLaser);
if ((distLaser < OILRIG2_MAX)
- || (distLaser > OILRIG2_MIN)) {
+ && (distLaser > OILRIG2_MIN)) {
fire_detected++;
} else {
fire_not_detected++;
}
}
+ break;
case 3:
for (int i = 0; i<5; i++) {
distLaser = getDistance();
if ((distLaser < OILRIG3_MAX)
- || (distLaser > OILRIG3_MIN)) {
+ && (distLaser > OILRIG3_MIN)) {
fire_detected++;
} else {
fire_not_detected++;
}
}
+ break;
+
default:
for (int i = 0; i<5; i++) {
distLaser = getDistance();
@@ -631,6 +643,7 @@
fire_not_detected++;
}
}
+ break;
}
if (fire_detected > fire_not_detected) {
@@ -670,7 +683,7 @@
location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
- while(location< 73) {
+ while(location< 68) {
location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
pid1.setInputLimits(0, set);
@@ -910,7 +923,7 @@
rightEncoder.reset();
motors.setMotor0Speed(0.2*127); //right
motors.setMotor1Speed(0.2*127); //left
- while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0) {
+ while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0) {
preLeft=leftEncoder.getPulses();
preRight=rightEncoder.getPulses();
wait_ms(100);
@@ -929,7 +942,7 @@
leftEncoder.reset();
rightEncoder.reset();
*/
-// while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
+// while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0){
/* preLeft=leftEncoder.getPulses();
preRight=rightEncoder.getPulses();
pc.printf("second while left %d right %d \r\n", preLeft, preRight);
@@ -971,8 +984,8 @@
motors.setMotor1Speed(.25*127); //left
if(section == TOOLS || section == MID) {
- while(IR.getDistance() > 20 ) {
- //pc.printf("IR %f\r\n", IR.getDistance());
+ while(IR.getIRDistance() > 20 ) {
+ //pc.printf("IR %f\r\n", IR.getIRDistance());
//pc.printf("third while left %d right %d \r\n", preLeft, preRight);
}
} else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
@@ -992,12 +1005,19 @@
// 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();
- //Tool aquiring
- //wait(2);
- // After tool is aquired
+ 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
+ wait_ms(5);
+ motors.stopBothMotors();
}
@@ -1049,7 +1069,7 @@
motors.begin();
- if(IR.getDistance() > 20) return;
+ if(IR.getIRDistance() > 20) return;
alignWithWall(MID);
@@ -1087,7 +1107,7 @@
motors.begin();
- if(IR.getDistance() > 20) return;
+ if(IR.getIRDistance() > 20) return;
alignWithWall(MID);
wall_follow2(LEFT,FORWARD,MID, current);
@@ -1114,11 +1134,12 @@
}
-void ledtoggle(void){
+void ledtoggle(void)
+{
pwm.setPWM(9, 0, 4094);
wait(2);
pwm.setPWM(9, 0, 0);
- }
+}