The Dream Team
Pop lock n drop it
Diff: main.cpp
- Revision:
- 10:86cb40bf1c17
- Parent:
- 9:493011bacdcd
- Child:
- 11:af3d70d153bb
--- a/main.cpp Mon May 26 09:50:52 2014 +0000
+++ b/main.cpp Mon Jun 02 08:44:08 2014 +0000
@@ -32,16 +32,20 @@
//ghtSen = sensors[3] / 1000;
//dSen = sensors[2] / 1000;
float val;
- val = (((1000*sensors[2] + 2000*sensors[3])/(sensors[1]+sensors[2]+sensors[3])));
- return -1+(val/1000);
-
+ if(sensors[1] < 50 && sensors[2] < 50 && sensors[3] < 50)
+ return -1;
+ else
+ {
+ val = (((1000*sensors[2] + 2000*sensors[3])/(sensors[1]+sensors[2]+sensors[3])));
+ return (-1)+(val/1000);
+ }
}
int rightCorner()
{
int sensors[5];
huey.calibrated_sensor(sensors);
- if(sensors[4] > 900)
+ if(sensors[4] > 950)
return 1;
else
return 0;
@@ -51,93 +55,127 @@
{
int sensors[5];
huey.calibrated_sensor(sensors);
- if(sensors[0] > 900)
+ if(sensors[0] > 950)
return 1;
else
return 0;
}
-//direction varriable is between 0 and 1, 0 meaning left 1 meaning right
-//returns 1 if it turned, 0 otherwise
-/*int crossing(float speed, int direction)
+//returns 0 if side sensors aren't light, 1 if light
+int sideSensorsLight(void)
+{
+ int sensors[5];
+ huey.calibrated_sensor(sensors);
+ if(sensors[4] > 50 && sensors[0] > 50)
+ return 0;
+ else
+ return 1;
+}
+
+//returns 0 if side sensors aren't dark, 1 if dark
+int sideSensorsDark(void)
{
if(rightCorner() == 1 && leftCorner() == 1)
+ return 1;
+ else
+ return 0;
+}
+
+void rightTurn(float speed)
+{
+ int side = sideSensorsLight();
+ huey.forward(speed/2);
+ while(side == 0)
{
- if(direction == 0)
- {
- huey.left(speed*2);
- return 1;
- }
- else
- {
- huey.right(speed*2);
- return 1;
- }
+ side = sideSensorsLight();
}
- else
+ wait(0.20);
+ float pos = myLinePos();
+ huey.right(speed/2);
+ while(pos > -0.1 && pos < 0.1)
{
- if(rightCorner() == 1)
- {
- huey.right(speed*2);
- return 0;
- }
- else if(leftCorner() == 1)
- {
- huey.left(speed*2);
- return 0;
- }
- else
- return 0;
+ pos = myLinePos();
}
-
+ pos = myLinePos();
+ while(pos > 0.05 || pos < -0.05)
+ {
+ pos = myLinePos();
+ }
}
-*/
+
+void leftTurn(float speed)
+{
+ int side = sideSensorsLight();
+ huey.forward(speed/2);
+ while(side == 0)
+ {
+ side = sideSensorsLight();
+ }
+ wait(0.20);
+
+ float pos = myLinePos();
+ huey.left(speed/2);
+ while(pos > -0.1 && pos < 0.1)
+ {
+ pos = myLinePos();
+ }
+ pos = myLinePos();
+ while(pos > 0.05 || pos < -0.05)
+ {
+ pos = myLinePos();
+ }
+}
+
void smoothFollow(float position, float speed)//, int direction)
{
float u = speed;
u = u * position;
-
- /*if(rightCorner() == 1 && leftCorner() == 1)
- {
- if(direction == 0)
- {
- huey.left(speed*2);
- return 1;
- }
- else
- {
- huey.right(speed*2);
- return 1;
- }
- }
- else
- {*/
+
if(rightCorner() == 1)
- {
- int sensors[5];
- huey.calibrated_sensor(sensors);
- while(sensors[4] > 100)
+ {
+ rightTurn(speed);
+ /*int side = sideSensorsLight();
+ huey.forward(speed/2);
+ while(side == 0)
{
- huey.forward(speed);
- huey.calibrated_sensor(sensors);
+ side = sideSensorsLight();
}
- //Stops and semi-properly turns at corners and t's but gets screwed up at + intersections because he turns until the middle sensors read black
- // But at a + intersection the sensors are black already so he doesnt turn
- huey.stop();
- wait(0.1);
- float pos = -1;
+ wait(0.20);
+ float pos = myLinePos();
+ huey.right(speed/2);
+ while(pos > -0.1 && pos < 0.1)
+ {
+ pos = myLinePos();
+ }
+ pos = myLinePos();
while(pos > 0.05 || pos < -0.05)
{
- huey.right(speed);
pos = myLinePos();
- }
-
- //return 0;
+ }*/
}
else if(leftCorner() == 1)
{
- huey.left(speed*2);
- //return 0;
+ leftTurn(speed);
+ /*
+ int side = sideSensorsLight();
+ huey.forward(speed/2);
+ while(side == 0)
+ {
+ side = sideSensorsLight();
+ }
+ wait(0.20);
+
+ float pos = myLinePos();
+ huey.left(speed/2);
+ while(pos > -0.1 && pos < 0.1)
+ {
+ pos = myLinePos();
+ }
+ pos = myLinePos();
+ while(pos > 0.05 || pos < -0.05)
+ {
+ pos = myLinePos();
+ }*/
}
else if(speed+u > 1)
{
@@ -149,10 +187,7 @@
{
huey.right_motor(speed-u);
huey.left_motor(speed+u);
- //return 0;
}
- //}
-
}
@@ -163,7 +198,7 @@
float i;
for(i = speed; i > 0; i = i - (speed/steps))
{
- //FIGURE THIS OUT KJALSHDFKJASHFLKJASHFLKJASHDLKJASHDLKJHASDLKJHASLKDJH
+
smoothFollow(huey.line_position(),i);
wait(waitTime);
}
@@ -182,6 +217,23 @@
/////////////////////////////////////////////////////////////////
int main() {
+
+
+ //Matrix of paths from one point to another: Right = R, Left = L, Straight = S
+ char* map[6][6] =
+ {
+ {"", "LRLLR", "LRLSS", "LRLSRLL", "LRRLSLRRRL", "LRRLR"},
+ {"LRRLR", "", "LLS", "LLRLL", "LSRLRRL", "LRSLR"},
+ {"SSRLR", "SRR", "", "LLL", "LLRSRL", "SSSLR"},
+ {"RLSRLR", "RLRR", "RR", "", "SSRL", "SRLRL"},
+ {"RLLLRSRLLR", "RLLRLSR", "RLSLR", "RLSS", "", "RLLLRL"},
+ {"LRLLR", "LRSLR", "LRSSS", "RLRLS", "RLRRRL", ""}
+ };
+
+
+
+
+
//time_t startt, endt;
//time(&startt);
//clock_t clock;
@@ -230,7 +282,7 @@
if (iline <2){
//time(&endt);
//btbee.printf("%f seconds - ", clock);
- btbee.printf("Battery Voltage: %f\n", huey.battery());
+ //btbee.printf("Battery Voltage: %f\n", huey.battery());
m3pi_led[0]=0;
huey.locate(0,0);
huey.printf("Sent %0.3d",iline);
@@ -275,28 +327,11 @@
float speed = 0.25;
float pos;
int z=1;
- int turns[3];
- turns[0] = 0;
- turns[1] = 1;
- turns[2] = 0;
-
+
while(z==1)
{
-
- //Trying to get it to follow a sequence of turns
- /*for(int q = 0; q < sizeof(turns); q++)
- {
- int change = 0;
- while(change == 0)
- {
- //change = crossing(speed, turns[q]);
- pos = myLinePos();
- change = smoothFollow(pos, speed, turns[q]);
-
- }
- }*/
-
+
pos = myLinePos();