Zoidbot Mk2 – Undvika hinder med stötsensor

Med denna kod kommer roboten att köra framåt tills den stöter emot ett hinder med sin stötsensor. Den kommer då att backa lite och sedan vända sig bort från hindret och sedan fortsätta köra framåt.
#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_BUMPER_PIN 4
#define RIGHT_BUMPER_PIN 5

#define ACCELERATION 5000
#define FORWARD_SPEED 2000
#define BACK_SPEED 2000
#define TURN_SPEED 2000

#define BACK_TIME 800
#define TURN_TIME 750


enum drive_states {
  drive_forward,
  backup_left,
  backup_right,
  drive_left,
  drive_right
};

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

enum drive_states drive_state;
unsigned long int drive_timer;


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 drive_robot() {
  switch(drive_state) {
    case drive_forward:
      forward();
      if (digitalRead(LEFT_BUMPER_PIN) == LOW) {
        drive_timer = millis();
        drive_state = drive_right;
        Serial.println("Left bumper hit");
      }
      else if (digitalRead(RIGHT_BUMPER_PIN) == LOW) {
        drive_timer = millis();
        drive_state = drive_left;
        Serial.println("Right bumper hit");
      }
      break;

    case backup_left:
      backward();
      if (millis() - drive_timer > BACK_TIME) {
        drive_timer = millis();
        drive_state = drive_left;
        Serial.println("Turning left");
      }
      break;

    case backup_right:
      backward();
      if (millis() - drive_timer > BACK_TIME) {
        drive_timer = millis();
        drive_state = drive_right;
        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() {
  // Setup motors
  left_motor.begin(LEFT_MOTOR_STEP_PIN, LEFT_MOTOR_DIR_PIN);
  left_motor.setEnablePin(LEFT_MOTOR_ENABLE_PIN, LOW);
  left_motor.setAcceleration(ACCELERATION);
  right_motor.begin(RIGHT_MOTOR_STEP_PIN, RIGHT_MOTOR_DIR_PIN);
  right_motor.setEnablePin(RIGHT_MOTOR_ENABLE_PIN, LOW);
  right_motor.setAcceleration(ACCELERATION);
  // Setup bumper
  pinMode(LEFT_BUMPER_PIN, INPUT_PULLUP);
  pinMode(RIGHT_BUMPER_PIN, INPUT_PULLUP);
  Serial.begin(115200);
}


void loop() {
  drive_robot();
  left_motor.loop();
  right_motor.loop();
}
Lämna ett svar Avbryt svar
