Permalink
Browse files

added sweep and decide to drive pattern

  • Loading branch information...
1 parent 1794863 commit 1d231e95f0de527327e2742bbad2c59fd044aed2 @svdgraaf committed Feb 26, 2012
Showing with 29 additions and 9 deletions.
  1. +29 −9 autonomous/autonomous/autonomous.ino
@@ -4,6 +4,7 @@ Servo head;
int distance = 0;
int headRotation = 0;
int pos = 0;
+int minimalReadout = 75;
//Arduino PWM Speed Control
int E1 = 6;
@@ -33,9 +34,9 @@ void loop()
Serial.println(distance);
// if close by an object, start rotating
- if(distance > 100)
+ if(distance > minimalReadout)
{
- Serial.println("uhoh, possible collision detected, sweeping")
+ Serial.println("uhoh, possible collision detected, sweeping");
sweep();
}
else
@@ -98,30 +99,46 @@ void sweep()
// next step found?
if(foundNext)
{
+ // first, backup a little
+ backward();
+ delay(1000);
+
+ // check in which direction we need to go
if(headRotation > 90)
{
// turn left
- left();
- delay(500);
+ turnLeft();
+ delay(1000);
}
else
{
- right();
- delay(500);
+ turnRight();
+ delay(1000);
}
}
}
void turnLeft()
{
+ Serial.println("turning left");
digitalWrite(M1, HIGH);
digitalWrite(M2, LOW);
analogWrite(E1, 128);
analogWrite(E2, 128);
}
+void backward()
+{
+ Serial.println("backward");
+ digitalWrite(M1, LOW);
+ digitalWrite(M2, LOW);
+ analogWrite(E1, 128);
+ analogWrite(E2, 128 );
+}
+
void turnRight()
{
+ Serial.println("turning right");
digitalWrite(M1, LOW);
digitalWrite(M2, HIGH);
analogWrite(E1, 128);
@@ -136,13 +153,16 @@ void centerHead()
boolean measure()
{
- currentReadout = analogRead(0);
+ int currentReadout = analogRead(0);
Serial.print("sees: ");
Serial.println(currentReadout);
// if more distant, set the direction
- if(currentReadout < distance)
+ if(currentReadout < distance && currentReadout < minimalReadout)
{
- headRotation = head.read()
+ headRotation = head.read();
+ return true;
}
+
+ return false;
}

0 comments on commit 1d231e9

Please sign in to comment.