Zoidbot Mk2 – Följare med ultraljudssensorer

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