Paolina Povolotskaya
revise
Dependencies: HCSR04 PID PololuQik2 QEI mbed-rtos Sharp
Diff: main.cpp
- Revision:
- 9:f34700716f1d
- Parent:
- 8:11ef93eebe07
- Child:
- 10:c57f6a5042d7
diff -r 11ef93eebe07 -r f34700716f1d main.cpp
--- a/main.cpp Sat Mar 22 21:47:14 2014 +0000
+++ b/main.cpp Thu Mar 27 17:49:30 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)
@@ -21,18 +21,18 @@
#define LEFT (1)
#define RIGHT (0)
#define FORWARD (1)
-#define BACKWARD (0)
+#define BACKWARD (0)
#define TOOLS (0)
#define MID (1)
-#define RIGS (2)
+#define RIGS (2)
#define FIRST_WAVE (0)
#define FAR (1)
-
-
-float range, pid_return;
+
+
+float range, range2, pid_return;
void errFunction(void);
bool cRc;
-
+
//Hardware Initialization
Serial bt(p13,p14);
Serial pc(USBTX,USBRX);
@@ -44,14 +44,15 @@
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);
void leftTurn(void);
+void slightleft(void);
void rightTurn(void);
void us_distance(void);
void tools_section(float* location, float ¤t);
@@ -60,37 +61,46 @@
void rig_section(float* location, float ¤t, int* direction, int rig);
void overBump(int section);
void alignWithWall(int section);
-
+
//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);
mid_section(location, current, direction);
- mid_section2(location, current, direction);
- /*while(1){
- bt.printf("IR %f\r\n", IR.getDistance());
+ //mid_section2(location, current, direction);
+ /* while(1) {
+ //bt.printf("IR %f\r\n", US.getDistance());
+ rangeFinderLeft.startMeas();
+ 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");
@@ -100,71 +110,67 @@
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< 78) {
+
+ 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){
+ if(side) {
rangeFinderLeft.startMeas();
wait_ms(38);
rangeFinderLeft.getMeas(range);
- }
- else{
+ } 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;
//bt.printf("wavegap %f\r\n",wavegap);
// AT WAVE OPENING!!!!
- motors.setMotor1Speed(dir*0.3*127);//left
- motors.setMotor0Speed(dir*0.3*127);//right
- }
- else{
-
+ 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){
+ if(side) {
motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
motors.setMotor1Speed(dir*MAX_SPEED);//left
- }
- else{
+ } else {
motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
motors.setMotor0Speed(dir*MAX_SPEED);//right
}
- }else if(pid_return < 0) {
- if(side){
+ } else if(pid_return < 0) {
+ if(side) {
motors.setMotor0Speed(dir*MAX_SPEED);//right
motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
- }
- else{
+ } else {
motors.setMotor1Speed(dir*MAX_SPEED);//left
motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
}
- }else {
+ } else {
motors.setMotor0Speed(dir*MAX_SPEED);//right
motors.setMotor1Speed(dir*MAX_SPEED);//left
}
@@ -172,45 +178,44 @@
}
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(loc + dir*location < 80) {
+
+ 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){
+
+ if(side) {
rangeFinderLeft.startMeas();
wait_ms(38);
rangeFinderLeft.getMeas(range);
- }
- else{
+ } 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 */
-
+
bt.printf("wall follow 2 range %f\r\n",range);
bt.printf("loc+location = %f\r\n", loc+location);
if(range > 20) {
@@ -219,26 +224,24 @@
// 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){
+
+ if(pid_return > 0) {
+ if(side) {
motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
motors.setMotor1Speed(dir*MAX_SPEED);//left
- }
- else{
+ } else {
motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
motors.setMotor0Speed(dir*MAX_SPEED);//right
}
- }else if(pid_return < 0) {
- if(side){
+ } else if(pid_return < 0) {
+ if(side) {
motors.setMotor0Speed(dir*MAX_SPEED);//right
motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
- }
- else{
+ } else {
motors.setMotor1Speed(dir*MAX_SPEED);//left
motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
}
@@ -249,38 +252,39 @@
}
motors.stopBothMotors();
}
-
-
-void alignWithWall(int section){
+
+
+void alignWithWall(int section)
+{
float usValue = 0;
-
- if(section == TOOLS){
+
+ if(section == TOOLS) {
// turn at an angle
leftEncoder.reset();
- rightEncoder.reset();
- motors.setMotor0Speed(-MAX_SPEED); //right
+ 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();
+ 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
+
+ // turn left towards wall
leftEncoder.reset();
rightEncoder.reset();
motors.setMotor0Speed(MAX_SPEED); //right
motors.setMotor1Speed(-MAX_SPEED); //left
- while(rightEncoder.getPulses() < 100);
-
- motors.stopBothMotors();
-
+ 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 {
@@ -288,165 +292,238 @@
motors.setMotor0Speed(-0.7*MAX_SPEED); //right
motors.setMotor1Speed(0.7*MAX_SPEED); //left
}
-
+
usValue = 0;
- while(1){
+ while(1) {
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
//bt.printf("Range %f \t OldValue %f\n\r",range, usValue);
- if(range > usValue && usValue != 0 && range < 25){
+ if(range > usValue && usValue != 0 && range < 25) {
break;
} else {
- usValue = range;
+ usValue = range;
}
}
motors.stopBothMotors();
}
-
+
void rightTurn(void)
{
+ motors.begin();
leftEncoder.reset();
rightEncoder.reset();
- motors.setMotor0Speed(-0.4*127);//right
- motors.setMotor1Speed(0.4*127);//left
- while(leftEncoder.getPulses()<1000 || rightEncoder.getPulses()>-1000);
+ 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*127);// right
- motors.setMotor1Speed(-0.4*127);// left
+ 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 overBump(int section){
- int preLeft=5000, preRight=5000 ;
+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.15*127); //right
- motors.setMotor1Speed(0.15*127); //left
+ 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(200);
- bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
- if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+ preLeft=leftEncoder.getPulses();
+ preRight=rightEncoder.getPulses();
+ wait_ms(100);
+ bt.printf(" first while left %d right %d \r\n", preLeft, preRight);
+ if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
}
motors.stopBothMotors();
- motors.setMotor0Speed(0.15*127); //right
- motors.setMotor1Speed(0.15*127); //left
- preLeft=preRight=5000 ;
+ 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();
+ bt.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();
-
- while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getDistance() >20 && preLeft!=0){
+ motors.setMotor0Speed(0.3*127); //right
+ motors.setMotor1Speed(0.3*127); //left
+
+ while(!out) {
preLeft=leftEncoder.getPulses();
preRight=rightEncoder.getPulses();
- bt.printf("second while left %d right %d \r\n", preLeft, preRight);
- wait_ms(200);
- if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
+
+ 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(MAX_SPEED); //right
- motors.setMotor1Speed(MAX_SPEED); //left
-
- if(section == TOOLS){
+ 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);
+ }
+ } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
- preLeft=leftEncoder.getPulses();
- preRight=rightEncoder.getPulses();
- bt.printf("third while left %d right %d \r\n", preLeft, preRight);
- wait_ms(20);
- if(leftEncoder.getPulses() == preLeft && rightEncoder.getPulses()== preRight){
- motors.setMotor0Speed(0.4*127); //right
- motors.setMotor1Speed(0.4*127); //left
- }
- }
- }
- else while((abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses())< 300));
-
+ 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);
+
+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.stopBothMotors();
-
+
//Tool aquiring
wait(2);
// After tool is aquired
-
+
alignWithWall(TOOLS);
- current-=8;
+
wait_ms(100);
+ wall_follow2(LEFT,FORWARD,MID, current);
+ current= 78;
+
rangeFinderLeft.startMeas();
wait_ms(20);
rangeFinderLeft.getMeas(range);
-
- if(range < 20){
+
+ 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;
+ } else {
+ location[0]= 77;
leftTurn();
wait_ms(20);
overBump(FIRST_WAVE);
- }
-
+ }
+
bt.printf("wavegap = %f\r\n",location[0]);
}
-
-void mid_section(float* location, float ¤t, int* direction){
+
+void mid_section(float* location, float ¤t, int* direction)
+{
+
+ motors.begin();
- motors.begin();
+ if(IR.getDistance() > 20) 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){
+
+ if(current != 0) {
direction[0]= RIGHT;
current+= location[0];
location[1]= current;
- }
- else{
- current=location[0];
+ } 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);
}
-
+
bt.printf("wavegap2 = %f\r\n",location[1]);
- leftTurn();
+ leftTurn();
overBump(TOOLS);
// go forward
leftEncoder.reset();
@@ -455,33 +532,37 @@
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();
-void mid_section2(float* location, float ¤t, int* direction){
+ if(IR.getDistance() > 20) return;
- motors.begin();
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){
+
+ if(current != 0) {
direction[1]= RIGHT;
current+= location[1];
location[2]= current;
- }
- else{
- current=location[1];
+ } 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();
+
+ leftTurn();
overBump(RIGS);
-}
-
-void rig_section(float* location, float ¤t, int* direction, int rig){
-
-
+}
+
+void rig_section(float* location, float ¤t, int* direction, int rig)
+{
+
+
}
\ No newline at end of file