OrientationToPython.ino

#include <ArduinoMotorCarrier.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

uint16_t BNO055_SAMPLERATE_DELAY_MS = 100;

Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire);

#define INTERRUPT_PIN 6

/*uint64_t servo_one_angle = 180;
uint64_t servo_two_angle = 180/2;*/

float servoOneSmoothed;
float servoOnePrev;
float servoTwoSmoothed;
float servoTwoPrev;

#define UPDATE_SERVO_DELAY 15
unsigned long previous_update_servo = 0;

#define UPDATE_SERVO_TWO_DELAY 15
unsigned long previous_update_two_servo = 0;

#define UPDATE_IMU_DELAY 80
unsigned long previous_update_imu = 0;

#define SERVO_ONE_MIN 40
#define SERVO_ONE_MAX 180 - SERVO_ONE_MIN

#define SERVO_TWO_MIN 40
#define SERVO_TWO_MAX 180 - SERVO_TWO_MIN

int servo_one_angle = SERVO_ONE_MIN + ((SERVO_ONE_MAX - SERVO_ONE_MIN) / 2);
int servo_two_angle = SERVO_TWO_MIN + ((SERVO_TWO_MAX - SERVO_TWO_MIN) / 2);

bool servoOneToggle = false;
bool servoTwoToggle = false;

void sendToPC(double* data1, double* data2, double* data3)
{
  byte* byteData1 = (byte*)(data1);
  byte* byteData2 = (byte*)(data2);
  byte* byteData3 = (byte*)(data3);
  byte buf[24] = {
    *(byteData1 + 0), *(byteData1 + 1), *(byteData1 + 2), *(byteData1 + 3), *(byteData1 + 4), *(byteData1 + 5), *(byteData1 + 6), *(byteData1 + 7),
    *(byteData2 + 0), *(byteData2 + 1), *(byteData2 + 2), *(byteData2 + 3), *(byteData2 + 4), *(byteData2 + 5), *(byteData2 + 6), *(byteData2 + 7),
    *(byteData3 + 0), *(byteData3 + 1), *(byteData3 + 2), *(byteData3 + 3), *(byteData3 + 4), *(byteData3 + 5), *(byteData3 + 6), *(byteData3 + 7)
    };
  Serial.write(buf, 24);
}

void sendLongToPC(uint64_t* data1, uint64_t* data2, uint64_t* data3)
{
  byte* byteData1 = (byte*)(data1);
  byte* byteData2 = (byte*)(data2);
  byte* byteData3 = (byte*)(data3);
  byte buf[24] = {
    *(byteData1 + 0), *(byteData1 + 1), *(byteData1 + 2), *(byteData1 + 3), *(byteData1 + 4), *(byteData1 + 5), *(byteData1 + 6), *(byteData1 + 7),
    *(byteData2 + 0), *(byteData2 + 1), *(byteData2 + 2), *(byteData2 + 3), *(byteData2 + 4), *(byteData2 + 5), *(byteData2 + 6), *(byteData2 + 7),
    *(byteData3 + 0), *(byteData3 + 1), *(byteData3 + 2), *(byteData3 + 3), *(byteData3 + 4), *(byteData3 + 5), *(byteData3 + 6), *(byteData3 + 7)
    };
  Serial.write(buf, 24);
}

void setup(void)
{
  Serial.begin(115200);

  while (!Serial) delay(10);  // wait for serial port to open!

  /* Initialise the sensor */
  if (!bno.begin())
  {
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while (1);
  }

  delay(2000);

  //Establishing the communication with the Motor Carrier
  if (controller.begin()) 
    {
      // Serial.print("MKR Motor Carrier connected, firmware version ");
      // Serial.println(controller.getFWVersion());
    } 
  else 
    {
      Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch");
      while (1);
    }

  // Reboot the motor controller; brings every value back to default
  // Serial.println("reboot");
  controller.reboot();
  delay(500);
  servo1.setAngle(servo_one_angle);
  servo2.setAngle(90);
  delay(2000);
  //Take the battery status
  /*float batteryVoltage = (float)battery.getConverted();
  Serial.print("Battery voltage: ");
  Serial.print(batteryVoltage);
  Serial.print("V, Raw ");
  Serial.println(battery.getRaw());*/
}

void loop(void)
{
  unsigned long currentImuMillis = millis();
  if (currentImuMillis - previous_update_imu >= UPDATE_IMU_DELAY)
  {
    previous_update_imu = currentImuMillis;
    sensors_event_t orientationData;
    bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
    printEvent(&orientationData);
  /*double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem
  imu::Quaternion quat = bno.getQuat();
  // Serial.print("qW: ");
  // Serial.print(quat.w(), 4);
  x = quat.x();
  y = quat.y();
  z = quat.z();
  sendToPC(&x, &y, &z);*/
  /*delay(BNO055_SAMPLERATE_DELAY_MS);*/
  }
  

  /*while (Serial.available() > 0) {
    uint32_t servo_one_read = Serial.parseInt();
    uint32_t servo_two_read = Serial.parseInt();

    if (Serial.read() == '\n') {
      servo_one_angle = constrain(servo_one_read, 0, 180);
      servo_two_angle = constrain(servo_two_read, 0, 180);
    }
  }*/
  
  unsigned long currentServoMillis = millis();
  if (currentServoMillis - previous_update_servo >= UPDATE_SERVO_DELAY)
  {
    previous_update_servo = currentServoMillis;
    /*servoOneSmoothed = (servo_one_angle * 0.001) + (0.999 * servoOnePrev);
    servoOnePrev = servoOneSmoothed;
    Serial.println((int) servoOneSmoothed);*/
    // Serial.println(servo_one_angle);
    if(servoOneToggle) {
      if (servo_one_angle <= SERVO_ONE_MAX) {
        servo_one_angle += 1;
      } else {
        servoOneToggle = !servoOneToggle;
      }
    } else {
      if (servo_one_angle >= SERVO_ONE_MIN) {
        servo_one_angle -= 1;
      } else {
        servoOneToggle = !servoOneToggle;
      }
    }
    servo1.setAngle(servo_one_angle);
    /*if (servo_one_angle < 100) {
      servo_one_angle++;
    } else {
      servo_one_angle = 0;
    }
    servo1.setAngle((int) servoOneSmoothed);*/
    /*uint64_t placeholder = 0;
    sendLongToPC(&servo_one_angle, &servo_two_angle, &placeholder);*/
  }
  unsigned long currentServoTwoMillis = millis();
  if (currentServoTwoMillis - previous_update_two_servo >= UPDATE_SERVO_TWO_DELAY)
  {
    previous_update_two_servo = currentServoTwoMillis;
    /*if (servo_two_angle > 0) {
      servo_two_angle--;
    } else {
      servo_two_angle = 180;
    }*/
    if(servoTwoToggle) {
      if (servo_two_angle <= SERVO_TWO_MAX) {
        servo_two_angle += 1;
      } else {
        servoTwoToggle = !servoTwoToggle;
      }
    } else {
      if (servo_two_angle >= SERVO_TWO_MIN) {
        servo_two_angle -= 1;
      } else {
        servoTwoToggle = !servoTwoToggle;
      }
    }
    servo2.setAngle(servo_two_angle);;
    /*uint64_t placeholder = 0;
    sendLongToPC(&servo_one_angle, &servo_two_angle, &placeholder);*/
  }  
}

void printEvent(sensors_event_t* event) {
  double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem
  if (event->type == SENSOR_TYPE_ORIENTATION) {
    // Serial.print("Orient:");
    x = event->orientation.x;
    y = event->orientation.y;
    z = event->orientation.z;

    sendToPC(&x, &y, &z);
  }
}