Last active
November 22, 2017 18:51
-
-
Save kierdavis/afffdd924c23d7f65f2498a657c94772 to your computer and use it in GitHub Desktop.
Barebones code for line following on an Arduino-based robot (untested)
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <QTRSensors.h> | |
// Pins for controlling left motor | |
#define ENA 10 // needs to be a PWM pin | |
#define IN1 4 | |
#define IN2 5 | |
// Pins for controlling right motor | |
#define ENB 9 // needs to be a PWM pin | |
#define IN3 6 | |
#define IN4 7 | |
// Constants for passing to set_left_motor/set_right_motor. | |
#define OFF 0 | |
#define FORWARD 1 | |
#define REVERSE 2 | |
// The speed the robot normally drives forward at. | |
// The range is between 0 and 255, so this value is (roughly) in the middle. | |
#define DRIVE_SPEED 128 | |
// The number of sensors that are plugged into the arduino. | |
// The QTR-8RC module provides up to eight sensors. | |
#define NUM_SENSORS 4 | |
// The arduino pins that the sensors are connected to, from left to right. | |
static unsigned char sensor_pins[NUM_SENSORS] = {2, 3, 11, 12}; | |
QTRSensorsRC sensors(sensor_pins, NUM_SENSORS); | |
// direction is one of OFF, FORWARD or REVERSE. | |
// speed is an integer between 0 and 255. | |
void set_left_motor(int direction, int speed) { | |
if (direction == OFF) { | |
digitalWrite(IN1, LOW); | |
digitalWrite(IN2, LOW); | |
analogWrite(ENA, 0); | |
} | |
else if (direction == FORWARD) { | |
digitalWrite(IN1, HIGH); | |
digitalWrite(IN2, LOW); | |
analogWrite(ENA, speed); | |
} | |
else if (direction == REVERSE) { | |
digitalWrite(IN1, LOW); | |
digitalWrite(IN2, HIGH); | |
analogWrite(ENA, speed); | |
} | |
} | |
// Works similarly to set_left_motor. | |
void set_right_motor(int direction, int speed) { | |
if (direction == OFF) { | |
digitalWrite(IN3, LOW); | |
digitalWrite(IN4, LOW); | |
analogWrite(ENA, 0); | |
} | |
else if (direction == FORWARD) { | |
digitalWrite(IN3, HIGH); | |
digitalWrite(IN4, LOW); | |
analogWrite(ENA, speed); | |
} | |
else if (direction == REVERSE) { | |
digitalWrite(IN3, LOW); | |
digitalWrite(IN4, HIGH); | |
analogWrite(ENA, speed); | |
} | |
} | |
void setup() { | |
// Set pin modes. | |
pinMode(ENA, OUTPUT); | |
pinMode(IN1, OUTPUT); | |
pinMode(IN2, OUTPUT); | |
pinMode(ENB, OUTPUT); | |
pinMode(IN3, OUTPUT); | |
pinMode(IN4, OUTPUT); | |
// Start with the motors switched off. | |
set_left_motor(OFF, 0); | |
set_right_motor(OFF, 0); | |
// Calibrate the sensors. | |
// This is set to take about 5 seconds, during which you should move the robot | |
// around so that all sensors see both the white table and black line. | |
for (int i = 0; i < 250; i++) { | |
sensors.calibrate(); | |
delay(20); | |
} | |
} | |
void loop() { | |
// Read the light sensors. | |
// If the line is detected by at least one of the sensors, this returns a | |
// number between 0 and 3000, where 0 means the line is directly under the | |
// leftmost sensor and 3000 means it's directly under the rightmost one. | |
// Otherwise, this returns either 0 or 3000 depending on which side the line | |
// was last seen on. Consider that if we can't see the line at all, the best | |
// thing to do is to turn towards the side we last saw it on. | |
unsigned int sensor_values[NUM_SENSORS]; | |
int line_pos = sensors.readLine(sensor_values); | |
// Decide what to do based on where the line is relative to the robot. | |
if (line_pos >= 1500 && line_pos <= 2500) { | |
// The line is more or less in the middle, so we should drive straight. | |
set_left_motor(FORWARD, DRIVE_SPEED); | |
set_right_motor(FORWARD, DRIVE_SPEED); | |
} | |
else if (line_pos < 1500) { | |
// The line is too far to the robot's left, so we should turn left. We do | |
// this setting the right motor to spin faster than the left motor. | |
set_left_motor(FORWARD, DRIVE_SPEED - 30); | |
set_right_motor(FORWARD, DRIVE_SPEED + 30); | |
} | |
else if (line_pos > 2500) { | |
// The line is too far to the robot's right, so we should turn right. | |
set_left_motor(FORWARD, DRIVE_SPEED + 30); | |
set_right_motor(FORWARD, DRIVE_SPEED - 30); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment