Justin Carr
Yeah
Dependencies: HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of
NavigationTest
by 
Paolina Povolotskaya
Diff: main.cpp
- Revision:
- 5:70ccef3734ae
- Parent:
- 4:f2333e66ec2c
- Child:
- 6:f5c26372b2d0
--- a/main.cpp Tue Mar 18 23:11:28 2014 +0000
+++ b/main.cpp Thu Mar 20 17:47:31 2014 +0000
@@ -6,7 +6,7 @@
#include "HCSR04.h"
#include "stdio.h"
#include "LPC17xx.h"
-
+
#define PIN_TRIGGERL (p12)
#define PIN_ECHOL (p11)
#define PIN_TRIGGERR (p29)
@@ -24,11 +24,11 @@
#define TOOLS (0)
#define MID (1)
#define RIGS (2)
-
+
float range, pid_return;
void errFunction(void);
bool cRc;
-
+
//Hardware Initialization
Serial bt(p13,p14);
Serial pc(USBTX,USBRX);
@@ -39,10 +39,10 @@
QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
//InterruptIn encoder(p29);
-
-
+
+
//Functions
-
+
float wall_follow(int side, int direction, int section);
void wall_follow2(int side, int direction, int section);
void wall_follow3(int ¤tLocation, int &WaveOpening);
@@ -51,14 +51,15 @@
void us_distance(void);
void tools_section(float* location, float ¤t);
void overBump(void);
-
+void align(float speed);
+
//Variables
-
+
int main(void)
{
float location[3], current=0;
int direction[3];
-
+
pc.baud(115200);
bt.baud(115200);
motors.begin();
@@ -70,38 +71,48 @@
//motors.setMotor1Speed(MAX_SPEED); //left
//while((abs(leftEncoder.getPulses())/(PPR) + abs(rightEncoder.getPulses())/(PPR))/2 < 3)
-
+
//Go to tools
tools_section(location, current);
- /*
+
//////////////////////////////// without predefined wavegaps//////////////////////////////////////////////
- current=0;
+ /* current=0;
if(location[0]< 75){
- turnRight();
- current=wall_follow(LEFT,FORWARD);
- if(current == 0)turnLeft();
+ rightTurn();
+ current=wall_follow(LEFT,FORWARD,MID);
+ if(current != 0) direction[0]= RIGHT;
else{
- direction[0]= RIGHT;
- turnLeft();
- overBump();
+ direction[0]= LEFT;
+ wall_follow2(LEFT,BACKWARD,MID);
+ location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-MAX_SPEED); //right
+ motors.setMotor1Speed(-MAX_SPEED); //left
+ while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75);
+ motors.stopBothMotors();
}
+ leftTurn();
}
- else if(location[0]>=75 || current == 0){
- turnLeft();
- wall_follow2(RIGHT,FORWARD);
+ else{
+ leftTurn();
+ direction[0]= LEFT;
+ wall_follow2(RIGHT,FORWARD,MID);
+ location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+ rightTurn();
}
-
+ overBump();
-
+ */
// left or right
- location[1]=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
@@ -109,27 +120,28 @@
- leftTurn();
+ //leftTurn();
//wall_follow2(RIGHT);
- rightTurn();
+ // rightTurn();
-
-
+
+
bt.printf("LOCATION %f\n\r",location);
-
- motors.stopBothMotors();
-// leftTurn();
-// wait(1);
-// rightTurn();
-*/
-
+
+ //overBump();
+ //motors.stopBothMotors();
+ //leftTurn();
+ //wait(1);
+ //rightTurn();
+
+
}
-
+
void errFunction(void)
{
//Nothing
}
-
+
void us_distance(void)
{
pc.printf("Ultra Sonic\n\r");
@@ -139,16 +151,16 @@
pc.printf("Range = %f\n\r", range);
}
}
-
+
float wall_follow(int side, int direction, int section)
{
- float location, wavegap;
- int dir=1, set=5;
+ float location, wavegap=0, set=5;
+ int dir=1;
pid1.reset();
if(direction == BACKWARD) dir=-1;
- if(section == TOOLS)set= 9;
+ if(section == TOOLS)set= 10;
leftEncoder.reset();
rightEncoder.reset();
@@ -181,7 +193,7 @@
pid1.setProcessValue(range);
pid_return = pid1.compute();
-
+
if(pid_return > 0) {
if(side){
motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
@@ -207,27 +219,28 @@
}
return wavegap;
}
-
+
/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
-
+
void wall_follow2(int side, int direction, int section)
{
- int SeeWaveGap = false, count=0, dir=1, set=5;
+ int SeeWaveGap = false, dir=1;
+ float set=5;
pid1.reset();
if(direction == BACKWARD) dir=-1;
- if(section == TOOLS)set= 9;
+ if(section == TOOLS)set= 6.5;
leftEncoder.reset();
rightEncoder.reset();
-
+
while(1) {
-
- pid1.setInputLimits(0.0, 5.0);
+
+ pid1.setInputLimits(0.0, set);
pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
- pid1.setSetPoint(5.0);
-
+ pid1.setSetPoint(set);
+
if(side){
rangeFinderLeft.startMeas();
wait_ms(20);
@@ -238,12 +251,12 @@
wait_ms(20);
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("range %f\r\n",range);
if(range > 20) {
motors.stopBothMotors();
@@ -251,11 +264,11 @@
// AT WAVE OPENING!!!!
break;
}
-
+
pid1.setProcessValue(range);
pid_return = pid1.compute();
//bt.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
@@ -280,46 +293,46 @@
}
}
}
-
-
+
+
/* MODIFIED WALL_FOLLOW FOR NAVIGATION WITH WAVE OPENINGS PASSED IN */
/* MEANT FOR RETURNING FROM OIL RIGS */
-
+
void wall_follow3(int ¤tLocation, int &WaveOpening)
{
while(1) {
-
-
+
+
pid1.setInputLimits(0, 5);
pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
pid1.setSetPoint(5.0);
-
+
rangeFinderLeft.startMeas();
wait_ms(100);
if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0)) {
//bt.printf("Range = %f\n", range);
}
-
+
/*************CHECK FOR WAVE OPENING*****************/
/* If after 100 ms the ultrasonic still sees 20+ cm */
/* then robot is at wave opening */
-
-
+
+
if(range > 20 ) {
currentLocation--;
}
-
+
if( currentLocation == WaveOpening) {
// AT WAVE OPENING!!!!
-
+
break;
}
-
-
+
+
pid1.setProcessValue(range);
pid_return = pid1.compute();
bt.printf("Range: %f\n PID: %f", range, pid_return);
-
+
if(pid_return > 0) {
motors.setMotor0Speed(MAX_SPEED - pid_return);
motors.setMotor1Speed(MAX_SPEED);
@@ -332,28 +345,61 @@
}
}
}
-
+
+void align(float speed){
+ float left, right;
+ left=right=speed;
+
+ if(abs(leftEncoder.getPulses())> rightEncoder.getPulses()){
+ if(speed>0)left-=.01;
+ else left+=.01;
+ }
+ else{
+ if(speed>0) right+=.01;
+ else right-=.01;
+ }
+ motors.setMotor0Speed(right*127);//right
+ motors.setMotor1Speed(left*127);//left
+ pc.printf("speed left %f right %f\r\n\n",left, right);
+
+}
+
void rightTurn(void)
{
- float speedL, speedR;
-
- speedL=speedR= 0.4;
-
leftEncoder.reset();
rightEncoder.reset();
- motors.setMotor0Speed(-speedR*127);//right
- motors.setMotor1Speed(speedL*127);//left
- while(leftEncoder.getPulses()<1050 || rightEncoder.getPulses()>-1050);
+ motors.setMotor0Speed(-0.4*127);//right
+ motors.setMotor1Speed(0.4*127);//left
+ while(leftEncoder.getPulses()<1030 || rightEncoder.getPulses()>-1030);
motors.stopBothMotors();
}
-
+
void leftTurn(void)
-{
+{
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(0.4*127);// right
motors.setMotor1Speed(-0.4*127);// left
- while(leftEncoder.getPulses()>-1200 || rightEncoder.getPulses()<1200);
+ while(abs(leftEncoder.getPulses())<1120 || rightEncoder.getPulses()<1120); //align(0.4);
+ motors.stopBothMotors();
+}
+
+void slightRight(void)
+{
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-0.4*127);//right
+ motors.setMotor1Speed(0.4*127);//left
+ while(leftEncoder.getPulses()<515 || rightEncoder.getPulses()>-515);
+ motors.stopBothMotors();
+}
+void slightLeft(void)
+{
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(0.4*127);// right
+ motors.setMotor1Speed(-0.4*127);// left
+ while(abs(leftEncoder.getPulses())<400 || rightEncoder.getPulses()< 400); //align(0.4);
motors.stopBothMotors();
}
@@ -361,56 +407,78 @@
leftEncoder.reset();
rightEncoder.reset();
- motors.setMotor0Speed(127*.15); //right
- motors.setMotor1Speed(127*.15); //left
- while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <9);
+ motors.setMotor0Speed(MAX_SPEED); //right
+ motors.setMotor1Speed(MAX_SPEED); //left
+ while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <9)//align(MAX_SPEED);
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(MAX_SPEED); //right
motors.setMotor1Speed(MAX_SPEED); //left
- while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <3);
+ while(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR)/2 <6);
motors.stopBothMotors();
}
-
+
void tools_section(float* location, float ¤t){
location[0]=wall_follow(LEFT,FORWARD, TOOLS); //location from the left edge of the field
current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
bt.printf("wavegap %f \t current %f \r\n",location[0],current);
- motors.stopBothMotors();
+ motors.stopBothMotors();
+
+ slightRight();
+ //backward
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(-MAX_SPEED);//right
+ motors.setMotor1Speed(-MAX_SPEED);//left
+ while(abs(leftEncoder.getPulses())<500 || abs(rightEncoder.getPulses())<500);
+ motors.stopBothMotors();
+
+ slightLeft();
+
+ /*leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(MAX_SPEED);//right
+ motors.setMotor1Speed(MAX_SPEED);//left
+ while(abs(leftEncoder.getPulses())<100 || abs(rightEncoder.getPulses())<100);*/
+
+
+
////////////////////////////////////////// determine shape and pick up tool ///////////////////////////////////////////////////////
+ wall_follow2(LEFT,BACKWARD,TOOLS);
+ /*
if(current >location[0]){
wall_follow2(LEFT,BACKWARD,TOOLS);
- wait_ms(500);
- // forward
- /* leftEncoder.reset();
+ wait_ms(1000);
+ // back
+ leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-MAX_SPEED); //right
motors.setMotor1Speed(-MAX_SPEED); //left
- while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
+ while(abs(leftEncoder.getPulses())<75 || abs(rightEncoder.getPulses())<75);
motors.stopBothMotors();
- wait_ms(500);*/
current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
}
- else{/*
- wall_follow2(LEFT,FORWARD);
+ else{
+ wall_follow2(LEFT,FORWARD,TOOLS);
// backward
+ wait_ms(1000);
leftEncoder.reset();
rightEncoder.reset();
- motors.setMotor0Speed(MAX_SPEED); //right
- motors.setMotor1Speed(MAX_SPEED); //left
- while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
+ motors.setMotor0Speed(-MAX_SPEED); //right
+ motors.setMotor1Speed(-MAX_SPEED); //left
+ while(abs(leftEncoder.getPulses())<150 || abs(rightEncoder.getPulses())<150);
motors.stopBothMotors();
- current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;*/
- }
-
+ current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
+ }*/
+ wait_ms(1000);
leftTurn();
//Go over
overBump();
-}
+}
\ No newline at end of file