Team 6: Christine Pang, George Baier, Carlos Guijarro
Our group did a modification to the Face Towards Opponent. We added two additional threshold values, which let us tell the robot when an object was too close and it should back up, and when an object was getting too far away and it should drive towards it.
We added an additional boolean for the robot to check the sensors for our new threshold values, reading true or false based on the object's distance from the robot. Next we modified one of the internal if statements by adding two else ifs to drive the robot forward or backward depending on our new booleans.
Lastly we also added a note for the robot to play when an object gets too close by adding some code to the else if that controlled the backwards movement.
#include <Wire.h>
#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4Motors motors;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;
// A sensors reading must be greater than or equal to this
// threshold in order for the program to consider that sensor as
// seeing an object.
const uint8_t sensorThreshold = 1;
//threshold for program to consider object close enough
const uint8_t distanceThreshold = 4;
const uint8_t tooClose = 6;
// The maximum speed to drive the motors while turning. 400 is
// full speed.
const uint16_t turnSpeedMax = 400;
// The minimum speed to drive the motors while turning. 400 is
// full speed.
const uint16_t turnSpeedMin = 100;
// The amount to decrease the motor speed by during each cycle
// when an object is seen.
const uint16_t deceleration = 10;
// The amount to increase the speed by during each cycle when an
// object is not seen.
const uint16_t acceleration = 10;
#define LEFT 0
#define RIGHT 1
// Stores the last indication from the sensors about what
// direction to turn to face the object. When no object is seen,
// this variable helps us make a good guess about which direction
// to turn.
bool senseDir = RIGHT;
// True if the robot is turning left (counter-clockwise).
bool turningLeft = false;
// True if the robot is turning right (clockwise).
bool turningRight = false;
// If the robot is turning, this is the speed it will use.
uint16_t turnSpeed = turnSpeedMax;
// The time, in milliseconds, when an object was last seen.
uint16_t lastTimeObjectSeen = 0;
void setup()
{
proxSensors.initFrontSensor();
// Wait for the user to press A before driving the motors.
lcd.clear();
lcd.print(F("Press A"));
buttonA.waitForButton();
lcd.clear();
}
void turnRight()
{
motors.setSpeeds(turnSpeed, -turnSpeed);
turningLeft = false;
turningRight = true;
}
void turnLeft()
{
motors.setSpeeds(-turnSpeed, turnSpeed);
turningLeft = true;
turningRight = false;
}
void driveForward()
{
motors.setSpeeds(400, 400);
turningLeft = false;
turningRight = false;
}
void driveBack()
{
motors.setSpeeds(-400,-400);
turningLeft = false;
turningRight = false;
}
void stop()
{
motors.setSpeeds(0,0);
turningLeft = false;
turningRight = false;
}
void loop()
{
// Read the front proximity sensor and gets its left value (the
// amount of reflectance detected while using the left LEDs)
// and right value.
proxSensors.read();
uint8_t leftValue = proxSensors.countsFrontWithLeftLeds();
uint8_t rightValue = proxSensors.countsFrontWithRightLeds();
// Determine if an object is visible or not.
bool objectSeen = leftValue >= sensorThreshold || rightValue >= sensorThreshold;
bool objectClose = leftValue<= distanceThreshold || rightValue <= distanceThreshold;
bool objectTooClose = leftValue>= tooClose || rightValue >= tooClose;
if (objectSeen)
{
// An object is visible, so we will start decelerating in
// order to help the robot find the object without
// overshooting or oscillating.
turnSpeed -= deceleration;
}
else
{
// An object is not visible, so we will accelerate in order
// to help find the object sooner.
turnSpeed += acceleration;
}
// Constrain the turn speed so it is between turnSpeedMin and
// turnSpeedMax.
turnSpeed = constrain(turnSpeed, turnSpeedMin, turnSpeedMax);
if (objectSeen)
{
// An object seen.
ledYellow(1);
lastTimeObjectSeen = millis();
bool lastTurnRight = turnRight;
if (leftValue < rightValue)
{
// The right value is greater, so the object is probably
// closer to the robot's right LEDs, which means the robot
// is not facing it directly. Turn to the right to try to
// make it more even.
turnRight();
senseDir = RIGHT;
}
else if (leftValue > rightValue)
{
// The left value is greater, so turn to the left.
turnLeft();
senseDir = LEFT;
}
else if (objectSeen && objectClose)
{
// The values are equal, so drive forward.
driveForward();
}
else if (objectTooClose)
{
driveBack();
buzzer.playNote(NOTE_G_SHARP(4), 100, 15);
}
else
{
stop();
}
}
else
{
// No object is seen, so just keep turning in the direction
// that we last sensed the object.
ledYellow(0);
if (senseDir == RIGHT)
{
turnRight();
}
else
{
turnLeft();
}
}
lcd.gotoXY(0, 0);
lcd.print(leftValue);
lcd.print(' ');
lcd.print(rightValue);
lcd.gotoXY(0, 1);
lcd.print(turningRight ? 'R' : (turningLeft ? 'L' : ' '));
lcd.print(' ');
lcd.print(turnSpeed);
lcd.print(' ');
lcd.print(' ');
}