Zoidbot Mk2 – Följare med ultraljudssensorer och LED-matrisdisplayer

#include <ContinuousStepper.h>
#include <LEDMatrixDriver.hpp>

#define LEFT_MOTOR_EN_PIN 2
#define LEFT_MOTOR_STEP_PIN 3
#define LEFT_MOTOR_DIR_PIN 4
#define RIGHT_MOTOR_EN_PIN 5
#define RIGHT_MOTOR_STEP_PIN 6
#define RIGHT_MOTOR_DIR_PIN 7

#define LEFT_US_SENSOR_TRIG_PIN A0
#define LEFT_US_SENSOR_ECHO_PIN A2
#define RIGHT_US_SENSOR_TRIG_PIN A1
#define RIGHT_US_SENSOR_ECHO_PIN A3 
#define LEFT_SENSOR 0
#define RIGHT_SENSOR 1

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

#define FOLLOW_DISTANCE 200
#define FOLLOW_SPEED_FACTOR 2.5

#define DISPLAY_CS_PIN 9
#define DISPLAY_BRIGHTNESS 5

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

LEDMatrixDriver display(2, DISPLAY_CS_PIN);


byte eyes_forward[] = {
  B00111100,
  B01111110,
  B11111111,
  B11100011,
  B11100011,
  B11111111,
  B01111110,
  B00111100
};

byte eyes_left[] = {
  B00111100,
  B01111110,
  B11111111,
  B11111111,
  B11111111,
  B11100011,
  B01100010,
  B00111100
};

byte eyes_right[] = {
  B00111100,
  B01100010,
  B11100011,
  B11111111,
  B11111111,
  B11111111,
  B01111110,
  B00111100
};

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


struct us_sensor {
  uint8_t trig_pin;
  uint8_t echo_pin;
  int distance;
};

unsigned long int us_sensor_timer = 0;
unsigned long int display_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;

int left_motor_speed = 0;
int right_motor_speed = 0;


struct us_sensor us_sensors[] = {
  {
    .trig_pin = LEFT_US_SENSOR_TRIG_PIN,
    .echo_pin = LEFT_US_SENSOR_ECHO_PIN,
    .distance = -1
  },
  {
    .trig_pin = RIGHT_US_SENSOR_TRIG_PIN,
    .echo_pin = RIGHT_US_SENSOR_ECHO_PIN,
    .distance = -1
  }
};

unsigned int us_sensor_index = LEFT_SENSOR;

void forward() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(FORWARD_SPEED);
  right_motor.spin(FORWARD_SPEED);
}

void backward() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(-BACK_SPEED);
  right_motor.spin(-BACK_SPEED);
}

void turn_left() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(0);
  right_motor.spin(FORWARD_SPEED);
}

void turn_right() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(FORWARD_SPEED);
  right_motor.spin(0);
}

void rotate_left() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(-TURN_SPEED);
  right_motor.spin(TURN_SPEED);
}

void rotate_right() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(TURN_SPEED);
  right_motor.spin(-TURN_SPEED);
}

void back_left() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(-BACK_SPEED);
  right_motor.spin(0);
}

void back_right() {
  left_motor.powerOn();
  right_motor.powerOn();
  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:
      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_follower() {
  // Left motor
  if (us_sensors[LEFT_SENSOR].distance < 0) {
    left_motor_speed = FORWARD_SPEED;
  }
  left_motor_speed = (us_sensors[LEFT_SENSOR].distance - 300) * FOLLOW_SPEED_FACTOR;
  left_motor_speed = constrain(left_motor_speed, -FORWARD_SPEED, FORWARD_SPEED);
  left_motor.powerOn();
  left_motor.spin(left_motor_speed);
  // Right motor
  if (us_sensors[RIGHT_SENSOR].distance < 0) {
    right_motor_speed = FORWARD_SPEED;
  }
  right_motor_speed = (us_sensors[RIGHT_SENSOR].distance - 300) * FOLLOW_SPEED_FACTOR;
  right_motor_speed = constrain(right_motor_speed, -FORWARD_SPEED, FORWARD_SPEED);
  right_motor.powerOn();
  right_motor.spin(right_motor_speed);
}



void update_display() {
  if (millis() - display_timer < 500) {
    return;
  }
  display_timer = millis();
  int direction = left_motor_speed - right_motor_speed;
  if (direction < -100) {
    drawEyes(eyes_left);
  }
  else if (direction > 100) {
    drawEyes(eyes_right);
  }
  else {
    drawEyes(eyes_forward);
  }
}

void drawEyes(byte *eyes) {
  for (int col = 0; col < 8; col++) {
    display.setColumn(col, eyes[col]);
    display.setColumn(col + 8, eyes[col]);
  }
  display.display();
}



void setup() {
  Serial.begin(115200);
  Serial.println("Initializing");
  // Setup left motor
  left_motor.begin(LEFT_MOTOR_STEP_PIN, LEFT_MOTOR_DIR_PIN);
  left_motor.setAcceleration(ACCELERATION);
  left_motor.powerOff();
  // Setup right motor
  right_motor.begin(RIGHT_MOTOR_STEP_PIN, RIGHT_MOTOR_DIR_PIN);
  right_motor.setAcceleration(ACCELERATION);
  right_motor.powerOff();
  // 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);
  // Init LED display
  display.setEnabled(true);
  display.setIntensity(DISPLAY_BRIGHTNESS);
  Serial.println("Starting");
}


unsigned long status_timer = 0;
void loop() {
  run_us_sensor();
  drive_robot_follower();
  update_display();

  if (left_motor.speed() == 0) {
    left_motor.powerOff();
  }
  if (right_motor.speed() == 0) {
    right_motor.powerOff();
  }
  left_motor.loop();
  right_motor.loop();

  if (millis() - status_timer > 500) {
    status_timer = millis();
    Serial.print(us_sensors[LEFT_SENSOR].distance);
    Serial.print(",");
    Serial.print(us_sensors[RIGHT_SENSOR].distance);
    Serial.print(",");
    Serial.print(left_motor_speed);
    Serial.print(",");
    Serial.println(-right_motor_speed);
  }
}