Robot not turning on one set of code, but turns on the other similar set of code - arduino-uno

Question

Im making an alarm robot using the Sparkfun Redbot for a class project. When the robot detects something in front of it, or detects a drop off, it would turn 90 degrees. It worked on the Autonomous Navigation Test file, but it doesn't turn with the Prototype code file, both Autonomous Navigation functions are the same.
Autonomous Navigation Code
// SparkFun RedBot Library
#include <RedBot.h>
#include <RedBotSoftwareSerial.h>
// create objects using classes in RedBot library
RedBotButton button;
RedBotMotors motors;
RedBotEncoder encoder(A2, 10);
RedBotSensor leftLine(A3);
RedBotSensor centerLine(A6);
RedBotSensor rightLine(A7);
// global variables for buzzer and LED pin numbers
const int buzzer = 9;
const int led = 13;
// global variables for ultrasonic sensor pin numbers
const int TRIG_PIN = A0;
const int ECHO_PIN = A1;
// global variables to keep track of which scenario to demonstrate
int scenario = 1;
boolean started = false;
// variables for wheel encoder counts and motor powers
long leftCount, rightCount;
long prevLeftCount, prevRightCount;
int leftPower, rightPower;
const int motorPower = 150; // change value if needed - used by driveStraight() function
void setup() {
pinMode(buzzer, OUTPUT);
pinMode(led, OUTPUT);
// ultrasonic sensor setup
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
digitalWrite(TRIG_PIN, LOW);
}
void loop() {
checkButton();
if (started == true) {
if (scenario == 1) scenario1();
else if (scenario == 2) scenario2();
else if (scenario == 3) scenario3();
}
}
void scenario1() {
// Scenario 1: Drive autonomously by avoiding lines and obstacles
unsigned long time = millis(); // get current time in milliseconds
unsigned long endTime = time + 30000; // make end time 30 seconds from now
// while current time is less than end time, run demo
while (time < endTime) {
avoidCollision();
checkDropOff();
time = millis(); // check current time again
}
// time's up
motors.stop();
alertSound();
// set global variables for 2nd scenario
scenario = 2;
started = false;
}
void scenario2() {
// add code for Scenario 2
// set global variables for 3rd scenario
scenario = 3;
started = false;
}
void scenario3() {
// add code for Scenario 3
// reset global variables for 1st scenario
scenario = 1;
started = false;
}
//---------------------------------------------------------
void checkDropOff() {
// IR threshold indicating surface drop-off (table edge, hole, etc.)
int dropOff = 950; // adjust value if necessary
// get IR sensor readings
int leftSensor = leftLine.read();
int centerSensor = centerLine.read();
int rightSensor = rightLine.read();
// see if any IR sensors detect drop-off
if (leftSensor > dropOff || centerSensor > dropOff || rightSensor > dropOff) {
// add code to perform (brake, reverse, change direction, etc.)
motors.brake();
delay(1000);
motors.drive(-150); // back up
delay(500);
motors.brake();
pivotAngle(90);
started = false; // allow test to be repeated
}
}
void checkButton() {
if (button.read() == true) {
// reverse value of "started"
started = !started;
// single-blink and single-beep as feedback
digitalWrite(led, HIGH);
tone(buzzer, 3000);
delay(200);
digitalWrite(led, LOW);
noTone(buzzer);
}
}
void alertSound() {
// modify code to play whatever sound pattern you want
for (int i = 0; i < 3; i++) {
tone(buzzer, 4000);
delay(100);
noTone(buzzer);
delay(100); // pause before next beep
}
}
void driveDistance(float distance, int power) {
// use wheel encoders to drive straight for specified distance at specified power
// set initial power for left and right motors
int leftPower = power;
int rightPower = power;
// amount to offset motor powers to drive straight
int offset = 5;
// if negative distance, make powers & offset also negative
if (distance < 0) {
leftPower *= -1;
rightPower *= -1;
offset *= -1;
}
// adjust distance to improve accuracy
float correction = -1.0; // change value based on test results
if (distance > 0) distance += correction;
else if (distance < 0) distance -= correction;
// variables for tracking wheel encoder counts
long leftCount = 0;
long rightCount = 0;
long prevLeftCount = 0;
long prevRightCount = 0;
long leftDiff, rightDiff;
// RedBot values based on encoders, motors & wheels
float countsPerRev = 192.0; // 192 encoder ticks per wheel revolution
float wheelDiam = 2.56; // wheel diameter = 65 mm = 2.56 in
float wheelCirc = PI * wheelDiam; // wheel circumference = 3.14 x 2.56 in = 8.04 in
// based on distance, calculate number of wheel revolutions
float numRev = distance / wheelCirc;
// calculate target encoder count
float targetCount = numRev * countsPerRev;
// reset encoder counters and start driving
encoder.clearEnc(BOTH);
delay(100);
motors.leftDrive(leftPower);
motors.rightDrive(rightPower);
// keeps looping while right encoder count less than target count
while (abs(rightCount) < abs(targetCount)) {
// get current wheel encoder counts
leftCount = encoder.getTicks(LEFT);
rightCount = encoder.getTicks(RIGHT);
// calculate increase in count from previous reading
leftDiff = abs(leftCount - prevLeftCount);
rightDiff = abs(rightCount - prevRightCount);
// store current counts as "previous" counts for next reading
prevLeftCount = leftCount;
prevRightCount = rightCount;
// adjust left & right motor powers to keep counts similar (drive straight)
// if left rotated more than right, slow down left & speed up right
if (leftDiff > rightDiff) {
leftPower = leftPower - offset;
rightPower = rightPower + offset;
}
// else if right rotated more than left, speed up left & slow down right
else if (leftDiff < rightDiff) {
leftPower = leftPower + offset;
rightPower = rightPower - offset;
}
// apply adjusted motor powers
motors.leftDrive(leftPower);
motors.rightDrive(rightPower);
delay(10); // short delay before next reading
}
// target count reached
motors.brake(); // or use: motors.stop()
delay(500); // brief delay to wait for complete stop
}
void pivotAngle(float angle) {
// use wheel encoders to pivot (turn) by specified angle
// set motor power for pivoting
int power = 100; // turn CW
if (angle < 0) power *= -1; // use negative power to turn CCW
// adjust angle to improve accuracy
float correction = -5.0; // change value based on test results
if (angle > 0) angle += correction;
else if (angle < 0) angle -= correction;
// variable for tracking wheel encoder counts
long rightCount = 0;
// RedBot values based on encoders, motors & wheels
float countsPerRev = 192.0; // 192 encoder ticks per wheel revolution
float wheelDiam = 2.56; // wheel diameter = 65 mm = 2.56 in
float wheelCirc = PI * wheelDiam; // wheel circumference = 3.14 x 2.56 in = 8.04 in
float pivotDiam = 6.125; // pivot diameter = distance between centers of wheel treads = 6.125 in
float pivotCirc = PI * pivotDiam; // pivot circumference = 3.14 x 6.125 in = 19.23 in
// based on angle, calculate distance (arc length)
float distance = abs(angle) / 360 * pivotCirc;
// based on distance, calculate number of wheel revolutions
float numRev = distance / wheelCirc;
// calculate target encoder count
float targetCount = numRev * countsPerRev;
// reset encoder counters and start pivoting
encoder.clearEnc(BOTH);
delay(100);
motors.pivot(power);
// keeps looping while right encoder count less than target count
while (abs(rightCount) < abs(targetCount)) {
// get current wheel encoder count
rightCount = encoder.getTicks(RIGHT);
delay(10); // short delay before next reading
}
// target count reached
motors.brake();
delay(250);
// clearEncoders(); // only needed if using driveStraight() or countLine()
}
void avoidLine() {
/* AVOID LINE
To avoid dark line on light surface:
Use high threshold & see if sensors greater than threshold
To avoid light line on dark surface:
Use low threshold & see if sensors less than threshold
Use test readings from line to determine best value for threshold
*/
// adjust value if necessary
int lineThreshold = 800;
// get IR sensor readings
int leftSensor = leftLine.read();
int rightSensor = rightLine.read();
// when either sensor on line, first brake motors
if (leftSensor > lineThreshold || rightSensor > lineThreshold) {
motors.brake();
delay(250);
}
// when both sensors on line, turn around
if (leftSensor > lineThreshold && rightSensor > lineThreshold) {
long rnd = random(750, 1250);
motors.pivot(100);
delay(rnd);
motors.stop();
}
// when line under left sensor only, pivot right
else if (leftSensor > lineThreshold) {
long rnd = random(500, 750);
motors.pivot(100);
delay(rnd);
motors.stop();
}
// when line under right sensor only, pivot left
else if (rightSensor > lineThreshold) {
long rnd = random(500, 750);
motors.pivot(-100);
delay(rnd);
motors.stop();
}
delay(25); // change delay to adjust line detection sensitivity
}
void avoidCollision() {
// set minimum distance between RedBot and obstacle
float minDist = 8.0; // change value as necessary (need decimal)
// measure distance to nearest obstacle
float sensorDist = measureDistance();
// if obstacle is too close, avoid collision
if (sensorDist <= minDist) {
// add code to perform (brake, change direction, etc.)
motors.brake();
pivotAngle(90);
}
else{
motors.drive(120);
}
delay(60); // need min 60ms between ultrasonic sensor readings
}
float measureDistance() {
// declare local variables for function
unsigned long time1;
unsigned long time2;
unsigned long pulse_time;
float dist_cm;
float dist_in;
// trigger ultrasonic signal for minimum 10 microseconds
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
// wait for echo to be received
while (digitalRead(ECHO_PIN) == 0);
// now measure how long echo lasts (pulse time)
time1 = micros(); // get start time in microseconds
while (digitalRead(ECHO_PIN) == 1); // wait until echo pulse ends
time2 = micros(); // get end time
pulse_time = time2 - time1; // subtract to get amount of time
// pulse time of 23200 represents maximum range for sensor
if (pulse_time > 23200) pulse_time = 23200;
/*
CALCULATE DISTANCE TO OBJECT USING PULSE TIME
distance = pulse time * speed of sound / 2
Divide by 2 because pulse time includes signal traveling to object and then back
Speed of sound in air at sea level is approximately 340 m/s
Numbers below are calculated constants for cm or inches
Decimal point in numbers necessary for accurate float calculation
*/
dist_cm = pulse_time / 58.0;
dist_in = pulse_time / 148.0;
// return measurement (dist_cm or dist_in)
return dist_in;
}
Prototype Code:
// SparkFun RedBot Library - Version: Latest
#include <RedBot.h>
#include <RedBotSoftwareSerial.h>
// GLOBAL VARIABLES AND OBJECTS
// create objects using classes in RedBot library
RedBotButton button;
RedBotMotors motors;
RedBotEncoder encoder(A2, 10);
RedBotSensor leftLine(A3);
RedBotSensor centerLine(A6);
RedBotSensor rightLine(A7);
// global variables for buzzer and LED pin numbers
const int buzzer = 9;
const int led = 13;
// global variables for ultrasonic sensor pin numbers
const int TRIG_PIN = A0;
const int ECHO_PIN = A1;
// global variables to keep track of which scenario to demonstrate
int scenario = 1;
boolean started = false;
// variables for wheel encoder counts and motor powers
long leftCount, rightCount;
long prevLeftCount, prevRightCount;
int leftPower, rightPower;
const int motorPower = 150; // change value if needed - used by driveStraight() function
int speaker = 9;
// SETUP FUNCTION
void setup() {
// put your setup code here, to run once:
pinMode(buzzer, OUTPUT);
pinMode(led, OUTPUT);
pinMode(speaker, OUTPUT);
// ultrasonic sensor setup
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
digitalWrite(TRIG_PIN, LOW);
}
// LOOP FUNCTION
void loop() {
checkButton();
if (started == true) {
if (scenario == 1) scenario1();
else if (scenario == 2) scenario2();
else if (scenario == 3) scenario3();
}
}
// CUSTOM FUNCTIONS
void scenario1() {
// add code to perform task scenario 1
tone(speaker, 500);
delay(500); //0.5s
noTone(speaker);
delay(10000); //10s
tone(speaker, 500);
// at end of this task, reset for next task
scenario = 2;
started = false;
}
void scenario2() {
// add code to perform task scenario 2
noTone(speaker);
delay(10000); //10s
tone(speaker, 500);
// at end of this task, reset for next task
scenario = 3;
started = false;
}
void scenario3() {
// add code to perform task scenario 3
unsigned long time = millis(); // get current time in milliseconds
unsigned long endTime = time + 600000; // make end time 10 min from now
// while current time is less than end time, run demo
while (time < endTime) {
avoidCollision();
checkDropOff();
time = millis(); // check current time again
}
// time's up
motors.stop();
alertSound();
// at end of this task, reset for next task
scenario = 1;
started = false;
}
//------------------------------------------------------------------------------------------------
void checkDropOff() {
// IR threshold indicating surface drop-off (table edge, hole, etc.)
int dropOff = 950; // adjust value if necessary
// get IR sensor readings
int leftSensor = leftLine.read();
int centerSensor = centerLine.read();
int rightSensor = rightLine.read();
// see if any IR sensors detect drop-off
if (leftSensor > dropOff || centerSensor > dropOff || rightSensor > dropOff) {
// add code to perform (brake, reverse, change direction, etc.)
motors.brake();
tone(speaker, 1000, 200);
delay(1000);
motors.drive(-150); // back up
delay(1000);
motors.brake();
started = false; // allow test to be repeated
}
}
void checkButton() {
if (button.read() == true) {
// reverse value of "started"
started = !started;
// single-blink and single-beep as feedback
digitalWrite(led, HIGH);
tone(buzzer, 3000);
delay(200);
digitalWrite(led, LOW);
noTone(buzzer);
}
}
void alertSound() {
// modify code to play whatever sound pattern you want
for (int i = 0; i < 3; i++) {
tone(buzzer, 4000);
delay(100);
noTone(buzzer);
delay(100); // pause before next beep
}
}
void driveDistance(float distance, int power) {
// use wheel encoders to drive straight for specified distance at specified power
// set initial power for left and right motors
int leftPower = power;
int rightPower = power;
// amount to offset motor powers to drive straight
int offset = 5;
// if negative distance, make powers & offset also negative
if (distance < 0) {
leftPower *= -1;
rightPower *= -1;
offset *= -1;
}
// adjust distance to improve accuracy
float correction = -1.0; // change value based on test results
if (distance > 0) distance += correction;
else if (distance < 0) distance -= correction;
// variables for tracking wheel encoder counts
long leftCount = 0;
long rightCount = 0;
long prevLeftCount = 0;
long prevRightCount = 0;
long leftDiff, rightDiff;
// RedBot values based on encoders, motors & wheels
float countsPerRev = 192.0; // 192 encoder ticks per wheel revolution
float wheelDiam = 2.56; // wheel diameter = 65 mm = 2.56 in
float wheelCirc = PI * wheelDiam; // wheel circumference = 3.14 x 2.56 in = 8.04 in
// based on distance, calculate number of wheel revolutions
float numRev = distance / wheelCirc;
// calculate target encoder count
float targetCount = numRev * countsPerRev;
// reset encoder counters and start driving
encoder.clearEnc(BOTH);
delay(100);
motors.leftDrive(leftPower);
motors.rightDrive(rightPower);
// keeps looping while right encoder count less than target count
while (abs(rightCount) < abs(targetCount)) {
// get current wheel encoder counts
leftCount = encoder.getTicks(LEFT);
rightCount = encoder.getTicks(RIGHT);
// calculate increase in count from previous reading
leftDiff = abs(leftCount - prevLeftCount);
rightDiff = abs(rightCount - prevRightCount);
// store current counts as "previous" counts for next reading
prevLeftCount = leftCount;
prevRightCount = rightCount;
// adjust left & right motor powers to keep counts similar (drive straight)
// if left rotated more than right, slow down left & speed up right
if (leftDiff > rightDiff) {
leftPower = leftPower - offset;
rightPower = rightPower + offset;
}
// else if right rotated more than left, speed up left & slow down right
else if (leftDiff < rightDiff) {
leftPower = leftPower + offset;
rightPower = rightPower - offset;
}
// apply adjusted motor powers
motors.leftDrive(leftPower);
motors.rightDrive(rightPower);
delay(10); // short delay before next reading
}
// target count reached
motors.brake(); // or use: motors.stop()
delay(500); // brief delay to wait for complete stop
}
void pivotAngle(float angle) {
// use wheel encoders to pivot (turn) by specified angle
// set motor power for pivoting
int power = 100; // turn CW
if (angle < 0) power *= -1; // use negative power to turn CCW
// adjust angle to improve accuracy
float correction = -5.0; // change value based on test results
if (angle > 0) angle += correction;
else if (angle < 0) angle -= correction;
// variable for tracking wheel encoder counts
long rightCount = 0;
// RedBot values based on encoders, motors & wheels
float countsPerRev = 192.0; // 192 encoder ticks per wheel revolution
float wheelDiam = 2.56; // wheel diameter = 65 mm = 2.56 in
float wheelCirc = PI * wheelDiam; // wheel circumference = 3.14 x 2.56 in = 8.04 in
float pivotDiam = 6.125; // pivot diameter = distance between centers of wheel treads = 6.125 in
float pivotCirc = PI * pivotDiam; // pivot circumference = 3.14 x 6.125 in = 19.23 in
// based on angle, calculate distance (arc length)
float distance = abs(angle) / 360 * pivotCirc;
// based on distance, calculate number of wheel revolutions
float numRev = distance / wheelCirc;
// calculate target encoder count
float targetCount = numRev * countsPerRev;
// reset encoder counters and start pivoting
encoder.clearEnc(BOTH);
delay(100);
motors.pivot(power);
// keeps looping while right encoder count less than target count
while (abs(rightCount) < abs(targetCount)) {
// get current wheel encoder count
rightCount = encoder.getTicks(RIGHT);
delay(10); // short delay before next reading
}
// target count reached
motors.brake();
delay(250);
// clearEncoders(); // only needed if using driveStraight() or countLine()
}
void avoidLine() {
/* AVOID LINE
To avoid dark line on light surface:
Use high threshold & see if sensors greater than threshold
To avoid light line on dark surface:
Use low threshold & see if sensors less than threshold
Use test readings from line to determine best value for threshold
*/
// adjust value if necessary
int lineThreshold = 800;
// get IR sensor readings
int leftSensor = leftLine.read();
int rightSensor = rightLine.read();
// when either sensor on line, first brake motors
if (leftSensor > lineThreshold || rightSensor > lineThreshold) {
motors.brake();
delay(250);
}
// when both sensors on line, turn around
if (leftSensor > lineThreshold && rightSensor > lineThreshold) {
long rnd = random(750, 1250);
motors.pivot(100);
delay(rnd);
motors.stop();
}
// when line under left sensor only, pivot right
else if (leftSensor > lineThreshold) {
long rnd = random(500, 750);
motors.pivot(100);
delay(rnd);
motors.stop();
}
// when line under right sensor only, pivot left
else if (rightSensor > lineThreshold) {
long rnd = random(500, 750);
motors.pivot(-100);
delay(rnd);
motors.stop();
}
delay(25); // change delay to adjust line detection sensitivity
}
void avoidCollision() {
// set minimum distance between RedBot and obstacle
float minDist = 8.0; // change value as necessary (need decimal)
// measure distance to nearest obstacle
float sensorDist = measureDistance();
// if obstacle is too close, avoid collision
if (sensorDist <= minDist) {
// add code to perform (brake, change direction, etc.)
motors.brake();
pivotAngle(30);
}
else{
motors.drive(200);
}
delay(60); // need min 60ms between ultrasonic sensor readings
}
float measureDistance() {
// declare local variables for function
unsigned long time1;
unsigned long time2;
unsigned long pulse_time;
float dist_cm;
float dist_in;
// trigger ultrasonic signal for minimum 10 microseconds
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
// wait for echo to be received
while (digitalRead(ECHO_PIN) == 0);
// now measure how long echo lasts (pulse time)
time1 = micros(); // get start time in microseconds
while (digitalRead(ECHO_PIN) == 1); // wait until echo pulse ends
time2 = micros(); // get end time
pulse_time = time2 - time1; // subtract to get amount of time
// pulse time of 23200 represents maximum range for sensor
if (pulse_time > 23200) pulse_time = 23200;
/*
CALCULATE DISTANCE TO OBJECT USING PULSE TIME
distance = pulse time * speed of sound / 2
Divide by 2 because pulse time includes signal traveling to object and then back
Speed of sound in air at sea level is approximately 340 m/s
Numbers below are calculated constants for cm or inches
Decimal point in numbers necessary for accurate float calculation
*/
dist_cm = pulse_time / 58.0;
dist_in = pulse_time / 148.0;
// return measurement (dist_cm or dist_in)
return dist_in;
}