Help adding in a mission abort system

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

You’re setting previousMillis in the wrong place.

Typically, what you want is:

unsigned long lastGoodTime;

void setup() {
  lastGoodTime = millis();
}

void loop() {
  if (not aborted) {
    unsigned long now = millis();
    if (good) {
      lastGoodTime = now;
    } else if (now - lastGoodTime > ABORT_THRESHOLD) {
      abort here
    } else {
      waiting for good or abort
    }
  }
}

So, each time you know you have some good sensor, you update a record of when that was. Then, if you find that it’s been more than 10 seconds since you had a good reading, abort. There will be an interval between it being good, and abort, where you’ll do nothing yet, because 10 seconds hasn’t expired yet.

Thanks so much for the help! It really helped how you explained it and now it works perfectly with a minor addition. I wrote a simple program to essentially do what the line follower will do, but with a single phototransistor. Since it was giving me readings of 0-5, it set a threshold where if it was below 1 (basically equal to 0), then it would start the counter. What I needed to add is shown below for the case where if it was a good reading, then it would essentially reset. That way, if the sensor fell below the threshold and then recovered, it would be reset and now remember the timings. Now everything works perfectly with the minor addition. I’ve included the small test platform code below if you’re interested. Thanks again for the much needed help and sorting out all the ifs!

// Test platform for setting timer that resets when back in a good reading

int photo = A5;
int sensorValue = 0;

unsigned long lastGoodTime;

void setup() {                
  pinMode(photo, OUTPUT); 
  Serial.begin(9600);
  lastGoodTime = millis();
}

void loop() 
{
  sensorValue = analogRead(A5);
  Serial.println(sensorValue);
    delay(500);
   
if (sensorValue<1) 
 {
    unsigned long now = millis();
    if (sensorValue >=1) 
     {
      lastGoodTime = now;
     } 
    else if (now - lastGoodTime > 10000) 
     {
      Serial.println("Abort!!");
     } 
    else {}
 }

if (sensorValue>=1) 
 {
  lastGoodTime = millis();
 }
 
}

Thanks!

1 Like