Zoidbot Mk2 – Fjärrstyrning med bluetooth

#include <SoftwareSerial.h>
#include <ContinuousStepper.h>

#define LEFT_MOTOR_STEP_PIN 5
#define LEFT_MOTOR_DIR_PIN 13
#define RIGHT_MOTOR_STEP_PIN 18
#define RIGHT_MOTOR_DIR_PIN 19
#define BLUETOOTH_RX_PIN 26
#define BLUETOOTH_TX_PIN 25

#define ACCELERATION 12000
#define FORWARD_SPEED 8000
#define BACK_SPEED 8000
#define TURN_SPEED 8000


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

SoftwareSerial BTReceiver(BLUETOOTH_RX_PIN, BLUETOOTH_TX_PIN);



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 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);
  BTReceiver.begin(9600);
  Serial.begin(115200);
}

void loop() {
  if (BTReceiver.available() > 0) {
    int ch = BTReceiver.read();
    switch(ch) {
      case 'F':
        forward();
        Serial.println("Forward");
        break;
      case 'G':
        backward();
        Serial.println("Backward");
        break;
      case 'L':
        turn_left();
        Serial.println("Rotate left");
        break;
      case 'R':
        turn_right();
        Serial.println("Rotate right");
        break;
      case 'S':
        stop();
        Serial.println("Stop");
        break;
      default:
        break;
    }
  }

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