Zoidbot Mk2 – Följare med ultraljudssensorer

Med denna kod ska roboten alltid köra mot det närmaste hindret och sedan försöka hålla sig på 20 cm avstånd. Om du går framför roboten på en öppen yta kommer roboten försöka följa efter dig.

#include <ContinuousStepper.h>

#define LEFT_MOTOR_STEP_PIN 4
#define LEFT_MOTOR_DIR_PIN 3
#define RIGHT_MOTOR_STEP_PIN 7
#define RIGHT_MOTOR_DIR_PIN 6

#define US_SENSOR_TRIG_PIN A0
#define LEFT_US_SENSOR_ECHO_PIN A2
#define RIGHT_US_SENSOR_ECHO_PIN A1

#define ACCELERATION 1500
#define FORWARD_SPEED 600
#define BACK_SPEED 400
#define TURN_SPEED 400

#define FOLLOW_DISTANCE 200
#define FOLLOW_SPEED_FACTOR 2

ContinuousStepper<StepperDriver> left_motor;
ContinuousStepper<StepperDriver> right_motor;


enum us_states {
  start_measure,
  wait_trigger_sensor,
  wait_sensor_ready,
  read_echo,
  calculate_distances,
  timeout
};


unsigned long int us_sensor_timer = 0;
enum us_states us_sensor_state = start_measure;
int left_distance = 0;
int right_distance = 0;
unsigned long int left_echo_time;
unsigned long int right_echo_time;

int left_motor_speed = 0;
int right_motor_speed = 0;


void run_us_sensor() {
  unsigned long now = micros();
  switch (us_sensor_state) {
    case start_measure:
      digitalWrite(US_SENSOR_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_SENSOR_TRIG_PIN, LOW);
        us_sensor_timer = now;
        us_sensor_state = wait_sensor_ready;
      }
      break;

    case wait_sensor_ready:
      if (digitalRead(LEFT_US_SENSOR_ECHO_PIN) == HIGH &&
          digitalRead(RIGHT_US_SENSOR_ECHO_PIN) == HIGH) {
        us_sensor_timer = now;
        left_echo_time = 0;
        right_echo_time = 0;
        us_sensor_state = read_echo;
      }
      if (now - us_sensor_timer > 30000) {
        // Timeout
        Serial.println("ERROR: Ultrasonic sensor not ready");
        us_sensor_state = timeout;
      }
      break;

    case read_echo:
      if (digitalRead(LEFT_US_SENSOR_ECHO_PIN) == LOW && left_echo_time == 0) {
        left_echo_time = now - us_sensor_timer;
      }
      if (digitalRead(RIGHT_US_SENSOR_ECHO_PIN) == LOW && right_echo_time == 0) {
        right_echo_time = now - us_sensor_timer;
      }
      if (now - us_sensor_timer > 30000) {
        us_sensor_state = calculate_distances;
      }
      break;

    case calculate_distances:
      if (left_echo_time == 0) {
        left_distance = -1;
      }
      else {
        left_distance = left_echo_time / (2 * 3.4);
      }
      if (right_echo_time == 0) {
        right_distance = -1;
      }
      else {
        right_distance = right_echo_time / (2 * 3.4);
      }
      us_sensor_state = start_measure;
      break;

    case timeout:
      left_distance = -1;
      right_distance = -1;
      us_sensor_state = start_measure;
      break;

    default:
      us_sensor_state = start_measure;
      break;
  }
}


void drive_robot_follower() {
  // Left motor
  if (left_distance < 0) {
    left_motor_speed = FORWARD_SPEED;
  }
  left_motor_speed = (left_distance - 300) * FOLLOW_SPEED_FACTOR;
  left_motor_speed = constrain(left_motor_speed, -FORWARD_SPEED, FORWARD_SPEED);
  left_motor.spin(left_motor_speed);
  // Right motor
  if (right_distance < 0) {
    right_motor_speed = -FORWARD_SPEED;
  }
  right_motor_speed = (right_distance - 300) * FOLLOW_SPEED_FACTOR;
  right_motor_speed = constrain(right_motor_speed, -FORWARD_SPEED, FORWARD_SPEED);
  right_motor.spin(right_motor_speed);
}



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_SENSOR_TRIG_PIN, OUTPUT);
  pinMode(LEFT_US_SENSOR_ECHO_PIN, INPUT_PULLUP);
  pinMode(RIGHT_US_SENSOR_ECHO_PIN, INPUT_PULLUP);
  Serial.println("Starting");
}


unsigned long status_timer = 0;
void loop() {
  run_us_sensor();
  drive_robot_follower();
  left_motor.loop();
  right_motor.loop();

  if (millis() - status_timer > 500) {
    status_timer = millis();
    Serial.print(left_distance);
    Serial.print(",");
    Serial.print(right_distance);
    Serial.print(",");
    Serial.print(left_motor_speed);
    Serial.print(",");
    Serial.println(-right_motor_speed);
  }
}

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.