Zoidbot Mk2 – Fjärrstyrning av robot med arm och LED-matrisdisplayer via Bluetooth

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

#define LEFT_MOTOR_STEP_PIN 3
#define LEFT_MOTOR_DIR_PIN 2
#define RIGHT_MOTOR_STEP_PIN A4
#define RIGHT_MOTOR_DIR_PIN A5
#define BLUETOOTH_RX_PIN 10
#define BLUETOOTH_TX_PIN 8
#define ARM_SERVO_PIN 5
#define GRIPPER_SERVO_PIN 6

#define ARM_DEFAULT_POSITION 100
#define GRIPPER_DEFAULT_POSITION 150

#define ACCELERATION 1500
#define FORWARD_SPEED 800
#define BACK_SPEED 800
#define TURN_SPEED 800

#define DISPLAY_CS_PIN 9
#define DISPLAY_BRIGHTNESS 5


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

LEDMatrixDriver display(2, DISPLAY_CS_PIN);

Servo arm;
Servo gripper;

unsigned long int arm_timer = 0;
unsigned long int display_timer = 0;
unsigned long int blink_timer = 0;

enum direction {
  dir_forward,
  dir_backward,
  dir_left,
  dir_right,
  dir_stop
};
enum direction current_direction = dir_stop;


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

byte eyes_blink[] = {
  B00000000,
  B00010000,
  B00010000,
  B00010000,
  B00010000,
  B00010000,
  B00010000,
  B00000000,
};


void forward() {
  left_motor.spin(FORWARD_SPEED);
  right_motor.spin(FORWARD_SPEED);
  current_direction = dir_forward;
}

void backward() {
  left_motor.spin(-BACK_SPEED);
  right_motor.spin(-BACK_SPEED);
  current_direction = dir_backward;
}

void turn_left() {
  left_motor.spin(0);
  right_motor.spin(FORWARD_SPEED);
  current_direction = dir_left;
}

void turn_right() {
  left_motor.spin(FORWARD_SPEED);
  right_motor.spin(0);
  current_direction = dir_right;
}

void rotate_left() {
  left_motor.spin(-TURN_SPEED);
  right_motor.spin(TURN_SPEED);
  current_direction = dir_left;
}

void rotate_right() {
  left_motor.spin(TURN_SPEED);
  right_motor.spin(-TURN_SPEED);
  current_direction = dir_right;
}

void stop() {
  left_motor.stop();
  right_motor.stop();
  current_direction = dir_stop;
}


void run_robot(String command) {
  int pos;
  switch(command[0]) {
    case 'F':
      forward();
      break;
    case 'G':
      backward();
      break;
    case 'L':
      turn_left();
      break;
    case 'R':
      turn_right();
      break;
    case 'S':
      stop();
      break;
    case 'J':
      pos = command.substring(1, command.length()).toInt();
      arm.write(pos);
      break;
    case 'K':
      pos = command.substring(1, command.length()).toInt();
      gripper.write(pos);
      break;
    default:
      break;
  }
}


void update_display() {
  unsigned long int now = millis();
  if (now - display_timer < 100) {
    return;
  }
  display_timer = now;
  if (now - blink_timer > 3000) {
    drawEyes(eyes_blink);
    blink_timer = now;
    return;
  }
  switch(current_direction) {
    case dir_forward:
    case dir_backward:
    case dir_stop:
      drawEyes(eyes_forward);
      break;
    case dir_left:
      drawEyes(eyes_left);
      break;
    case dir_right:
      drawEyes(eyes_right);
      break;
  }
}


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(9600);
  // Setup motors
  left_motor.begin(LEFT_MOTOR_STEP_PIN, LEFT_MOTOR_DIR_PIN);
  right_motor.begin(RIGHT_MOTOR_STEP_PIN, RIGHT_MOTOR_DIR_PIN);
  left_motor.setAcceleration(ACCELERATION);
  right_motor.setAcceleration(ACCELERATION);
  // Setup servos
  arm.attach(ARM_SERVO_PIN);
  arm.write(ARM_DEFAULT_POSITION);
  gripper.attach(GRIPPER_SERVO_PIN);
  gripper.write(GRIPPER_DEFAULT_POSITION);
  // Init LED display
  display.setEnabled(true);
  display.setIntensity(DISPLAY_BRIGHTNESS);
}

String command;
void loop() {
  if (Serial.available() > 0) {
    int ch = Serial.read();
    if (ch == '\n') {
      run_robot(command);
      command = "";
    }
    else {
      command += (char)ch;
    }
  }

  left_motor.loop();
  right_motor.loop();
  update_display();
}