Zoidbot Mk2 – Fjärrstyrning av robot med robotarm via bluetooth

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

#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


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

Servo arm;
Servo gripper;

unsigned long int arm_timer = 0;



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 stop() {
  left_motor.stop();
  right_motor.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 setup() {
  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);
  arm.attach(ARM_SERVO_PIN);
  arm.write(ARM_DEFAULT_POSITION);
  gripper.attach(GRIPPER_SERVO_PIN);
  gripper.write(GRIPPER_DEFAULT_POSITION);
  Serial.begin(9600);
}

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