#include <SoftwareSerial.h>
#include "DFRobot_BNO055.h"
#include "Wire.h"

typedef DFRobot_BNO055_IIC BNO;


BNO bno(&Wire, 0x28);
SoftwareSerial mySerial(8, 7);  // RX, TX
String bt;
int btint;
int pitch, roll, yaw;

void setup() {
  // Open serial communications and wait for port to open:
  Serial.begin(115200);
  mySerial.begin(38400);


  // BNO STARTUP
  bno.reset();
  while (bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin failed");
    delay(2000);
  }
  Serial.println("bno begin success");

  mySerial.begin(38400);
  pinMode(6, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(4, OUTPUT);
}

void loop() {  // run over and over


  bt = mySerial.readString();
  Serial.println(busy);
  bt.trim();

  if (bt.substring(0, 8) == "RDriveM:") {
    bt.remove(0, 8);
    btint = bt.toInt();


    if (btint < 10) {
      btint = 0;
    }

    Serial.println(btint);
    analogWrite(5, btint);
    digitalWrite(6, LOW);
  } else if (bt.substring(0, 8) == "LDriveM:") {
    bt.remove(0, 8);
    btint = bt.toInt();


    if (btint < 10) {
      btint = 0;
    }

    Serial.println(btint);
    digitalWrite(5, LOW);
    analogWrite(6, btint);
  }

  if (bt.substring(0, 9) == "RRWheelM:") {
    bt.remove(0, 9);
    btint = bt.toInt();


    if (btint < 10) {
      btint = 0;
    }
    Serial.println(btint);
    analogWrite(10, btint);
    digitalWrite(9, LOW);

  } else if (bt.substring(0, 9) == "LRWheelM:") {
    bt.remove(0, 9);
    btint = bt.toInt();


    if (btint < 10) {
      btint = 0;
    }

    Serial.println(btint);
    digitalWrite(10, LOW);
    analogWrite(9, btint);
  }

  if (bt.substring(0, 6) == "HeadM:") {

    bt.remove(0, 6);
    btint = bt.toInt();

    if (btint < 10) {
      btint = 0;
    }

    Serial.println(btint);
    analogWrite(4, btint);
  }

  BNO::sEulAnalog_t gyro;
  gyro = bno.getEul();
  yaw = gyro.head;
  roll = gyro.pitch;
  pitch = gyro.roll;
  mySerial.print("Pitch:");
  mySerial.print(pitch);
  mySerial.print("Roll:");
  mySerial.print(roll);
  mySerial.print("Yaw:");
  mySerial.print(yaw);
  // delay(10);
}