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

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. Denna kod visar dessutom ett par animerade ögon på två LED-matrisdisplayer. Ögonen rör sig åt det håll om roboten kör åt.
#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);
  }
}
Lämna ett svar Avbryt svar
