Zoidbot Mk2 – Undvika hinder med ultraljudssensorer

#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();
}

Lämna ett svar

Din e-postadress kommer inte publiceras. Obligatoriska fält är märkta *


The reCAPTCHA verification period has expired. Please reload the page.