Paolina Povolotskaya
revise
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos Sharp
Diff: main.cpp
- Revision:
- 10:c57f6a5042d7
- Parent:
- 9:f34700716f1d
- Child:
- 11:d67a3958127a
--- a/main.cpp Thu Mar 27 17:49:30 2014 +0000
+++ b/main.cpp Thu Mar 27 22:40:22 2014 +0000
@@ -7,7 +7,7 @@
#include "stdio.h"
#include "LPC17xx.h"
#include "Sharp.h"
-
+
#define PIN_TRIGGERL (p12)
#define PIN_ECHOL (p11)
#define PIN_TRIGGERR (p29)
@@ -20,6 +20,7 @@
#define PPR (4331/4)
#define LEFT (1)
#define RIGHT (0)
+#define STRAIGHT (2)
#define FORWARD (1)
#define BACKWARD (0)
#define TOOLS (0)
@@ -27,12 +28,12 @@
#define RIGS (2)
#define FIRST_WAVE (0)
#define FAR (1)
-
-
+
+
float range, range2, pid_return;
void errFunction(void);
bool cRc;
-
+
//Hardware Initialization
Serial bt(p13,p14);
Serial pc(USBTX,USBRX);
@@ -44,10 +45,10 @@
QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
Sharp IR(p20);
//InterruptIn encoder(p29);
-
-
+
+
//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);
@@ -61,19 +62,21 @@
void rig_section(float* location, float ¤t, int* direction, int rig);
void overBump(int section);
void alignWithWall(int section);
+void UntilWall(int dir);
+
//Variables
-
+
int main(void)
{
float location[3], current=0;
int direction[3];
double distance;
-
+
pc.baud(115200);
bt.baud(115200);
motors.begin();
-
+
bt.printf("START\r\n");
//Go to tools
tools_section(location, current);
@@ -85,22 +88,22 @@
wait_ms(20);
rangeFinderLeft.getMeas(range);
bt.printf("Range = %f\n\r", range);
-
+
wait_ms(200);
}*/
-
-
+
+
//leftTurn();
//rightTurn();
-
-
+
+
}
-
+
void errFunction(void)
{
//Nothing
}
-
+
void us_distance(void)
{
pc.printf("Ultra Sonic\n\r");
@@ -110,25 +113,25 @@
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);
@@ -142,7 +145,7 @@
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;
//bt.printf("wavegap %f\r\n",wavegap);
@@ -150,10 +153,10 @@
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
@@ -178,29 +181,33 @@
}
return wavegap;
}
-
+
/* MODIFIED WALL_FOLLOW FOR NAVIGATION */
-
+
void wall_follow2(int side, int direction, int section, float location)
{
- int SeeWaveGap = false, dir=1;
+ bool SeeWaveGap = false, dir=1, limit=83;
float set=5, loc=0;
-
+
pid1.reset();
-
+
if(direction == BACKWARD) dir=-1;
- if(section == TOOLS)set= 5;
+ if(section == TOOLS){
+ set= 5;
+ limit = 78;
+ }
+
leftEncoder.reset();
rightEncoder.reset();
-
- while(dir*loc + location < 78) {
+
+ while(dir*loc + location <= limit) {
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);
@@ -210,25 +217,36 @@
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 */
-
- bt.printf("wall follow 2 range %f\r\n",range);
- bt.printf("loc+location = %f\r\n", loc+location);
- if(range > 20) {
- motors.stopBothMotors();
+
+ if(section == RIGS){
+ rangeFinderRight.startMeas();
+ wait_ms(38);
+ rangeFinderRight.getMeas(range);
+ if(range< 20){
+ motors.stopBothMotors();
+ break;
+ }
+ }
+
+
+ //bt.printf("wall follow 2 range %f\r\n",range);
+ //bt.printf("loc+location = %f\r\n", loc+location);
+ if(range > 20 && !SeeWaveGap) {
+ motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
+ motors.setMotor1Speed(dir*MAX_SPEED);//left
+ wait_ms(30);
+ SeeWaveGap = true;
+ } else if(range > 20 && SeeWaveGap){
+ motors.stopBothMotors();
bt.printf("wavegap\r\n");
// AT WAVE OPENING!!!!
break;
- }
-
+ } else {
+
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
@@ -249,15 +267,15 @@
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();
@@ -266,25 +284,25 @@
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 {
@@ -292,7 +310,7 @@
motors.setMotor0Speed(-0.7*MAX_SPEED); //right
motors.setMotor1Speed(0.7*MAX_SPEED); //left
}
-
+
usValue = 0;
while(1) {
rangeFinderLeft.startMeas();
@@ -307,7 +325,7 @@
}
motors.stopBothMotors();
}
-
+
void rightTurn(void)
{
motors.begin();
@@ -318,7 +336,7 @@
while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
motors.stopBothMotors();
}
-
+
void leftTurn(void)
{
/*
@@ -334,20 +352,43 @@
rightEncoder.reset();
motors.setMotor0Speed(0.5*127);// right
motors.setMotor1Speed(-0.5*127);// left
- while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
+ while(abs(leftEncoder.getPulses())<1100 || rightEncoder.getPulses()<1100);
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);
+ while(abs(leftEncoder.getPulses())<100 || rightEncoder.getPulses()<100);
motors.stopBothMotors();
}
-
+void UntilWall(int dir){
+
+ if(dir == BACKWARD) dir=-1;
+
+ leftEncoder.reset();
+ rightEncoder.reset();
+ motors.setMotor0Speed(dir*0.2*127); //right
+ motors.setMotor1Speed(dir*0.2*127); //left
+
+ range = 30;
+
+ while(range > 20){
+ rangeFinderRight.startMeas();
+ wait_ms(20);
+ rangeFinderRight.getMeas(range);
+ }
+
+ motors.setMotor0Speed(dir*-0.2*127); //right
+ motors.setMotor1Speed(dir*-0.2*127); //left
+ wait_ms(5);
+ motors.stopBothMotors();
+}
+
void overBump(int section)
{
int preLeft=5000, preRight=5000, out=0;
@@ -369,7 +410,7 @@
preLeft=leftEncoder.getPulses();
preRight=rightEncoder.getPulses();
wait_ms(100);
- bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
+ //bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
}
@@ -396,25 +437,25 @@
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;
+ if(abs(leftEncoder.getPulses()) >1000 || abs(leftEncoder.getPulses())>1000) out=1;
}
-
+
motors.stopBothMotors();
wait(2);
motors.begin();
@@ -424,36 +465,50 @@
rightEncoder.reset();
motors.setMotor0Speed(.25*127); //right
motors.setMotor1Speed(.25*127); //left
-
+
if(section == TOOLS || section == MID) {
while(IR.getDistance() > 20 ) {
- bt.printf("IR %f\r\n", IR.getDistance());
- bt.printf("third while left %d right %d \r\n", preLeft, preRight);
+ //bt.printf("IR %f\r\n", IR.getDistance());
+ //bt.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 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;
bt.printf("current %f \r\n",current);
-
+
+
+ motors.setMotor0Speed(-.2*127); //right
+ motors.setMotor1Speed(-.2*127); //left
+ wait_ms(5);
motors.stopBothMotors();
-
+
+ motors.setMotor0Speed(.2*127); //right
+ motors.setMotor1Speed(.2*127); //left
+ while(IR.getDistance()>6);
+
+ motors.setMotor0Speed(-.2*127); //right
+ motors.setMotor1Speed(-.2*127); //left
+ wait_ms(5);
+ motors.stopBothMotors();
+
+
//Tool aquiring
wait(2);
// After tool is aquired
-
+
alignWithWall(TOOLS);
wait_ms(100);
@@ -464,9 +519,13 @@
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
-
+
if(range < 20) {
wall_follow2(LEFT,BACKWARD,TOOLS, current);
+
+ location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ current= location[0];
+
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(-MAX_SPEED); //right
@@ -479,11 +538,8 @@
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);
@@ -493,24 +549,27 @@
wait_ms(20);
overBump(FIRST_WAVE);
}
-
- bt.printf("wavegap = %f\r\n",location[0]);
+
+ bt.printf("First Wavegap = %f\r\n",location[0]);
}
-
+
void mid_section(float* location, float ¤t, int* direction)
{
-
+
motors.begin();
- if(IR.getDistance() > 20) return;
+ if(IR.getDistance() > 20){
+ direction[0]= STRAIGHT;
+ return;
+ }
alignWithWall(MID);
-
+
bt.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);
bt.printf("after wf2 current = %f\r\n",current);
-
+
if(current != 0) {
direction[0]= RIGHT;
current+= location[0];
@@ -521,7 +580,7 @@
wall_follow2(LEFT,BACKWARD,MID,current);
location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
}
-
+
bt.printf("wavegap2 = %f\r\n",location[1]);
leftTurn();
overBump(TOOLS);
@@ -532,20 +591,23 @@
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;
+ if(IR.getDistance() > 20){
+ direction[0]= STRAIGHT;
+ 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];
@@ -556,13 +618,43 @@
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)
{
-
-
+ float loc;
+
+ if(rig == 1) loc= 16;
+ else if(rig == 2) loc= 37;
+ else loc = 58;
+
+ rightTurn();
+
+
+ if(current > loc){
+ UntilWall(BACKWARD);
+ current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ wall_follow2(RIGHT, BACKWARD, RIGS, current);
+ current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+
+ if((current- loc)>10){
+ wall_follow2(RIGHT, BACKWARD, RIGS, current);
+ current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ }
+ }
+ else{
+ UntilWall(FORWARD);
+ wall_follow2(RIGHT, FORWARD, RIGS, current);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+
+ if((current- loc)<-10){
+ wall_follow2(RIGHT, FORWARD, RIGS, current);
+ current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
+ }
+ }
+
+
}
\ No newline at end of file