#include <ContinuousStepper.h>
#define LEFT_MOTOR_STEP_PIN 10
#define LEFT_MOTOR_DIR_PIN 9
#define LEFT_MOTOR_ENABLE_PIN 8
#define RIGHT_MOTOR_STEP_PIN 13
#define RIGHT_MOTOR_DIR_PIN 12
#define RIGHT_MOTOR_ENABLE_PIN 11
#define LEFT_US_SENSOR_TRIG_PIN 46
#define LEFT_US_SENSOR_ECHO_PIN 47
#define RIGHT_US_SENSOR_TRIG_PIN 48
#define RIGHT_US_SENSOR_ECHO_PIN 49
#define LEFT_SENSOR 0
#define RIGHT_SENSOR 1
#define ACCELERATION 800
#define FORWARD_SPEED 400
#define BACK_SPEED 400
#define TURN_SPEED 400
#define BACK_TIME 800
#define TURN_TIME 600
ContinuousStepper<StepperDriver> left_motor;
ContinuousStepper<StepperDriver> right_motor;
enum us_states {
start_measure,
wait_sensor_idle,
trigger_sensor,
wait_trigger_sensor,
wait_sensor_ready,
wait_echo,
echo_received,
timeout,
wait_echo_die_out,
switch_sensor
};
enum drive_states {
drive_forward,
backup_left,
backup_right,
drive_left,
drive_right
};
struct us_sensor {
uint8_t trig_pin;
uint8_t echo_pin;
int distance;
};
unsigned long int us_sensor_timer = 0;
unsigned long int drive_timer = 0;
enum us_states us_sensor_state = wait_sensor_idle;
int left_us_sensor_distance = 0;
int right_us_sensor_distance = 0;
int turn_time;
struct us_sensor us_sensors[] = {
{
.trig_pin = 46,
.echo_pin = 47,
.distance = -1
},
{
.trig_pin = 48,
.echo_pin = 49,
.distance = -1
}
};
unsigned int us_sensor_index = LEFT_SENSOR;
enum drive_states drive_state;
void forward() {
left_motor.spin(FORWARD_SPEED);
right_motor.spin(-FORWARD_SPEED);
}
void backward() {
left_motor.spin(-BACK_SPEED);
right_motor.spin(BACK_SPEED);
}
void turn_left() {
left_motor.spin(0);
right_motor.spin(-FORWARD_SPEED);
}
void turn_right() {
left_motor.spin(FORWARD_SPEED);
right_motor.spin(0);
}
void rotate_left() {
left_motor.spin(-TURN_SPEED);
right_motor.spin(-TURN_SPEED);
}
void rotate_right() {
left_motor.spin(TURN_SPEED);
right_motor.spin(TURN_SPEED);
}
void back_left() {
left_motor.spin(-BACK_SPEED);
right_motor.spin(0);
}
void back_right() {
left_motor.spin(0);
right_motor.spin(BACK_SPEED);
}
void stop() {
left_motor.stop();
right_motor.stop();
}
void run_us_sensor() {
unsigned long now = micros();
switch (us_sensor_state) {
case start_measure:
us_sensor_timer = now;
us_sensor_state = wait_sensor_idle;
break;
case wait_sensor_idle:
if (digitalRead(us_sensors[us_sensor_index].echo_pin) == LOW) {
us_sensor_state = trigger_sensor;
}
if (now - us_sensor_timer > 50000) {
// Timeout
Serial.println("ERROR: Ultrasonic sensor not ready");
us_sensor_state = timeout;
}
break;
case trigger_sensor:
digitalWrite(us_sensors[us_sensor_index].trig_pin, HIGH);
us_sensor_timer = now;
us_sensor_state = wait_trigger_sensor;
break;
case wait_trigger_sensor:
if (now - us_sensor_timer > 10) {
digitalWrite(us_sensors[us_sensor_index].trig_pin, LOW);
us_sensor_timer = now;
us_sensor_state = wait_sensor_ready;
}
break;
case wait_sensor_ready:
if (digitalRead(us_sensors[us_sensor_index].echo_pin) == HIGH) {
us_sensor_timer = now;
us_sensor_state = wait_echo;
}
if (now - us_sensor_timer > 30000) {
// Timeout
Serial.println("ERROR: Ultrasonic sensor not ready");
us_sensor_state = timeout;
}
break;
case wait_echo:
if (digitalRead(us_sensors[us_sensor_index].echo_pin) == LOW) {
us_sensor_state = echo_received;
}
if (now - us_sensor_timer > 30000) {
// Timeout
us_sensor_state = timeout;
}
break;
case echo_received:
us_sensors[us_sensor_index].distance = (now - us_sensor_timer) / (2 * 3.4);
us_sensor_state = wait_echo_die_out;
break;
case wait_echo_die_out:
if (now - us_sensor_timer > 30000) {
us_sensor_state = switch_sensor;
}
break;
case timeout:
us_sensors[us_sensor_index].distance = -1;
us_sensor_state = switch_sensor;
break;
case switch_sensor:
/*
Serial.print("Sensor ");
Serial.print(us_sensor_index);
Serial.print(": ");
Serial.print(us_sensors[us_sensor_index].distance);
Serial.println(" mm");
*/
if (us_sensor_index == LEFT_SENSOR) {
us_sensor_index = RIGHT_SENSOR;
}
else {
us_sensor_index = LEFT_SENSOR;
}
us_sensor_state = start_measure;
break;
default:
us_sensor_state = start_measure;
break;
}
}
void drive_robot() {
switch(drive_state) {
case drive_forward:
forward();
if (us_sensors[LEFT_SENSOR].distance < 200 && us_sensors[LEFT_SENSOR].distance > 0) {
drive_timer = millis();
drive_state = backup_right;
Serial.println("Obsacle to the left");
}
else if (us_sensors[RIGHT_SENSOR].distance < 200 && us_sensors[RIGHT_SENSOR].distance > 0) {
drive_timer = millis();
drive_state = backup_left;
Serial.println("Obstacle to the right");
}
break;
case backup_left:
backward();
if (millis() - drive_timer > BACK_TIME) {
drive_timer = millis();
drive_state = drive_left;
turn_time = TURN_TIME - 300 + random(600);
Serial.println("Turning left");
}
break;
case backup_right:
backward();
if (millis() - drive_timer > BACK_TIME) {
drive_timer = millis();
drive_state = drive_right;
turn_time = TURN_TIME - 300 + random(600);
Serial.println("Turning right");
}
break;
case drive_left:
back_left();
if (millis() - drive_timer > turn_time) {
drive_state = drive_forward;
Serial.println("Driving forward");
}
break;
case drive_right:
back_right();
if (millis() - drive_timer > turn_time) {
drive_state = drive_forward;
Serial.println("Driving forward");
}
break;
default:
drive_state = drive_forward;
break;
}
}
void setup() {
srand(A0);
Serial.begin(115200);
Serial.println("Initializing");
// Setup left motor
left_motor.begin(LEFT_MOTOR_STEP_PIN, LEFT_MOTOR_DIR_PIN);
left_motor.setAcceleration(ACCELERATION);
// Setup right motor
right_motor.begin(RIGHT_MOTOR_STEP_PIN, RIGHT_MOTOR_DIR_PIN);
right_motor.setAcceleration(ACCELERATION);
// Setup ultra-sonic sensors
pinMode(us_sensors[LEFT_SENSOR].trig_pin, OUTPUT);
pinMode(us_sensors[RIGHT_SENSOR].trig_pin, OUTPUT);
pinMode(us_sensors[LEFT_SENSOR].echo_pin, INPUT_PULLUP);
pinMode(us_sensors[RIGHT_SENSOR].echo_pin, INPUT_PULLUP);
Serial.println("Starting");
}
unsigned long test_timer = 0;
void loop() {
run_us_sensor();
drive_robot();
left_motor.loop();
right_motor.loop();
}