//======================== COMMENTS
=============================================================
// This example is designed for use with the QTR-6RC ReflectanceSensorArray.
// These reflectance sensors arrays are connected as follows:
// === Pinout ===
// Pins: 4 A3 11 A0 A2 5 2 13
// Sensor #: 0 1 2 3 4 5 IR LEDs LED13
// Line Location: x
// Line Location: x
// Line Location: x
// Line Location: x
// Line Location: x
// Line Location: x
// Position Value: 0 1000 2000 3000 4000 5000
// The void setup() phase of this code calibrates the sensor for ten seconds and turns on
// the LED built in to the Arduino on pin 13 while calibration is going on.
// During this phase, you should expose each reflectance sensor to the lightest and
// darkest readings they will encounter. These values are then printed to the Serial Monitor.
// For example, if you are making a line follower, you should slide the sensors across the
// line during the calibration phase so that each sensor can get a reading of how dark the
// line is and how light the ground is. IMPROPER CALIBRATION will result in poor readings.
// Program created by: Dan Karran | 4.1.2022
//=====================================================================================================
#include <QTRSensors.h>
#include <ZumoMotors.h>
#include <Pushbutton.h>
#include <Wire.h>
#include <ZumoShield.h>
#define NUM_SENSORS 6 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
ZumoBuzzer buzzer;
ZumoMotors motors;
Pushbutton button(ZUMO_BUTTON);
QTRSensorsRC qtrrc((unsigned char[]) {4, A3, 11, A0, A2, 5}, NUM_SENSORS, TIMEOUT, EMITTER_PIN); // setup sensor array
unsigned int sensorValues[NUM_SENSORS];
int location;
float error;
int sub_routine;
int i;
unsigned long startTime;
unsigned long timer;
int stp = 1;
void setup()
{
Serial.begin(115200);
//=== Calibration section (BEGIN) ===
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
delay(500);
button.waitForButton();
// extra compass calibratin stuff can go here
for (i = 0; i < 5; i++)
{
//-------------------------------------------
if (i == 0 || i == 4)
{
motors.setSpeeds(-175, 175); // spin left
timer = 700;
}
else if (i == 2)
{
motors.setSpeeds(175, -175); // spin right
timer = 1400;
}
else
{
motors.setSpeeds(0, 0); // stop motors
timer = 10;
}
//-------------------------------------------
startTime = millis();
while (millis() - startTime < timer)
{
qtrrc.calibrate(); //unique to sensor array calibr
// extra compass calibratin stuff can go here
}
motors.setSpeeds(0, 0);
delay(10);
//-------------------------------------------
}
digitalWrite(13, LOW);
//___ print the calibration Min/Max values measured ___________
Serial.print("\n---------------------------------------------------");
Serial.print("\n------------- Min Sensor Values -------------------");
Serial.print("\n");
// Serial.print("\t");
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print("\t");
}
Serial.print("\n------------- Max Sensor Values -------------------");
Serial.print("\n");
// Serial.print("\t");
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print("\t");
}
Serial.print("\n---------------------------------------------------");
Serial.print("\n");
//_____________________________________________________________
// extra compass calibratin printing stuff can go here
//=== Calibration section (END) ===
} //end of void setup
void loop()
{
location = qtrrc.readLine(sensorValues);//reads the position of the line and stores it as "location"
error = location - 2500; //This calculates the "error" of the straightness of the zumo on the line. If the zumo is perfectly center, the location will be 2500 and the error will be zero
switch (sub_routine)//cases will switch based on the value of sub_routine
{
case 1:{
if (stp == 1){
function_1(error);
function_2(true);
}
if (stp == 2){
function_1(error);
function_2(false);
}
if (stp == 3){
function_1(error);
arrive();
}
break;
}
case 2:{
function_2(true);
break;
}
default: //default is the starting point of the switch cases. Switch cases won't begin until button is pushed
button.waitForButton();
sub_routine = 1;
}
}
//below is the code for function_1. It is a "void" function because it doesn't return anything; rather, it just executes the commands written in the function
void function_1(float err) // whatever input is entered into this function will be a float called "err" for executing the code inside the function
{
float adj = 1 - (abs(err)/2500*2);
if (err>0){
motors.setSpeeds (150,150* adj);
}
else if (err<0){
motors.setSpeeds (150* adj ,150);
}
else
{
motors.setSpeeds (150,150);
}
}
void function_2(bool decision) //bool means that function_2 will expect an input of either true or false, which it will then assign to "decision" for executing code inside the function
{
if (decision == true){
if((sensorValues[0]+sensorValues[1]/2)>1000){//here we add the average the output of sensors 4 and 5 and test whether they are greater than a certain value.
motors.setSpeeds (-50,150);
delay(1000);
//While (1)
stp ++;
}
}
else{
if((sensorValues[4]+sensorValues[5]/2)>1000){
buzzer.playFrequency(440, 200, 15);
motors.setSpeeds (150,0);
delay(1000);
//while (1)
stp ++;
}
}
}
void arrive()
{
if((sensorValues[1]+sensorValues[4]/2)>1000)
{
buzzer.playFrequency(440, 200, 15);
motors.setSpeeds (0,0);
delay(1000);
while(1)
motors.setSpeeds (100,100);
delay(500);
}
else
{
if((sensorValues[1]+sensorValues[4]/2)>2500)
{motors.setSpeeds (-150,150);
delay(500);
while(1)
}
}
}
Your code is missing some formatting elements that are required. I would suggest you review how to properly format switch cases as well as "while" loops. I think both of the are incorrect in your sketch. For example, you have a end curly bracket after your break in the switch. That is incorrect.
This while statement has no associated code.
if ((sensorValues[1] + sensorValues[4] / 2) > 2500)
{ motors.setSpeeds (-150, 150);
delay(500);
while (1)
}
Needs a ; after it.
Your topic has been moved to a more suitable location on the forum as it has nothing to do with Avrdude, stk500, Bootloader issues.
This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.