Paolina Povolotskaya
ksdflsjdfljas
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of
theRobot
by 
Thomas Ashworth
Diff: main.cpp
- Revision:
- 8:77a57909aa15
- Parent:
- 7:8fb4204f9600
- Child:
- 9:1b29cd9ed1ba
--- a/main.cpp Wed Mar 19 17:08:25 2014 +0000
+++ b/main.cpp Thu Mar 27 23:16:02 2014 +0000
@@ -1,16 +1,35 @@
#include "mbed.h"
#include "Adafruit_PWMServoDriver.h"
#include "ShapeDetect.h"
+#include "rtos.h"
+#include "PID.h"
+#include "PololuQik2.h"
+#include "QEI.h"
+#include "HCSR04.h"
+#include "stdio.h"
+#include "LPC17xx.h"
+#include "Sharp.h"
-DigitalOut myled1(LED1);
-DigitalOut myled2(LED2);
-DigitalOut myled3(LED3);
-DigitalOut myled4(LED4);
-
-Serial pc(USBTX,USBRX); //USB Comm
-Adafruit_PWMServoDriver pwm(p28,p27); //Servo Control PWM
-DigitalOut ServoOutputDisable(p8); //Servo Control Output Enable/Disable
-extern Serial lrf; //Laser Range Finder lrf(p13,p14)
+/* Navigation Definitions */
+#define PIN_TRIGGERL (p12)
+#define PIN_ECHOL (p11)
+#define PIN_TRIGGERR (p29)
+#define PIN_ECHOR (p30)
+#define PULSE_PER_REV (1192)
+#define WHEEL_CIRCUM (12.56637)
+#define DIST_PER_PULSE (0.01054225722682)
+#define MTRS_TO_INCH (39.3701)
+#define MAX_SPEED (0.3*127)
+#define PPR (4331/4)
+#define LEFT (1)
+#define RIGHT (0)
+#define FORWARD (1)
+#define BACKWARD (0)
+#define TOOLS (0)
+#define MID (1)
+#define RIGS (2)
+#define FIRST_WAVE (0)
+#define FAR (1)
//States
#define START 0
@@ -32,6 +51,7 @@
#define END 16
+
//Servo Static Positions
#define STORE_POSITION 0
#define OIL_RIG1 1
@@ -53,18 +73,45 @@
//******* TODO ********//
//*********************//
//Oil Rig distance thresholds
-#define OILRIG1_MAX 1200
-#define OILRIG1_MIN 1200
-#define OILRIG2_MAX 1200
-#define OILRIG2_MIN 1200
-#define OILRIG3_MAX 1200
-#define OILRIG3_MIN 1200
+#define OILRIG1_MAX 3000
+#define OILRIG1_MIN 1000
+#define OILRIG2_MAX 3000
+#define OILRIG2_MIN 1000
+#define OILRIG3_MAX 3000
+#define OILRIG3_MIN 1000
//for servo normalization
#define MIN_SERVO_PULSE 900
#define MAX_SERVO_PULSE 2100
#define SERVO_MAX_ANGLE 180
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+void errFunction(void);
+bool cRc;
+
+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);
+HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
+HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
+PID pid1(15.0,0.0,4.0,0.02);
+PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
+QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
+QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
+Sharp IR(p20);
+//InterruptIn encoder(p29);
+
+
+
+
/***************
local servo functions
****************/
@@ -76,6 +123,25 @@
void setGripper(int open);
int fire_checker(int rig);
+
+//Navigation Functions
+float wall_follow(int side, int direction, int section);
+void wall_follow2(int side, int direction, int section, float location);
+void wall_follow3(int ¤tLocation, int &WaveOpening);
+void leftTurn(void);
+void slightleft(void);
+void rightTurn(void);
+void us_distance(void);
+void to_tools_section(float* location, float ¤t);
+void from_tools_section(float* location, float ¤t);
+void mid_section(float* location, float ¤t, int* direction);
+void mid_section2(float* location, float ¤t, int* direction);
+void rig_section(float* location, float ¤t, int* direction, int rig);
+void overBump(int section);
+void alignWithWall(int section);
+void ledtoggle(void);
+
+
/************
Main Variables
*************/
@@ -83,6 +149,8 @@
int fire = 0;
int tool_needed = 0;
int shape_detected = 0;
+float range, range2, pid_return;
+
/************
Variables for Servos
@@ -109,15 +177,17 @@
// POSITION ODER:
// base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
- {STORE_POSITION, 90, 0, 0, 0, 90, 90}, // storing position
- {OIL_RIG1, 90, 60, 75, 60, 90, 90}, // point laser at oilrig1
- {OIL_RIG2, 120, 60, 75, 60, 90, 90}, // point laser at oilrig2
- {OIL_RIG3, 135, 60, 75, 60, 90, 90}, // point laser at oilrig2
- {DRIVE_POSITION_NOTOOL, 135, 60, 75, 60, 90, 90}, // Drive through course
- {TOOL_1, 135, 60, 75, 60, 90, 90}, // Look over first tool
- {TOOL_2, 135, 60, 75, 60, 90, 90}, // Look over second tool
- {TOOL_3, 135, 60, 75, 60, 90, 90}, // Look over third tool
- {DRIVE_POSITION_TOOL, 135, 60, 75, 60, 90, 90}, // Drive with tool loaded
+//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
+ {OIL_RIG3, 130, 90, 90, 50, 100, 0}, // point laser at oilrig2
+ {DRIVE_POSITION_NOTOOL, 55, 70, 102, 74, 0, 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
+ {DRIVE_POSITION_TOOL, 55, 70, 102, 74, 0, 0}, // Drive with tool loaded
{ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted
};
@@ -129,7 +199,7 @@
int shape;
/* Variables for distance sensor*/
-int dist;
+int distLaser;
int fire_detected = 0;
int fire_not_detected = 0;
@@ -140,9 +210,14 @@
/*****************
INITIALIZATIONS
*******************/
+ float location[3], current=0;
+ int direction[3];
+ double distance;
+
pc.baud(115200);
//Laser Range Finder Initialization
lrf_baudCalibration();
+ motors.begin();
//Servo initialization
initServoDriver();
@@ -153,11 +228,25 @@
WHILE LOOP FOR TESTING
********************/
while(1) {
- //pc.scanf("%d %d", &servoNum, &servoAngle);
- //setServoPulse(servoNum, servoAngle);
+ pc.scanf("%d %d", &servoNum, &servoAngle);
+ if(servoAngle > 175) {
+ servoAngle = 175;
+ }
+ if(servoNum > 5 ) {
+ servoNum = 0;
+ servoAngle = 90;
+ }
+ setServoPulse(servoNum, servoAngle);
+
+ //ledtoggle();
- pc.scanf("%d", &posNum);
- servoPosition(posNum);
+ //pc.scanf("%d", &posNum);
+ //servoPosition(posNum);
+ //wait(3);
+ //shape_detected = shapeDetection();
+ //distLaser = getDistance();
+ //pc.printf("Distance %d", distLaser);
+
}
/********************************
@@ -165,7 +254,7 @@
*********************************/
while(1) {
switch (state) {
-
+
/**************************************************
* STAGE 0
*
@@ -188,7 +277,7 @@
**************************************************/
case OILRIG1_POS: //aims arm at square oil rig
servoPosition(OIL_RIG1); //position arm to point at first oilrig
- wait(1); //wait for servos to settle
+ wait(2); //wait for servos to settle
fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
//determines what tool is needed
@@ -223,14 +312,13 @@
servoPosition(DRIVE_POSITION_NOTOOL); //drive position without a tool
wait(1); //wait for servos to settle
- //*********************//
- //******* TODO ********//
- //*********************//
- //CODE TO NAVIGATE TO TOOLS
+//****************************************************//
+
+ to_tools_section(location, current);
state = IDENTIFY_TOOLS;
break;
-
+ while(1) {}
/**************************************************
* STAGE 3
*
@@ -451,12 +539,17 @@
void servoBegin(void)
{
pc.printf("Setting Initial Position\n\r");
- setServoPulseNo_delay(0, 90);
- setServoPulseNo_delay(1, 0);
- setServoPulseNo_delay(2, 177);
- setServoPulseNo_delay(3, 0);
- setServoPulseNo_delay(4, 0);
- setServoPulseNo_delay(5, 90);
+ setServoPulseNo_delay(3, 175);
+ wait(2);
+ setServoPulseNo_delay(2, 0);
+ wait(2);
+ setServoPulseNo_delay(1, 10);
+ wait(2);
+ setServoPulseNo_delay(0, 85);
+ wait(1);
+ setServoPulseNo_delay(4, 100);
+ wait(1);
+ setServoPulseNo_delay(5, 0);
setGripper(1);
}
@@ -465,7 +558,7 @@
int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
float pulselength = 20000; // 20,000 us per second
currentPosition[n] = pulse;
- pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
+ //pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
pulse = 4094 * pulse / pulselength;
pwm.setPWM(n, 0, pulse);
@@ -485,23 +578,24 @@
void servoPosition(int set)
{
//moves to current position
- setServoPulse(0, Arm_Table[set].base_rotate);
+ setServoPulse(3, Arm_Table[set].claw_arm);
setServoPulse(1, Arm_Table[set].base_arm);
setServoPulse(2, Arm_Table[set].big_arm);
- setServoPulse(3, Arm_Table[set].claw_arm);
+ setServoPulse(0, Arm_Table[set].base_rotate);
setServoPulse(4, Arm_Table[set].claw_rotate);
setServoPulse(5, Arm_Table[set].claw_open);
}
+
int fire_checker(int rig)
{
switch (rig) {
case 1:
for (int i = 0; i<5; i++) {
- dist = getDistance();
- if ((dist < OILRIG1_MAX)
- || (dist > OILRIG1_MIN)) {
+ distLaser = getDistance();
+ if ((distLaser < OILRIG1_MAX)
+ || (distLaser > OILRIG1_MIN)) {
fire_detected++;
} else {
fire_not_detected++;
@@ -509,9 +603,9 @@
}
case 2:
for (int i = 0; i<5; i++) {
- dist = getDistance();
- if ((dist < OILRIG2_MAX)
- || (dist > OILRIG2_MIN)) {
+ distLaser = getDistance();
+ if ((distLaser < OILRIG2_MAX)
+ || (distLaser > OILRIG2_MIN)) {
fire_detected++;
} else {
fire_not_detected++;
@@ -519,9 +613,9 @@
}
case 3:
for (int i = 0; i<5; i++) {
- dist = getDistance();
- if ((dist < OILRIG3_MAX)
- || (dist > OILRIG3_MIN)) {
+ distLaser = getDistance();
+ if ((distLaser < OILRIG3_MAX)
+ || (distLaser > OILRIG3_MIN)) {
fire_detected++;
} else {
fire_not_detected++;
@@ -529,9 +623,9 @@
}
default:
for (int i = 0; i<5; i++) {
- dist = getDistance();
- if ((dist < OILRIG1_MAX)
- || (dist > OILRIG1_MIN)) {
+ distLaser = getDistance();
+ if ((distLaser < OILRIG1_MAX)
+ || (distLaser > OILRIG1_MIN)) {
fire_detected++;
} else {
fire_not_detected++;
@@ -546,5 +640,485 @@
}
}
+void errFunction(void)
+{
+ //Nothing
+}
+
+void us_distance(void)
+{
+ pc.printf("Ultra Sonic\n\r");
+ rangeFinderLeft.startMeas();
+ wait_us(20);
+ if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
+ pc.printf("Range = %f\n\r", range);
+ }
+}
+
+float wall_follow(int side, int direction, int section)
+{
+ float location, wavegap=0, set=5;
+ int dir=1;
+
+ pid1.reset();
+
+ if(direction == BACKWARD) dir=-1;
+ if(section == TOOLS)set= 10;
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+
+ location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+
+ while(location< 73) {
+ location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+
+ pid1.setInputLimits(0, set);
+ pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
+ pid1.setSetPoint(set);
+ if(side) {
+ rangeFinderLeft.startMeas();
+ wait_ms(38);
+ rangeFinderLeft.getMeas(range);
+ } else {
+ rangeFinderRight.startMeas();
+ wait_ms(38);
+ rangeFinderRight.getMeas(range);
+ pc.printf("%d\r\n",range);
+ }
+
+ if(range > 20) {
+ wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+ //pc.printf("wavegap %f\r\n",wavegap);
+ // AT WAVE OPENING!!!!
+ motors.setMotor1Speed(dir*0.4*127);//left
+ motors.setMotor0Speed(dir*0.4*127);//right
+ } else {
+
+ pid1.setProcessValue(range);
+ pid_return = pid1.compute();
+
+ if(pid_return > 0) {
+ if(side) {
+ motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+ motors.setMotor1Speed(dir*MAX_SPEED);//left
+ } else {
+ motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
+ motors.setMotor0Speed(dir*MAX_SPEED);//right
+ }
+ } else if(pid_return < 0) {
+ if(side) {
+ motors.setMotor0Speed(dir*MAX_SPEED);//right
+ motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
+ } else {
+ motors.setMotor1Speed(dir*MAX_SPEED);//left
+ motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
+ }
+ } else {
+ motors.setMotor0Speed(dir*MAX_SPEED);//right
+ motors.setMotor1Speed(dir*MAX_SPEED);//left
+ }
+ }
+ }
+ return wavegap;
+}
+
+/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
+
+void wall_follow2(int side, int direction, int section, float location)
+{
+ int SeeWaveGap = false, dir=1;
+ float set=5, loc=0;
+
+ pid1.reset();
+
+ if(direction == BACKWARD) dir=-1;
+ if(section == TOOLS)set= 5;
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+
+ while(dir*loc + location <= 78) {
+ loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+
+ pid1.setInputLimits(0.0, set);
+ pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
+ pid1.setSetPoint(set);
+
+ if(side) {
+ rangeFinderLeft.startMeas();
+ wait_ms(38);
+ rangeFinderLeft.getMeas(range);
+ } else {
+ rangeFinderRight.startMeas();
+ wait_ms(38);
+ rangeFinderRight.getMeas(range);
+ }
+
+
+ /*************CHECK FOR WAVE OPENING*****************/
+ /* If after 20 ms the ultrasonic still sees 20+ cm */
+ /* then robot is at wave opening */
+
+ //pc.printf("wall follow 2 range %f\r\n",range);
+ //pc.printf("loc+location = %f\r\n", loc+location);
+ if(range > 20) {
+ motors.stopBothMotors();
+ pc.printf("wavegap\r\n");
+ // AT WAVE OPENING!!!!
+ break;
+ }
+
+ pid1.setProcessValue(range);
+ pid_return = pid1.compute();
+ //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
+
+ if(pid_return > 0) {
+ if(side) {
+ motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+ motors.setMotor1Speed(dir*MAX_SPEED);//left
+ } else {
+ motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
+ motors.setMotor0Speed(dir*MAX_SPEED);//right
+ }
+ } else if(pid_return < 0) {
+ if(side) {
+ motors.setMotor0Speed(dir*MAX_SPEED);//right
+ motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
+ } else {
+ motors.setMotor1Speed(dir*MAX_SPEED);//left
+ motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
+ }
+ } else {
+ motors.setMotor0Speed(dir*MAX_SPEED);
+ motors.setMotor1Speed(dir*MAX_SPEED);
+ }
+ }
+ motors.stopBothMotors();
+}
+void alignWithWall(int section)
+{
+ float usValue = 0;
+
+ if(section == TOOLS) {
+ // turn at an angle
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-1.2*MAX_SPEED); //right
+ motors.setMotor1Speed(0.4*MAX_SPEED); //left
+ while(rightEncoder.getPulses()>-1000);
+ motors.stopBothMotors();
+
+ //go backwards toward wall
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-MAX_SPEED); //right
+ motors.setMotor1Speed(-MAX_SPEED); //left
+ while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
+
+ motors.stopBothMotors();
+
+ // turn left towards wall
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(MAX_SPEED); //right
+ motors.setMotor1Speed(-MAX_SPEED); //left
+ while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10);
+
+ motors.stopBothMotors();
+
+ motors.setMotor0Speed(0.7*MAX_SPEED); //right
+ motors.setMotor1Speed(-0.7*MAX_SPEED); //left
+ } else {
+ rightTurn();
+ motors.setMotor0Speed(-0.7*MAX_SPEED); //right
+ motors.setMotor1Speed(0.7*MAX_SPEED); //left
+ }
+
+ usValue = 0;
+ while(1) {
+ 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;
+ }
+ }
+ motors.stopBothMotors();
+}
+
+void rightTurn(void)
+{
+ motors.begin();
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-0.5*127);//right
+ motors.setMotor1Speed(0.5*127);//left
+ while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
+ motors.stopBothMotors();
+}
+
+void leftTurn(void)
+{
+ /*
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.4*MAX_SPEED); //right
+ motors.setMotor1Speed(-MAX_SPEED); //left
+ while(abs(leftEncoder.getPulses())<2500);
+ motors.stopBothMotors();
+ */
+ motors.begin();
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.5*127);// right
+ motors.setMotor1Speed(-0.5*127);// left
+ while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
+ motors.stopBothMotors();
+}
+void slightleft(void)
+{
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.5*127);// right
+ motors.setMotor1Speed(-0.5*127);// left
+ while(abs(leftEncoder.getPulses())<50 || rightEncoder.getPulses()<50);
+ motors.stopBothMotors();
+}
+
+
+void overBump(int section)
+{
+ int preLeft=5000, preRight=5000, out=0;
+
+ motors.begin();
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-0.2*127); //right
+ motors.setMotor1Speed(-0.2*127); //left
+ while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
+ motors.stopBothMotors();
+
+ leftEncoder.reset();
+ 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) {
+ preLeft=leftEncoder.getPulses();
+ preRight=rightEncoder.getPulses();
+ wait_ms(100);
+ //pc.printf(" first while left %d right %d \r\n", preLeft, preRight);
+ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+ }
+
+ motors.stopBothMotors();
+ motors.begin();
+ wait(2);
+ /*
+ motors.stopBothMotors();
+ motors.setMotor0Speed(0.15*127); //right
+ motors.setMotor1Speed(0.15*127); //left
+ preLeft=preRight=5000 ;
+ leftEncoder.reset();
+ rightEncoder.reset();
+ */
+// while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
+ /* preLeft=leftEncoder.getPulses();
+ preRight=rightEncoder.getPulses();
+ pc.printf("second while left %d right %d \r\n", preLeft, preRight);
+ wait_ms(200);
+ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+ }*/
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.3*127); //right
+ motors.setMotor1Speed(0.3*127); //left
+
+ while(!out) {
+ preLeft=leftEncoder.getPulses();
+ preRight=rightEncoder.getPulses();
+
+ rangeFinderLeft.startMeas();
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
+ rangeFinderRight.getMeas(range2);
+ if(range < 10 || range2 < 10) out=1;
+
+ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
+ motors.setMotor0Speed(0.4*127); //right
+ motors.setMotor1Speed(0.4*127); //left
+ }
+ if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1;
+ }
+
+ motors.stopBothMotors();
+ wait(2);
+ motors.begin();
+
+ preLeft=preRight=5000 ;
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(.25*127); //right
+ motors.setMotor1Speed(.25*127); //left
+
+ if(section == TOOLS || section == MID) {
+ while(IR.getDistance() > 20 ) {
+ //pc.printf("IR %f\r\n", IR.getDistance());
+ //pc.printf("third while left %d right %d \r\n", preLeft, preRight);
+ }
+ } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
+
+ motors.setMotor0Speed(-.25*127); //right
+ motors.setMotor1Speed(-.25*127); //left
+ wait_ms(10);
+ motors.stopBothMotors();
+ wait(2);
+ motors.begin();
+
+}
+
+void to_tools_section(float* location, float ¤t)
+{
+ wall_follow(LEFT,FORWARD, TOOLS);
+ // 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
+
+}
+
+void from_tools_section(float* location, float ¤t)
+{
+ alignWithWall(TOOLS);
+
+ wait_ms(100);
+
+ wall_follow2(LEFT,FORWARD,MID, current);
+ current= 78;
+
+ rangeFinderLeft.startMeas();
+ wait_ms(20);
+ rangeFinderLeft.getMeas(range);
+
+ if(range < 20) {
+ wall_follow2(LEFT,BACKWARD,TOOLS, current);
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-MAX_SPEED); //right
+ motors.setMotor1Speed(-MAX_SPEED); //left
+ while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(MAX_SPEED); //right
+ motors.setMotor1Speed(MAX_SPEED); //left
+ while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
+ motors.stopBothMotors();
+
+ wait_ms(500);
+ location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ current= location[0];
+ leftTurn();
+ slightleft();
+ overBump(TOOLS);
+ } else {
+ location[0]= 77;
+ leftTurn();
+ wait_ms(20);
+ overBump(FIRST_WAVE);
+ }
+
+ pc.printf("First Wavegap = %f\r\n",location[0]);
+}
+void mid_section(float* location, float ¤t, int* direction)
+{
+
+ motors.begin();
+
+ if(IR.getDistance() > 20) return;
+
+ alignWithWall(MID);
+
+ pc.printf("mid section current = %f\r\n",current);
+ wall_follow2(LEFT,FORWARD,MID, current);
+ current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ pc.printf("after wf2 current = %f\r\n",current);
+
+ if(current != 0) {
+ direction[0]= RIGHT;
+ current+= location[0];
+ location[1]= current;
+ } else {
+ current=location[0];
+ direction[0]= LEFT;
+ wall_follow2(LEFT,BACKWARD,MID,current);
+ location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ }
+
+ pc.printf("wavegap2 = %f\r\n",location[1]);
+ leftTurn();
+ overBump(TOOLS);
+ // go forward
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.2*127); //right
+ motors.setMotor1Speed(0.2*127); //left
+ while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
+ motors.stopBothMotors();
+
+}
+
+void mid_section2(float* location, float ¤t, int* direction)
+{
+
+ motors.begin();
+
+ if(IR.getDistance() > 20) return;
+
+ alignWithWall(MID);
+ wall_follow2(LEFT,FORWARD,MID, current);
+ current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+
+ if(current != 0) {
+ direction[1]= RIGHT;
+ current+= location[1];
+ location[2]= current;
+ } else {
+ current=location[1];
+ direction[1]= LEFT;
+ wall_follow2(LEFT,BACKWARD,MID,current);
+ location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ }
+
+ leftTurn();
+ overBump(RIGS);
+}
+
+void rig_section(float* location, float ¤t, int* direction, int rig)
+{
+
+
+}
+
+void ledtoggle(void){
+ pwm.setPWM(9, 0, 4094);
+ wait(2);
+ pwm.setPWM(9, 0, 0);
+ }
+
+
+