HI Jon,
Thanks for the answers; we have managed to put in the LED countdown before starting… as you can see in the youtube video, we have reduced to speed to half (200) and then it is more or less ok, but as in the following video, running at 400 max speed, it just goes nuts… we have tried changing the standard 4 + 6 to anything higher or lower but we dont see any difference… https://youtu.be/GT9uE8g3p6Q
second part of the assignment for the competition is that after following the line, a can must be detected and pushed out… we have the code on how to scan for an object within the correct distance and to push it… how can we merge both code? meaning how to get the zumo to stop when there is no more line and go to the second command?
i put the codes below…
the line following:
#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 = 250;
Zumo32U4Buzzer buzzer;
Zumo32U4LineSensors lineSensors;
Zumo32U4Motors motors;
Zumo32U4ButtonA buttonA;
Zumo32U4ButtonB buttonB;
Zumo32U4LCD lcd;
Zumo32U4ProximitySensors proxSensors;
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);
}
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 calb"));
buttonA.waitForButton();
calibrateSensors();
lcd.clear();
lcd.print(F("Press B"));
lcd.gotoXY(0, 1);
lcd.print(F("to start"));
buttonB.waitForButton();
{
delay(1000);
// Turn the LEDs on.
ledRed(1);
// Wait for a second.
delay(1000);
// Turn the LEDs off.
ledRed(0);
// Turn the LEDs on.
ledYellow(1);
// Wait for a second.
delay(1000);
// Turn the LEDs off.
ledYellow(0);
// Turn the LEDs on.
ledGreen(1);
// Wait for a second.
delay(1000);
// Turn the LEDs off.
ledGreen(0);
// Turn the LEDs on.
ledRed(1);
// Wait for a second.
delay(1000);
// Turn the LEDs off.
ledRed(0);
// Turn the LEDs on.
ledYellow(1);
// Wait for a second.
delay(1000);
// Turn the LEDs off.
ledYellow(0);
}
lcd.clear();
lcd.print(F("Go!"));
}
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);
// Our "error" is how far we are away from the center of the
// line, which corresponds to position 2000.
int16_t error = position - 2000;
// 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 / 4;
lastError = error;
ledRed(1);
// Get individual motor speeds. The sign of speedDifference
// determines if the robot turns left or right.
int16_t leftSpeed = (int16_t)maxSpeed + (speedDifference + 20);
int16_t rightSpeed = (int16_t)maxSpeed - (speedDifference + 20);
leftSpeed = constrain(leftSpeed, 0, (int16_t)maxSpeed);
rightSpeed = constrain(rightSpeed, 0, (int16_t)maxSpeed);
motors.setSpeeds(leftSpeed, rightSpeed);
}
for the scan and ram:
#include <Wire.h>
#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4Motors motors;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4ButtonA buttonA;
// 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 = 4;
// The maximum speed to drive the motors while turning. 400 is
// full speed.
const uint16_t turnSpeedMax = 200;
// 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 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;
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);
motors.setSpeeds(200, 200);
delay(300);
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
{
// 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(' ');
}
