Thomas Ashworth
For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Diff: main.cpp
- Revision:
- 20:55dcff40c5d9
- Parent:
- 19:d4d967a885dc
- Child:
- 21:0907e1f5e16c
--- a/main.cpp Fri Apr 04 01:09:49 2014 +0000
+++ b/main.cpp Sat Apr 05 07:26:15 2014 +0000
@@ -10,6 +10,11 @@
#include "LPC17xx.h"
#include "Sharp.h"
+
+#define PI 3.14159
+
+
+
/* Navigation Definitions */
#define PIN_TRIGGERL (p12)
#define PIN_ECHOL (p11)
@@ -20,6 +25,7 @@
#define DIST_PER_PULSE (0.01054225722682)
#define MTRS_TO_INCH (39.3701)
#define MAX_SPEED (0.3*127)
+#define MAX_SPEED1 (0.25*127)
#define PPR (4331/4)
#define LEFT (1)
#define RIGHT (0)
@@ -33,6 +39,7 @@
#define RETURN (4)
#define FAR (1)
+
//States
#define START 0
#define OILRIG1_POS 1
@@ -107,7 +114,6 @@
Serial pc(USBTX,USBRX); //USB Comm
Adafruit_PWMServoDriver pwm(p28,p27); //pwm(SDA,SCL) - Servo Control PWM
-DigitalOut ServoOutputDisable(p8); //Servo Control Output Enable/Disable
extern Serial lrf; //Laser Range Finder lrf(p13,p14)
//Hardware Initialization
//Serial bt(p13,p14);
@@ -158,6 +164,10 @@
void alignWithWall(int section);
void UntilWall(int dir);
+
+float normd(int* pop, int count, int threshold);
+int Xadjust(int tool);
+
extern "C" void mbed_reset();
/************
@@ -165,7 +175,7 @@
*************/
int state = START;
int fire = 0;
-int tool_needed = 0;
+int tool_needed = 1;
int shape_detected = 0;
float range, range2, pid_return;
int num, input;
@@ -199,8 +209,8 @@
//increase in number 5 rotates gripper
{STORE_POSITION, 85, 10, 0, 165, 175, 0}, // storing position
- {OIL_RIG1, 162, 20, 60, 50, 175, 0}, // point laser at oilrig1
- {OIL_RIG2, 165, 20, 60, 50, 175, 0}, // point laser at oilrig2
+ {OIL_RIG1, 156, 20, 60, 47, 175, 0}, // point laser at oilrig1
+ {OIL_RIG2, 162, 20, 60, 47, 175, 0}, // point laser at oilrig2
{OIL_RIG3, 130, 90, 90, 52, 175, 0}, // point laser at oilrig2
{DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 175, 0}, // Drive through course
{TOOL_1, 101, 50, 80, 113, 90, 0}, // Look over first tool
@@ -246,11 +256,6 @@
-
-
-
-
-
int main()
{
@@ -282,6 +287,8 @@
MAIN WHILE LOOP FOR COMPETITION
*********************************/
+
+
while(1) {
if(button_start == 1) {
@@ -380,7 +387,7 @@
setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
wait(3); //wait for servos to settle
- slightMove(FORWARD,3100);
+ slightMove(FORWARD,3350);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
state = IDENTIFY_TOOLS;
@@ -397,7 +404,7 @@
**************************************************/
case IDENTIFY_TOOLS:
- wait(5);
+ //wait(5);
servoPosition(TOOL_2); //arm/camera looks over tool
wait(5); //wait for servos to settle
@@ -428,51 +435,17 @@
}
}*/
-
- for(int i = 0; i < 4; i++) {
- shape_detected = shapeDetection();
- wait(2);
- if(get_com_x() > X_CENTER ) {
- deltaX = get_com_x()-X_CENTER;
- setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) );
- Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
- }
- if(get_com_x() < X_CENTER) {
- deltaX = get_com_x()-X_CENTER;
- setServoPulse2(0,Arm_Table[TOOL_2].base_rotate += (deltaX/60.0) );
- Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
- }
- }
-
-
- /*
-
- while( shape_alignX_done == 0) {
- wait(1);
- shape_detected = shapeDetection();
-
- pc.printf("X - Adjust to center tool\n\r");
- if(get_com_x() > 95) {
- setServoPulse(0,Arm_Table[TOOL_2].base_rotate+=1);
-
- } else if(get_com_x() < 65) {
- setServoPulse(0,Arm_Table[TOOL_2].base_rotate-=1);
-
- } else {
- shape_alignX_done = 1;
- }
- }*/
-
+ // aveArea = sumArea/count;
//printImageToFile(BINARY);
//either goes to aquire the tool or look at the next shape
- if(shape_detected == tool_needed) {
+ if(Xadjust(TOOL_2) == tool_needed) {
+ //printImageToFile(BINARY);
state = AQUIRE_TOOL2;
break;
} else {
- slightMove(FORWARD,40);
+ //printImageToFile(BINARY);
+ slightMove(FORWARD,60);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
-
-
servoPosition(TOOL_1);
wait(5); //wait for servos to settle
shape_alignX_done = 0;
@@ -499,41 +472,7 @@
}
}*/
-
- for(int i = 0; i < 4; i++) {
- shape_detected = shapeDetection();
- wait(2);
- if (get_com_x() > X_CENTER) {
- deltaX = get_com_x()-X_CENTER;
- setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) );
- Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate;
- }
- if (get_com_x() < X_CENTER) {
- deltaX = get_com_x()-X_CENTER;
- setServoPulse2(0,Arm_Table[TOOL_1].base_rotate += (deltaX/60.0) );
- Arm_Table[PU_TOOL_1].base_rotate = Arm_Table[TOOL_1].base_rotate;
- }
- }
-
-
- /*
- while( shape_alignX_done == 0) {
- wait(1);
- shape_detected = shapeDetection();
-
- pc.printf("X - Adjust to center tool\n\r");
- if(get_com_x() > 100) {
- setServoPulse(0,Arm_Table[TOOL_1].base_rotate+=1);
-
- } else if(get_com_x() < 60) {
- setServoPulse(0,Arm_Table[TOOL_1].base_rotate-=1);
-
- } else {
- shape_alignX_done = 1;
- }
- }*/
-
- if (shape_detected == tool_needed) {
+ if (Xadjust(TOOL_1) == tool_needed) {
state = AQUIRE_TOOL1;
break;
} else {
@@ -550,13 +489,10 @@
servoPosition(PU_TOOL_1);
setServoPulse(4, 175);
wait(5);
- setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate );
+ setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate - 1);
wait(1);
setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 5);
wait(1);
- setServoPulse(5, 105);
- wait(.5);
- setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6);
wait(.5);
setServoPulse(5, 105);
@@ -584,15 +520,12 @@
servoPosition(PU_TOOL_2);
setServoPulse(4, 175);
wait(5);
- setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate );
+ setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate - 1);
wait(1);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
wait(1);
setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 2);
wait(1);
- setServoPulse(5, 105);
- wait(2);
- setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6);
wait(2);
setServoPulse(5, 105);
@@ -644,18 +577,7 @@
}
*/
- for(int i = 0; i < 6; i++) {
- shape_detected = shapeDetection();
- wait(2);
- if (get_com_x() > X_CENTER) {
- deltaX = get_com_x()-X_CENTER;
- setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) );
- }
- if (get_com_x() < X_CENTER) {
- deltaX = get_com_x()-X_CENTER;
- setServoPulse2(0,Arm_Table[PU_TOOL_3].base_rotate += (deltaX/60.0) );
- }
- }
+ Xadjust(TOOL_3);
setServoPulse(4, 175);
wait(5);
@@ -665,9 +587,6 @@
wait(1);
setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3);
wait(1);
- setServoPulse(5, 105);
- wait(.5);
- setServoPulse(5, 00);
setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 6);
wait(.5);
setServoPulse(5, 105);
@@ -824,7 +743,7 @@
pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
float pulselength = 20000; // 20,000 us per second
int i = currentPosition[n];
- pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
+ //pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
int pulse2;
if(currentPosition[n] < pulse) {
for(i; i < pulse; i++) {
@@ -840,7 +759,7 @@
}
}
currentPosition[n] = i;
- pc.printf("END: pulse: %d, angle: %d\n\r", i, angle);
+ //pc.printf("END: pulse: %d, angle: %d\n\r", i, angle);
}
void initServoDriver(void)
@@ -896,6 +815,12 @@
}
+
+
+
+
+
+
int fire_checker(int rig)
{
switch (rig) {
@@ -936,11 +861,16 @@
void errFunction(void)
{
- //Nothing
+ pc.printf("\n\nERROR: %d", motors.getError() );
+
}
+
+
+
+
void wall_follow(int side, int direction, int section)
{
- float location, set=6;
+ float location, set=4;
int dir=1;
pid1.reset();
@@ -967,7 +897,7 @@
rangeFinderRight.startMeas();
wait_ms(20);
rangeFinderRight.getMeas(range);
- pc.printf("%d\r\n",range);
+ //pc.printf("%d\r\n",range);
}
if(range > 15) {
@@ -1014,8 +944,8 @@
void wall_follow2(int side, int direction, int section, float location, int rig)
{
- int dir=1, limit=86, lowlim=5;
- float set=6, loc=0, Rigloc=0;
+ int dir=1, limit=80, lowlim=4;
+ float set=9, loc=0, Rigloc=0;
bool SeeWaveGap = false;
if(rig == 1) Rigloc= 16;
@@ -1027,28 +957,31 @@
if(direction == BACKWARD) {
dir=-1;
limit = 100;
- }
+ } else if(direction == FORWARD) lowlim=-20;
if(section == TOOLS) {
- set= 6;
+ set= 9;
limit = 86;
- }
- if(section == RETURN) lowlim=15;
+ } else if(section == RIGS) set = 9;
+ else if(section == RETURN) lowlim=4;
+ else if(section == MID2) limit =85;
+
+ if(location <4) limit=80;
leftEncoder.reset();
rightEncoder.reset();
- //pc.printf("before %f\r\n", location);
+ pc.printf("before %f\r\n", location);
- pc.printf("dir*loc+location %f\r\n",dir*loc + location );
- pc.printf("limit %d \r\n", limit);
+ //pc.printf("dir*loc+location %f\r\n",dir*loc + location );
+ //pc.printf("limit %d \r\n", limit);
while((dir*loc + location <= limit) && (dir*loc + location >= lowlim)) {
loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
- pc.printf("loc %f \r\n", loc);
+ //pc.printf("loc %f \r\n", loc);
pid1.setInputLimits(0.0, set);
- pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
+ pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1);
pid1.setSetPoint(set);
if(side) {
@@ -1066,7 +999,7 @@
wait_ms(20);
rangeFinderLeft.getMeas(range2);
- if(range2< 20) {
+ if(range2< 15) {
if( abs(dir*loc + location - Rigloc) < 10) {
//STOP
motors.stopBothMotors(127);
@@ -1078,18 +1011,19 @@
//pc.printf("wall follow 2 range %f\r\n",range);
//pc.printf("loc+location = %f\r\n", loc+location);
- if(range > 20 ) {
+ if(range > 15 ) {
if(section == RIGS || section == RETURN) {
motors.setMotor0Speed(dir*0.25*127); //right
motors.setMotor1Speed(dir*0.25*127); //left
} else {
if(!SeeWaveGap) {
+ wait_ms(40);
SeeWaveGap=true;
} else {
//STOP
motors.stopBothMotors(127);
- pc.printf("wavegap\r\n");
+ //pc.printf("wavegap\r\n");
// AT WAVE OPENING!!!!
break;
}
@@ -1098,7 +1032,7 @@
SeeWaveGap = false;
pid1.setProcessValue(range);
pid_return = pid1.compute();
- //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
+ pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
if(pid_return > 0) {
if(side) {
@@ -1128,15 +1062,12 @@
}
-
-
-
void alignWithWall(int section)
{
float usValue = 0;
if(section == TOOLS) {
- pc.printf("tools section align\r\n");
+ //pc.printf("tools section align\r\n");
// turn at an angle
leftEncoder.reset();
rightEncoder.reset();
@@ -1160,11 +1091,28 @@
motors.setMotor1Speed(-MAX_SPEED); //left
while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);
- motors.stopBothMotors(0);
+ motors.stopBothMotors(127);
+ wait_ms(300);
+ return;
+ /*
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderRight.getMeas(range);
+ if(range>15){
+ // turning left
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-MAX_SPEED); //right
+ motors.setMotor1Speed(MAX_SPEED); //left
+ while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50);
+ motors.stopBothMotors(127);
+ return;
+ }
+ */
// turning left
- motors.setMotor0Speed(0.9*MAX_SPEED); //right
- motors.setMotor1Speed(-0.9*MAX_SPEED); //left
+ //motors.setMotor0Speed(0.9*MAX_SPEED); //right
+ //motors.setMotor1Speed(-0.9*MAX_SPEED); //left
} else if(section == RIGS) {
// check distance to wall
@@ -1172,7 +1120,7 @@
wait_ms(20);
rangeFinderRight.getMeas(range);
- if(range < 3 || range > 20) return;
+ if(range < 3) return;
// turn at an angle
leftEncoder.reset();
@@ -1206,34 +1154,61 @@
motors.setMotor0Speed(-0.9*MAX_SPEED); //right
motors.setMotor1Speed(0.9*MAX_SPEED); //left
*/
- } else {// MID
- pc.printf("in mid section align\r\n");
+ } else if(section == MID2) {
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-1.2*MAX_SPEED); //right
+ motors.setMotor1Speed(0.4*MAX_SPEED); //left
+ while(rightEncoder.getPulses()>-1000);
+ motors.stopBothMotors(0);
+
+ //go backwards toward wall
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-0.25*127); //right
+ motors.setMotor1Speed(-0.25*127); //left
+ while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
+
+ // turn left towards wall
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.4*127); //right
+ motors.setMotor1Speed(-0.4*127); //left
+ while(abs(rightEncoder.getPulses()) < 65 || abs(leftEncoder.getPulses()) < 65);
+ motors.stopBothMotors(127);
+
+ slightMove(FORWARD,100);
+ return;
+
+ } else { // MID
+ //pc.printf("in mid section align\r\n");
// turn right towards wall
rightTurn();
// turning left towards wall
motors.setMotor0Speed(0.9*MAX_SPEED); //right
motors.setMotor1Speed(-0.9*MAX_SPEED); //left
+
}
usValue = 0;
- while(1) {
- if(section == 10) { // CURENTLY NOT USED (WAS RIGS)
- rangeFinderRight.startMeas();
- wait_ms(20);
- rangeFinderRight.getMeas(range);
- } else {
- rangeFinderLeft.startMeas();
- wait_ms(20);
- rangeFinderLeft.getMeas(range);
+ /* while(1) {
+ if(section == 10) { // CURENTLY NOT USED (WAS RIGS)
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderRight.getMeas(range);
+ } else {
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
+ }
+ //pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
+ if(range > usValue && usValue != 0 && range < 25) {
+ break;
+ } else {
+ usValue = range;
+ }
}
- pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
- if(range > usValue && usValue != 0 && range < 25) {
- break;
- } else {
- usValue = range;
- }
- }
- motors.stopBothMotors(0);
+ motors.stopBothMotors(0);*/
}
void rightTurn(void)
@@ -1243,7 +1218,7 @@
rightEncoder.reset();
motors.setMotor0Speed(-0.5*127);//right
motors.setMotor1Speed(0.5*127);//left
- while(abs(leftEncoder.getPulses())<1050 || abs(rightEncoder.getPulses())<1050);
+ while(abs(leftEncoder.getPulses())<850 || abs(rightEncoder.getPulses())<850);
motors.stopBothMotors(127);
}
@@ -1254,7 +1229,7 @@
rightEncoder.reset();
motors.setMotor0Speed(0.5*127);// right
motors.setMotor1Speed(-0.5*127);// left
- while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
+ while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075);
motors.stopBothMotors(127);
}
@@ -1265,7 +1240,7 @@
rightEncoder.reset();
motors.setMotor0Speed(0.5*127);// right
motors.setMotor1Speed(-0.5*127);// left
- while(abs(leftEncoder.getPulses())<90 || rightEncoder.getPulses()<90);
+ while(abs(leftEncoder.getPulses())<70 || rightEncoder.getPulses()<70);
motors.stopBothMotors(127);
}
@@ -1276,7 +1251,7 @@
rightEncoder.reset();
motors.setMotor0Speed(-0.4*127);// right
motors.setMotor1Speed(0.4*127);// left
- while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50);
+ while(abs(leftEncoder.getPulses())<200 || abs(rightEncoder.getPulses())<200);
motors.stopBothMotors(127);
}
@@ -1329,7 +1304,7 @@
while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
motors.stopBothMotors(127);
- pc.printf("slight backwards\r\n");
+ //pc.printf("slight backwards\r\n");
wait_ms(200);
// Over bump
@@ -1337,16 +1312,11 @@
rightEncoder.reset();
motors.setMotor0Speed(0.3*127); //right
motors.setMotor1Speed(0.3*127); //left
- while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 ) {
- /*
- preLeft=leftEncoder.getPulses();
- preRight=rightEncoder.getPulses();
- wait_ms(200);
- if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
- */
- }
+ while((abs(leftEncoder.getPulses()) < 800 || abs(rightEncoder.getPulses()) < 800) /*&& preLeft!=0*/ && IR.getIRDistance() >15 );
+
- pc.printf("forward \r\n");
+
+ //pc.printf("forward \r\n");
wait_ms(200);
motors.stopBothMotors(0);
@@ -1368,8 +1338,9 @@
wait_ms(200);
}
} else if(section == MID || section == MID2) {
- if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 400 || abs(rightEncoder.getPulses()) < 400));
- while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
+ if(section == MID2) while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100));
+
+ while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
if(IR.getIRDistance() > 38) break;
@@ -1394,6 +1365,7 @@
if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
}
motors.stopBothMotors(127);
+ motors.begin();
return;
}
@@ -1403,16 +1375,18 @@
motors.begin();
}
+
+
void to_tools_section1(float* location, float ¤t)
{
- slightMove(FORWARD,6600);
+ slightMove(FORWARD,6800);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
}
void to_tools_section2(float* location, float ¤t)
{
- slightMove(FORWARD,3200);
+ slightMove(FORWARD,3250);
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
}
@@ -1420,43 +1394,44 @@
void from_tools_section(float* location, float ¤t)
{
- alignWithWall(TOOLS);
- pc.printf("align\r\n");
- wait_ms(100);
+ //alignWithWall(TOOLS);
+ //current-=4;
+
+ //slightMove(FORWARD,150);
+ //current+=1;
+ //pc.printf("align\r\n");
+ //wait_ms(200);
//wall_follow2(LEFT,FORWARD,MID, current);
- //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ slightMove(BACKWARD,400);
+ current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
- if(range < 20) {
- wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
- pc.printf("wall follow\r\n");
+ if(range < 15) {
+ wall_follow2(LEFT,BACKWARD,MID, current,0);
+ //pc.printf("wall follow\r\n");
location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
current= location[0];
- pc.printf("current %f \r\n",current);
+ //pc.printf("current %f \r\n",current);
// go backwards
- leftEncoder.reset();
- rightEncoder.reset();
- motors.setMotor0Speed(-MAX_SPEED); //right
- motors.setMotor1Speed(-MAX_SPEED); //left
- while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
- // hard stop
- motors.stopBothMotors(127);
+ //slightMove(BACKWARD,200);
wait_ms(100);
leftTurn();
+ slightleft();
overBump(TOOLS);
} else {
- pc.printf("else greater than 20\r\n");
+ //pc.printf("else greater than 20\r\n");
location[0]= current;
leftTurn();
overBump(TOOLS);
}
- pc.printf("First Wavegap = %f\r\n",location[0]);
+ //pc.printf("First Wavegap = %f\r\n",location[0]);
}
void tools_section(float* location, float ¤t)
@@ -1470,14 +1445,15 @@
///////////////////////////////////////////////////////////////////////////////////////
// Move Forward
slightMove(FORWARD, 100);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
//////////////////////////////////////////Tool aquiring
wait(2);
//////////////////////////////////////////////////////////////////// After tool is aquired
- alignWithWall(TOOLS);
- pc.printf("align\r\n");
- wait_ms(100);
+ //alignWithWall(TOOLS);
+ //pc.printf("align\r\n");
+ //wait_ms(100);
//wall_follow2(LEFT,FORWARD,MID, current);
//current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
@@ -1488,10 +1464,10 @@
if(range < 20) {
wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
- pc.printf("wall follow\r\n");
+ //pc.printf("wall follow\r\n");
location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
current= location[0];
- pc.printf("current %f \r\n",current);
+ //pc.printf("current %f \r\n",current);
// go backwards
leftEncoder.reset();
rightEncoder.reset();
@@ -1506,32 +1482,36 @@
leftTurn();
overBump(TOOLS);
} else {
- pc.printf("else greater than 20\r\n");
+ //pc.printf("else greater than 20\r\n");
location[0]= current;
leftTurn();
overBump(TOOLS);
}
- pc.printf("First Wavegap = %f\r\n",location[0]);
+ //pc.printf("First Wavegap = %f\r\n",location[0]);
}
void mid_section(float* location, float ¤t, int* direction)
{
- motors.begin();
-
if(IR.getIRDistance() > 38) {
direction[0]= STRAIGHT;
overBump(MID);
return;
}
- pc.printf("before align with wall \r\n");
- alignWithWall(MID);
- wait_ms(100);
+ //pc.printf("before align with wall \r\n");
+ //alignWithWall(MID);
+ //current-=4;
+ //wait_ms(200);
- pc.printf("mid section current = %f\r\n",current);
+ //if(current > 20){
+ //alignWithWall(MID2);
+ //current-=4;
+ //}
+ rightTurn();
+ //pc.printf("mid section current = %f\r\n",current);
wall_follow2(LEFT,FORWARD,MID, current,0);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
- pc.printf("after wf2 current = %f\r\n",current);
+ //pc.printf("after wf2 current = %f\r\n",current);
wait_ms(500);
rangeFinderLeft.startMeas();
@@ -1541,8 +1521,9 @@
if(range > 20 ) {
direction[0]= RIGHT;
location[1]= current;
- slightMove(FORWARD,75);
- //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ wait_ms(300);
+ slightMove(FORWARD,100);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
direction[0]= LEFT;
wall_follow2(LEFT,BACKWARD,MID,current,0);
@@ -1550,14 +1531,23 @@
current= location[1];
if(location[1] < 18) {
- slightMove(FORWARD, 50);
+ slightMove(FORWARD, 75);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
+ //slightMove(BACKWARD,100);
}
- pc.printf("wavegap2 = %f\r\n",location[1]);
- leftTurn();
+ wait_ms(200);
+ //pc.printf("wavegap2 = %f\r\n",location[1]);
+ //left turn
+ motors.begin();
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.5*127);// right
+ motors.setMotor1Speed(-0.5*127);// left
+ while(abs(leftEncoder.getPulses())<1045 || rightEncoder.getPulses()<1045);
+ motors.stopBothMotors(127);
wait_ms(100);
@@ -1567,9 +1557,7 @@
void mid_section2(float* location, float ¤t, int* direction)
{
- motors.begin();
-
- pc.printf("mid section 2\r\n");
+ //pc.printf("mid section 2\r\n");
if(IR.getIRDistance() > 38) {
direction[1]= STRAIGHT;
@@ -1577,15 +1565,20 @@
return;
}
- alignWithWall(MID);
- pc.printf("midsection 2 alignt with wall mid \r\n");
+ //alignWithWall(MID);
+ wait_ms(100);
+
+ rightTurn();
+ slightright();
+ wait_ms(100);
+
wall_follow2(LEFT,FORWARD,MID, current,0);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
wait_ms(500);
- pc.printf("midseection 2 after wf2 %f",current);
+ //pc.printf("midseection 2 after wf2 %f",current);
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
@@ -1593,19 +1586,27 @@
if(range > 20 ) {
direction[1]= RIGHT;
location[2]= current;
- slightMove(FORWARD,100);
+ slightMove(FORWARD,50);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
direction[1]= LEFT;
wall_follow2(LEFT,BACKWARD,MID,current,0);
location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
current=location[2];
- //slightMove(FORWARD,100);
+ //slightMove(BACKWARD,100);
}
- leftTurn();
+ //LEFT turn
+ motors.begin();
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.5*127);// right
+ motors.setMotor1Speed(-0.5*127);// left
+ while(abs(leftEncoder.getPulses())<950 || rightEncoder.getPulses()<950);
+ motors.stopBothMotors(127);
+
overBump(RIGS);
- pc.printf("overbump rigs\r\n");
+ //pc.printf("overbump rigs\r\n");
}
void rig_section(float* location, float ¤t, int* direction, int rig)
@@ -1617,28 +1618,34 @@
else loc = 75;
// Slight forward for turn
- slightMove(FORWARD,100);
+ slightMove(FORWARD,150);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
wait_ms(100);
rightTurn();
- slightright();
- wait(5);
+ //slightright();
+
if(current > loc) {
- pc.printf("RIG section %f\r\n",current);
+ //pc.printf("RIG section %f\r\n",current);
wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
} else {
- pc.printf("RIG section %f\r\n",current);
+ //pc.printf("RIG section %f\r\n",current);
wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
- alignWithWall(RIGS);
- // Go forward until Rig
+ alignWithWall(MID2);
+ current-=4;
wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
- // line back wheel up with side of rig
- slightMove(FORWARD,300);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ slightMove(FORWARD, 75);
current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+
+
+
+
+
}
void tools_section_return(float* location, float ¤t)
@@ -1697,4 +1704,83 @@
}
rightTurn();
overBump(MID2);
-}
\ No newline at end of file
+}
+
+
+
+
+int Xadjust(int tool)
+{
+ int areaArray[10];
+ float C, T, S;
+ for(int i = 0; i < 10; i++) {
+ areaArray[i] = shapeDetection();
+ wait(2);
+ if(get_com_x() > X_CENTER ) {
+ deltaX = get_com_x()-X_CENTER;
+ setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
+ Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
+ }
+ if(get_com_x() < X_CENTER) {
+ deltaX = get_com_x()-X_CENTER;
+ setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
+ Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
+ }
+
+
+ }
+
+ C = normd(areaArray, 10, CIRCLE_AREA);
+ // S = normd(areaArray, 10, SQUARE_AREA);
+ // T = normd(areaArray, 10, TRIANGLE_AREA);
+
+ if((C < SQUARE_AREA) && (C > CIRCLE_AREA)) {
+ pc.printf("\n\nCIRCLE DETECTED\n\r");
+ return CIRCLE;
+ } else if( ( C > SQUARE_AREA ) ) {
+ pc.printf("\n\nSQUARE DETECTED\n\r");
+ return SQUARE;
+ } else {
+ pc.printf("\n\nTRIANGLE DETECTED\n\r");
+ return TRIANGLE;
+
+
+
+/*
+ if((C < S) && (C < T)) {
+ pc.printf("\n\nCIRCLE DETECTED\n\r");
+ return CIRCLE;
+ } else if( ( S<C ) && ( S<T ) ) {
+ pc.printf("\n\nSQUARE DETECTED\n\r");
+ return SQUARE;
+ } else {
+ pc.printf("\n\nTRIANGLE DETECTED\n\r");
+ return TRIANGLE;
+ }*/
+}
+
+float normd(int* pop, int count, int threshold)
+{
+ int i = 0;
+ float mean=0, std=0;
+ for(i=0; i<count; i++) {
+ mean += pop[i];
+ }
+ mean /= (float)count;
+ pc.printf("\n\nMean: %f\n\r", mean);
+
+ for(i=0; i<count; i++) {
+ std += pow(((float)pop[i]-mean),2);
+ }
+ std /= (float)count;
+ std = sqrt(std);
+ //pc.printf("\n\nStd: %f\n\r", std);
+
+ //pc.printf("\n\nNorm: %f\n\r", (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2))));
+
+
+ //return abs(mean - threshold);
+ return mean;
+ //return (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2)));
+
+}