Proj 2 - Zumo Robot 1st Effort Report

Post Reply
glegrady
Posts: 203
Joined: Wed Sep 22, 2010 12:26 pm

Proj 2 - Zumo Robot 1st Effort Report

Post by glegrady » Wed Apr 19, 2017 7:44 pm

Please provide a report of "lessons learned" or tips, or solutions for modifying one of the following Arduino functions. Include photos of your effort, and also attach the code so that others can learn from your results.

BORDER DETECT: Change the behavior by adjusting the speed in
https://github.com/pololu/zumo-32u4-ard ... Detect.ino

FACETowardOpponent - When a Zumo meets another face-to-face, what should it do? Try to approach but not collide, go away or sideways
https://github.com/pololu/zumo-32u4-ard ... ponent.ino

LINE FOLLOWER: See if you can follow the edge of the white tape on the floor
https://github.com/pololu/zumo-32u4-ard ... llower.ino

SUMOProximitySENSORS - Avoid collisions
https://github.com/pololu/zumo-32u4-ard ... ensors.ino

OR you can explore some of the other examples and see how you might change it.
https://github.com/pololu/zumo-32u4-ard ... r/examples
George Legrady
legrady@mat.ucsb.edu

taylormoon2014
Posts: 8
Joined: Wed Apr 12, 2017 5:47 pm

Re: Proj 2 - Zumo Robot 1st Effort Report

Post by taylormoon2014 » Tue Apr 25, 2017 3:00 pm

Group 3: Taylor Moon, Stephanni Larsen, Chantel Chan
Our group project was a modification of the FaceTowardOpponent code. We adjusted the boolean so that ObjectSeen would always be true, and, therefore, the Zumo would always be responding in a circular motion. We commented out excessive code regarding changing directions if it was already in a particular motion and parts about avoidance. The Zumo is mostly controlled by a simple if-statement (if ObjectSeen, then turnRight(); senseDir = RIGHT;). This was an artistic approach to the previously foreclosed options of fight or flight. In light of a sighted opponent, it is now, fight, flight, or dance! This assignment was a gateway for further exploration. With this code, we will later be able to mount a camera to the top that can take panoramic imaging in conjunction with its circular motion. Our assignment worked successfully in class and was coupled with sound. We had a large trial and error process, trying to modify codes of RotationRest and Line follower to get it to move into circular motion but to no avail. It was not until we tried the FaceTowardOpponent code that we struck success.
------------------------------------------------------------------------------------------------------------------------------------

#include <Wire.h>
#include <Zumo32U4.h>

Zumo32U4LCD lcd;
Zumo32U4Motors motors;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4ButtonA buttonA;


// 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 = 1;

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 stop()
{
motors.setSpeeds(0, 0);
turningLeft = false;
turningRight = false;
}

void loop()
{
proxSensors.read();
uint8_t rightValue = proxSensors.countsFrontWithRightLeds();
uint8_t leftValue = proxSensors.countsFrontWithLeftLeds();

bool objectSeen = 1;

if (objectSeen)
{
turnSpeed -= deceleration;
}


turnSpeed = constrain(turnSpeed, turnSpeedMin, turnSpeedMax);

if (objectSeen)
{

ledYellow(1);

lastTimeObjectSeen = millis();

bool lastTurnRight = turnRight;

{
turnRight();
senseDir = RIGHT;
}

}
else
{

ledYellow(0);


turnRight();
}

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(' ');
}

christinepang
Posts: 4
Joined: Wed Apr 12, 2017 5:10 pm

Re: Proj 2 - Zumo Robot 1st Effort Report

Post by christinepang » Wed Apr 26, 2017 10:31 am

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.
IMG_2277.JPG
IMG_3354.JPG
IMG_2274.JPG
#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(' ');
}

taylormoon2014
Posts: 8
Joined: Wed Apr 12, 2017 5:47 pm

Re: Proj 2 - Zumo Robot 1st Effort Report

Post by taylormoon2014 » Wed Apr 26, 2017 10:54 am

https://www.youtube.com/watch?v=3pP_gN6 ... e=youtu.be
^^^^^ THE VIDEO GROUP 3

In this video, we adapted the deceleration speed, increasing it from 100 to 300 in order that the Zumo move faster. We did this because since the assumption with ObjectSeen is that it turns at a deaccelerated speed, then we should increase the speed of deceleration NOT acceleration in order to get it to move faster (deceleration is the only speed at this point that is controlling the Zumo). In the second half of the video, we added a delay function to the "if ObjectSeen" statement in order to add some variety to its response.

anniewong
Posts: 4
Joined: Wed Apr 12, 2017 5:11 pm

Re: Proj 2 - Zumo Robot 1st Effort Report

Post by anniewong » Sun Apr 30, 2017 9:26 pm

Group 2: Leah Armer, Margie Fargas, Annie Wong

Our group attempted to modify the Line Follower code. Our greatest challenge was the acknowledgment that the original code was meant for a black line on a pure white background and that realistically, the space that we were dealing with consists of a white line on a glossy grey background. In trying to teach Leonard, our Zumo, how to identify different color values, we located the position sensor value "2000." We assumed that this value noted the sensor value of the line. We tried various numbers such as 4000, -4000, 100, 200, 0, 1, -0. We found that -0 worked the best but we still ran into multiple errors.
For one, every time we set Leonard towards the ground, only a side of his wheels would be functional, making him turn round and round when he did reach the ground. This barred him from driving in a full line.
We also attempted to completely forgo his ability to circulate by disabling it the function by making both speed differences of his motors positive. This, however, did not affect his line detecting ability. Instead, it feigned the line detecting function because he failed to turn at corners altogether.

In fixing the code, we changed the sensor value to 1000 and adjusted the constant of 4 to balance the derivative constant of 6 to find the appropriate background color that Leonard can detect. In furthering this project, we hope to be able to continue to make trials and errors in finding the color values of different line colors such as red, yellow, blue, and etc. Our project is for Leonard to learn these different line colors and play with that function.

***We have an mp4 video of Leonard successfully following the line but this site does not allow me to attach the file***



Code: Select all

/* This example uses the line sensors on the Zumo 32U4 to follow
a black line on a white background, using a PID-based algorithm.
It works decently on courses with smooth, 6" radius curves and
has been tested with Zumos using 75:1 HP motors.  Modifications
might be required for it to work well on different courses or
with different motors.
This demo requires a Zumo 32U4 Front Sensor Array to be
connected, and jumpers on the front sensor array must be
installed in order to connect pin 4 to DN4 and pin 20 to DN2. */

#include <Wire.h>
#include <Zumo32U4.h>

// This is the maximum speed the motors will be allowed to turn.
// A maxSpeed of 400 lets the motors go at top speed.  Decrease
// this value to impose a speed limit.
const uint16_t maxSpeed = 400;

Zumo32U4Buzzer buzzer;
Zumo32U4LineSensors lineSensors;
Zumo32U4Motors motors;
Zumo32U4ButtonA buttonA;
Zumo32U4LCD lcd;

int16_t lastError = 0;

#define NUM_SENSORS 5
unsigned int lineSensorValues[NUM_SENSORS];

// Sets up special characters in the LCD so that we can display
// bar graphs.
void loadCustomCharacters()
{
  static const char levels[] PROGMEM = {
    0, 0, 0, 0, 0, 0, 0, 63, 63, 63, 63, 63, 63, 63
  };
  lcd.loadCustomCharacter(levels + 0, 0);  // 1 bar
  lcd.loadCustomCharacter(levels + 1, 1);  // 2 bars
  lcd.loadCustomCharacter(levels + 2, 2);  // 3 bars
  lcd.loadCustomCharacter(levels + 3, 3);  // 4 bars
  lcd.loadCustomCharacter(levels + 4, 4);  // 5 bars
  lcd.loadCustomCharacter(levels + 5, 5);  // 6 bars
  lcd.loadCustomCharacter(levels + 6, 6);  // 7 bars
}

void printBar(uint8_t height)
{
  if (height > 8) { height = 8; }
  const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, 255};
  lcd.print(barChars[height]);
}

void calibrateSensors()
{
  lcd.clear();

  // Wait 1 second and then begin automatic sensor calibration
  // by rotating in place to sweep the sensors over the line
  delay(1000);
  for(uint16_t i = 0; i < 120; i++)
  {
    if (i > 30 && i <= 90)
    {
      motors.setSpeeds(-200, 200);
    }
    else
    {
      motors.setSpeeds(200, -200);
    }

    lineSensors.calibrate();
  }
  motors.setSpeeds(0, 0);
}

// Displays a bar graph of sensor readings on the LCD.
// Returns after the user presses A.
void showReadings()
{
  lcd.clear();

  while(!buttonA.getSingleDebouncedPress())
  {
    lineSensors.readCalibrated(lineSensorValues);

    lcd.gotoXY(0, 0);
    for (uint8_t i = 0; i < NUM_SENSORS; i++)
    {
      uint8_t barHeight = map(lineSensorValues[i], 0, 1000, 0, 8);
      printBar(barHeight);
    }
  }
}

void setup()
{
  // Uncomment if necessary to correct motor directions:
  //motors.flipLeftMotor(true);
  //motors.flipRightMotor(true);

  lineSensors.initFiveSensors();

  loadCustomCharacters();

  // Play a little welcome song
  buzzer.play(">g32>>c32");

  // Wait for button A to be pressed and released.
  lcd.clear();
  lcd.print(F("Press A"));
  lcd.gotoXY(0, 1);
  lcd.print(F("to calib"));
  buttonA.waitForButton();

  calibrateSensors();

  showReadings();

  // Play music and wait for it to finish before we start driving.
  lcd.clear();
  lcd.print(F("Go!"));
  buzzer.play("L16 cdegreg4");
  while(buzzer.isPlaying());
}

void loop()
{
  // Get the position of the line.  Note that we *must* provide
  // the "lineSensorValues" argument to readLine() here, even
  // though we are not interested in the individual sensor
  // readings.
  int16_t position = lineSensors.readLine(lineSensorValues, true);

  // Our "error" is how far we are away from the center of the
  // line, which corresponds to position 2000.
  int16_t error = position - 1000;

  // Get motor speed difference using proportional and derivative
  // PID terms (the integral term is generally not very useful
  // for line following).  Here we are using a proportional
  // constant of 1/4 and a derivative constant of 6, which should
  // work decently for many Zumo motor choices.  You probably
  // want to use trial and error to tune these constants for your
  // particular Zumo and line course.
  int16_t speedDifference = error / 2 + 6 * (error - lastError);

  lastError = error;

  // Get individual motor speeds.  The sign of speedDifference
  // determines if the robot turns left or right.
  int16_t leftSpeed = (int16_t)maxSpeed + speedDifference;
  int16_t rightSpeed = (int16_t)maxSpeed - speedDifference;

  // Constrain our motor speeds to be between 0 and maxSpeed.
  // One motor will always be turning at maxSpeed, and the other
  // will be at maxSpeed-|speedDifference| if that is positive,
  // else it will be stationary.  For some applications, you
  // might want to allow the motor speed to go negative so that
  // it can spin in reverse.
  leftSpeed = constrain(leftSpeed, 0, (int16_t)maxSpeed);
  rightSpeed = constrain(rightSpeed, 0, (int16_t)maxSpeed);

  motors.setSpeeds(leftSpeed, rightSpeed);
}
Attachments
IMG_8127.JPG

xinghan_liu
Posts: 4
Joined: Wed Apr 12, 2017 5:48 pm

Re: Proj 2 - Zumo Robot 1st Effort Report (Group #1)

Post by xinghan_liu » Mon May 01, 2017 11:22 am

#include <Wire.h>
#include <Zumo32U4.h>

// This is the maximum speed the motors will be allowed to turn.
// A maxSpeed of 400 lets the motors go at top speed. Decrease
// this value to impose a speed limit.
const uint16_t maxSpeed = 400;

Zumo32U4Buzzer buzzer;
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4Motors motors;
Zumo32U4ButtonA buttonA;
Zumo32U4ButtonB buttonB;
Zumo32U4LCD lcd;

int16_t lastError = 0;

#define NUM_SENSORS 5
unsigned int lineSensorValues[NUM_SENSORS];

// Sets up special characters in the LCD so that we can display
// bar graphs.
void loadCustomCharacters()
{
static const char levels[] PROGMEM = {
0, 0, 0, 0, 0, 0, 0, 63, 63, 63, 63, 63, 63, 63
};
lcd.loadCustomCharacter(levels + 0, 0); // 1 bar
lcd.loadCustomCharacter(levels + 1, 1); // 2 bars
lcd.loadCustomCharacter(levels + 2, 2); // 3 bars
lcd.loadCustomCharacter(levels + 3, 3); // 4 bars
lcd.loadCustomCharacter(levels + 4, 4); // 5 bars
lcd.loadCustomCharacter(levels + 5, 5); // 6 bars
lcd.loadCustomCharacter(levels + 6, 6); // 7 bars
}

void printBar(uint8_t height)
{
if (height > 8) { height = 8; }
const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, 255};
lcd.print(barChars[height]);
}

void calibrateSensors()
{
lcd.clear();

// Wait 1 second and then begin automatic sensor calibration
// by rotating in place to sweep the sensors over the line
delay(1000);
for(uint16_t i = 0; i < 120; i++)
{
if (i > 30 && i <= 90)
{
motors.setSpeeds(-200, 200);
}
else
{
motors.setSpeeds(200, -200);
}

lineSensors.calibrate();
}
motors.setSpeeds(0, 0);
}

// Displays a bar graph of sensor readings on the LCD.
// Returns after the user presses A.
void showReadings()
{
lcd.clear();

while(!buttonA.getSingleDebouncedPress())
{
lineSensors.readCalibrated(lineSensorValues);

lcd.gotoXY(0, 0);
for (uint8_t i = 0; i < NUM_SENSORS; i++)
{
uint8_t barHeight = map(lineSensorValues, 0, 1000, 0, 8);
printBar(barHeight);
}
}
}

void setup()
{
// Uncomment if necessary to correct motor directions:
//motors.flipLeftMotor(true);
//motors.flipRightMotor(true);

lineSensors.initFiveSensors();

loadCustomCharacters();

// Play a little welcome song
buzzer.play(">g32>>c32");

// Wait for button A to be pressed and released.
lcd.clear();
lcd.print(F("Press A"));
lcd.gotoXY(0, 1);
lcd.print(F("to calib"));
buttonA.waitForButton();

calibrateSensors();

showReadings();

// Play music and wait for it to finish before we start driving.
lcd.clear();
lcd.print(F("Go!"));
buzzer.play("L16 cdegreg4");
while(buzzer.isPlaying());
}

void loop()
{
// Get the position of the line. Note that we *must* provide
// the "lineSensorValues" argument to readLine() here, even
// though we are not interested in the individual sensor
// readings.
int16_t position = lineSensors.readLine(lineSensorValues, true);

// Our "error" is how far we are away from the center of the
// line, which corresponds to position 2000.
int16_t error = position - 1000;

// Get motor speed difference using proportional and derivative
// PID terms (the integral term is generally not very useful
// for line following). Here we are using a proportional
// constant of 1/4 and a derivative constant of 6, which should
// work decently for many Zumo motor choices. You probably
// want to use trial and error to tune these constants for your
// particular Zumo and line course.
int16_t speedDifference = error / 2 + 6 * (error - lastError);

lastError = error;

// Get individual motor speeds. The sign of speedDifference
// determines if the robot turns left or right.
int16_t leftSpeed = (int16_t)maxSpeed + speedDifference;
int16_t rightSpeed = (int16_t)maxSpeed - speedDifference;

// Constrain our motor speeds to be between 0 and maxSpeed.
// One motor will always be turning at maxSpeed, and the other
// will be at maxSpeed-|speedDifference| if that is positive,
// else it will be stationary. For some applications, you
// might want to allow the motor speed to go negative so that
// it can spin in reverse.
leftSpeed = constrain(leftSpeed, 0, (int16_t)maxSpeed);
rightSpeed = constrain(rightSpeed, 0, (int16_t)maxSpeed);

motors.setSpeeds(leftSpeed, rightSpeed);
}

eszaboky
Posts: 2
Joined: Wed Apr 12, 2017 5:09 pm

Re: Proj 2 - Zumo Robot 1st Effort Report

Post by eszaboky » Wed May 24, 2017 1:24 pm

Group 5: Emeric Szaboky, Zoë Rathbun, Margarita Fargas
_________________________________________________________________________________________________

Objective: We coded the Roger the Arduino Shybot with the purpose of creating his personality to be curious but careful, a very evolutionarily primal natured robot. He adjusts his direction/orientation based on the objects he is focusing on with his sensors, however when the object comes too close to Roger (within a certain specified threshold), he immediately backs up at the highest speed.

Throughout writing this code and the experimental/physical debugging process, we found some finicky responses to certain threshold choices. We wanted objects to be able come as close as possible to Roger before he retreated; so, intuitively, we chose the largest threshold value. We experienced problems with this however: the object would be able to get too close to Roger and his sensors would become confused and not respond. We started to notice that when increasing the threshold by constant increments, Rogers actions were not changing predictably. It became clear that the threshold spectrum was alinear, maybe changing exponentially. This caused the higher threshold values to correspond to such a short distance between Roger and an object that it was almost no distance. Therefore, the object could approach until it was touching Roger and his sensors would malfunction, causing him to not back up. Through continual experimentation, we found the proper threshold values, which allowed for the minimum distance between Roger and an object, while still allowing him to respond appropriately and back away.
_________________________________________________________________________________________________

Documentation:

https://vimeo.com/218861528

https://vimeo.com/218862120

_________________________________________________________________________________________________

Creative Code Edit for FaceTowardsOpponent:

Code: Select all

/* This example uses the front proximity sensor on the Zumo 32U4
Front Sensor Array to locate an opponent robot or any other
reflective object. Using the motors to turn, it scans its
surroundings. If it senses an object, it turns on its yellow LED
and attempts to face towards that object. */

#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;


// new threshold for program to determine when robot should react to object
const uint8_t veryClose = 6.95;   // code below tells robot to back up in response to this threshold closeness
const uint8_t safeDistance = 4;  // code below tells robot to stop backing up in response to this threshold closeness

// 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 driveReverse()
{
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 objectVeryClose = leftValue >= veryClose || rightValue >= veryClose; // boolean checking if @ veryClose
  bool objectSafeDistance = leftValue <= safeDistance || rightValue <= safeDistance;  // boolean checking if @ safeDistance


  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 (objectVeryClose)
    {
      driveReverse();
      buzzer.playNote(NOTE_A(4), 100, 15);
    }
    else if (objectSafeDistance)
    {
      stop();
    }
    else
    {
      // The values are equal, so stop the motors.
      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(' ');
}

Post Reply