Hi All,
I am in need of help with a project I’m working on. Essentially, it is a line following robot that delivers a payload when reaching a target drop area. One of the criteria invlove adding a automatic mission abort system that triggers when the vehicle cannot see the line after 10 seconds. What I need help with is figuring out an effective timer that is able to start once all the sensors are below a certain threshold, and then reset once back on the line. I have been messing around with the BlinkWithoutDelay example, but cannot seem to reset the timer back to zero once it finds a line again. Note the section I am having problems with is listed under “Mission Abort Criteria”.
Here is the code I am working with right now. It is essentially a combination of a couple of examples for the hardware I am using. I use an Arduino Leonardo, Pololu QTR-8RC Sensor, and Pololu VNH5019 Dual Motor Driver Shield. Note that the motor shield is on Timer1 and the servos have been switched to Timer3.
#include <QTRSensors.h>
#include <Servo.h>
#include <DualVNH5019MotorShield.h>
DualVNH5019MotorShield md;
// Combination of QTRRC Example, Sweep Servo Example, and VNH5019 Example
// Note that Servo.h has been modified to be on Timer 3 (as per: https://www.pololu.com/docs/0J57/8.b)
#define Kp 0.1 // Experiment to determine this, start by something small that just makes your bot follow the line at a slow speed
#define Kd 3 // Experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd)
#define M2MaxSpeed 350 // Max speed of the right motor actually 400. change once tuned.
#define M1MaxSpeed 350 // Max speed of the left motor actually 400. change once tuned.
#define M2BaseSpeed 100 // This is the speed at which the motors should spin when the robot is perfectly on the line
#define M1BaseSpeed 100 // This is the speed at which the motors should spin when the robot is perfectly on the line
#define NUM_SENSORS 8 // Number of sensors used
#define TIMEOUT 2500 // Waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN 13 // Emitter is controlled by digital pin 13 (LEDON)
// Sensors 0 through 7 are connected to digital pins below, respectively
QTRSensorsRC qtrrc((unsigned char[]) {A2, A3, A4, A5, 0, 1, 3, 5}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
Servo myservo; // Create servo object to control a servo
int pos = 90; // Variable to store the servo position (MUST BE SET FOR FINAL DESIGN)
long previousMillis = 0; // last time update
long interval = 10000; // interval at which to do something (milliseconds)
// Motor testing (diagnostic)
void stopIfFault()
{
if (md.getM1Fault())
{
Serial.println("Left Motor (M1) fault");
while(1);
}
if (md.getM2Fault())
{
Serial.println("Right Motor (M2) fault");
while(1);
}
}
void setup()
{
myservo.attach(11); // Attaches the servo on pin 11 to the servo object
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Sensor Auto-Calibration Section
delay(2000);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH); // Turn on Arduino's LED to indicate we are in calibration mode
for (int i = 0; i < 400; i++) // Make the calibration take about 10 seconds
{
if (i < 100 || i >= 300) // Automatic Calibration - turn motor right and left during calibration to expose to max & min values
{
md.setM2Speed(100); // Right motor forward
md.setM1Speed(-100); // Left motor backward
}
else
{
md.setM2Speed(-100); // Right motor backward
md.setM1Speed(100); // Left motor forward
}
qtrrc.calibrate(QTR_EMITTERS_ON); // Reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
}
md.setBrakes(400,400); // Stop motors at end of calibration
digitalWrite(13, LOW); // Turn off Arduino's LED to indicate we are through with calibration
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Diagnostic Sensor Data Display. Comment out for final upload.
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
// Print the calibration maximum values measured when emitters were on - Comment out when for final upload
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
delay(1000);
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
md.init(); // Initiate pinModes and timer1 for Motors
md.setSpeeds(0,0); // Set initial speeds to zero
}
int lastError = 0; // Set intial error to zero
void loop()
{
// Read calibrated sensor values and obtain a measure of the line position from 0 to 7000
unsigned int position = qtrrc.readLine(sensorValues, QTR_EMITTERS_ON, 1);
Serial.println("Line Position: ");
Serial.println(position); // Comment this line out if you are using raw values
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Mission Abort Criteria - change to > 750 if white line switch isnt working
if (sensorValues[0] < 100 && sensorValues[1] < 100 && sensorValues[2] < 100 && sensorValues[3] < 100 && sensorValues[4] < 100 && sensorValues[5] < 100 && sensorValues[6] < 100 && sensorValues[7] < 100)
{
unsigned long currentMillis = millis();
if(currentMillis - previousMillis > interval)
{
previousMillis = currentMillis;
Serial.print('Abort!');
md.setBrakes(350,350);
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Stop and Servo Trigger to drop load - change to < 100 if white line switch isnt working
if (sensorValues[0] > 750 && sensorValues[1] > 750 && sensorValues[2] > 750 && sensorValues[3] > 750 && sensorValues[4] > 750 && sensorValues[5] > 750 && sensorValues[6] > 750 && sensorValues[7] > 750)
{
// All Sensors over line, trigger servo
Serial.println("Target Drop Zone Reached!"); // Diagnostic
Serial.print('\n');
md.setBrakes(400, 400); // Stop the UGV
delay(1000); // Allow time to brake
myservo.write(110); // Trigger servos
delay(5000); // Allow time to trigger servos and payload to clear
myservo.write(90); // Bring servo back to original position
delay(500); // Delay to allow time for servo movement
md.setSpeeds(100, 100); // Change speed after testing. Manual movement to get across target drop
delay(2000); // Run motors for 2 seconds to clear target drop. reconnect with return line
}
else
{
Serial.println("Following Line");
Serial.println('\n');
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Motor Control
{
unsigned int sensors[8];
int position = qtrrc.readLine(sensors,QTR_EMITTERS_ON, 1); // The secondary parameter, 1, indicates white line on black background
int error = position - 3500; // Change to 3500 - position for white line on black background?
int motorSpeed = Kp * error + Kd * (error - lastError);
lastError = error;
int M2MotorSpeed = M2BaseSpeed + motorSpeed; // Right motor
int M1MotorSpeed = M1BaseSpeed - motorSpeed; // Left motor
if (M2MotorSpeed > M2MaxSpeed ) M2MotorSpeed = M2MaxSpeed; // prevent the motor from going beyond max speed
if (M1MotorSpeed > M1MaxSpeed ) M1MotorSpeed = M1MaxSpeed; // prevent the motor from going beyond max speed
if (M2MotorSpeed < 0) M2MotorSpeed = 0; // keep the motor speed positive
if (M1MotorSpeed < 0) M1MotorSpeed = 0; // keep the motor speed positive
md.setSpeeds(M1MotorSpeed, M2MotorSpeed);
delay(250);
}
}
Thanks,
LH