IMU-Stabilization.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 = 50;

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

#define INTERRUPT_PIN 6

//Variable to store the battery voltage
#define MIN_BAT_V 8
int batteryVoltage;

// Constants
float ujoint_to_head = 22;
float bjoint_head_offset = 12;
float bjoint_head_x3_offset = -10;
float head_width = 45; 
float servo_base_width = 45; 
float servo_base_ujoint = -50; 
float arm_length = 60; 
float ujoint_height = 42; 
float height_offset_servo_ujoint = -10; 
float servo_arm_length = 14; 

int receive_config = 0;

double pitch_offset = 0;
double yaw_offset = 0;


double rotate_pitch_vector_new_x1(float pitch_angle, double old_v_x1, double old_v_x2, double old_v_x3) {
  return (cos(pitch_angle) * old_v_x1) + (sin(pitch_angle) * old_v_x3);
}
double rotate_pitch_vector_new_x2(float pitch_angle, double old_v_x1, double old_v_x2, double old_v_x3) {
  return old_v_x2;
}
double rotate_pitch_vector_new_x3(float pitch_angle, double old_v_x1, double old_v_x2, double old_v_x3) {
  return ((-1 * sin(pitch_angle) * old_v_x1) + (cos(pitch_angle)*old_v_x3));
}

double rotate_yaw_vector_new_x1(float yaw_angle, double old_v_x1, double old_v_x2, double old_v_x3) {
  return (cos(yaw_angle) * old_v_x1) + ((-1 * sin(yaw_angle)) * old_v_x2);
}
double rotate_yaw_vector_new_x2(float yaw_angle, double old_v_x1, double old_v_x2, double old_v_x3) {
  return (sin(yaw_angle) * old_v_x1) + (cos(yaw_angle) * old_v_x2);
}
double rotate_yaw_vector_new_x3(float yaw_angle, double old_v_x1, double old_v_x2, double old_v_x3) {
  return old_v_x3;
}

int calcServoOnePos(double angle_x2, double angle_x3) {
  double ujoint_bjoint_1_vector_x1 = ujoint_to_head - bjoint_head_offset;
  double ujoint_bjoint_1_vector_x2 = head_width / 2;
  double ujoint_bjoint_1_vector_x3 = bjoint_head_x3_offset;
  //Serial.print("Original Vektor: ");
  //Serial.print(ujoint_bjoint_1_vector_x1);
  //Serial.print(", ");
  //Serial.print(ujoint_bjoint_1_vector_x2);
  //Serial.print(", ");
  //Serial.println(ujoint_bjoint_1_vector_x3);
  
  double pitch_v_x1 = rotate_pitch_vector_new_x1(angle_x2, ujoint_bjoint_1_vector_x1, ujoint_bjoint_1_vector_x2, ujoint_bjoint_1_vector_x3);
  double pitch_v_x2 = rotate_pitch_vector_new_x2(angle_x2, ujoint_bjoint_1_vector_x1, ujoint_bjoint_1_vector_x2, ujoint_bjoint_1_vector_x3);
  double pitch_v_x3 = rotate_pitch_vector_new_x3(angle_x2, ujoint_bjoint_1_vector_x1, ujoint_bjoint_1_vector_x2, ujoint_bjoint_1_vector_x3);
  double rot_v_x1 = rotate_yaw_vector_new_x1(angle_x3, pitch_v_x1, pitch_v_x2, pitch_v_x3);
  double rot_v_x2 = rotate_yaw_vector_new_x2(angle_x3, pitch_v_x1, pitch_v_x2, pitch_v_x3);
  double rot_v_x3 = rotate_yaw_vector_new_x3(angle_x3, pitch_v_x1, pitch_v_x2, pitch_v_x3);
  //Serial.print("Pitch and Yaw rotated new Vektor: ");
  //Serial.print(rot_v_x1);
  //Serial.print(", ");
  //Serial.print(rot_v_x2);
  //Serial.print(", ");
  //Serial.println(rot_v_x3);
  double gegen_kat = rot_v_x3 - height_offset_servo_ujoint;
  double projected_hypo = sqrt((arm_length * arm_length) - (gegen_kat * gegen_kat));
  double topview_gegen_kat = rot_v_x2 - (servo_base_width / 2.0);
  double calc_shift = sqrt((projected_hypo * projected_hypo) - (topview_gegen_kat*topview_gegen_kat));
  double new_servo_x1_pos = rot_v_x1 - calc_shift;
  //Serial.print("New Servo x1 Pos: ");
  //Serial.println(new_servo_x1_pos);
  double constrained_servo_x1_pos = constrain(new_servo_x1_pos, servo_base_ujoint - servo_arm_length, servo_base_ujoint + servo_arm_length);
  //Serial.print("Constrained Servo x1 Pos: ");
  //Serial.println(constrained_servo_x1_pos);
  double mapped_servo_x1_pos = map(constrained_servo_x1_pos, servo_base_ujoint - servo_arm_length, servo_base_ujoint + servo_arm_length, 0, 180);
  //Serial.print("Mapped Servo x1 Pos: ");
  //Serial.println(mapped_servo_x1_pos);
  
  return mapped_servo_x1_pos;
}

int calcServoTwoPos(float angle_x2, float angle_x3) {
  double ujoint_bjoint_1_vector_x1 = ujoint_to_head - bjoint_head_offset;
  double ujoint_bjoint_1_vector_x2 = -1 * (head_width / 2);
  double ujoint_bjoint_1_vector_x3 = bjoint_head_x3_offset;
  //Serial.print("Original Vektor: ");
  //Serial.print(ujoint_bjoint_1_vector_x1);
  //Serial.print(", ");
  //Serial.print(ujoint_bjoint_1_vector_x2);
  //Serial.print(", ");
  //Serial.println(ujoint_bjoint_1_vector_x3);
  
  double pitch_v_x1 = rotate_pitch_vector_new_x1(angle_x2, ujoint_bjoint_1_vector_x1, ujoint_bjoint_1_vector_x2, ujoint_bjoint_1_vector_x3);
  double pitch_v_x2 = rotate_pitch_vector_new_x2(angle_x2, ujoint_bjoint_1_vector_x1, ujoint_bjoint_1_vector_x2, ujoint_bjoint_1_vector_x3);
  double pitch_v_x3 = rotate_pitch_vector_new_x3(angle_x2, ujoint_bjoint_1_vector_x1, ujoint_bjoint_1_vector_x2, ujoint_bjoint_1_vector_x3);
  double rot_v_x1 = rotate_yaw_vector_new_x1(angle_x3, pitch_v_x1, pitch_v_x2, pitch_v_x3);
  double rot_v_x2 = rotate_yaw_vector_new_x2(angle_x3, pitch_v_x1, pitch_v_x2, pitch_v_x3);
  double rot_v_x3 = rotate_yaw_vector_new_x3(angle_x3, pitch_v_x1, pitch_v_x2, pitch_v_x3);
  //Serial.print("Pitch and Yaw rotated new Vektor: ");
  //Serial.print(rot_v_x1);
  //Serial.print(", ");
  //Serial.print(rot_v_x2);
  //Serial.print(", ");
  //Serial.println(rot_v_x3);
  double gegen_kat = rot_v_x3 - height_offset_servo_ujoint;
  double projected_hypo = sqrt((arm_length * arm_length) - (gegen_kat * gegen_kat));
  double topview_gegen_kat = rot_v_x2 - (-1* (servo_base_width / 2.0));
  double calc_shift = sqrt((projected_hypo * projected_hypo) - (topview_gegen_kat*topview_gegen_kat));
  double new_servo_x1_pos = rot_v_x1 - calc_shift;
  //Serial.print("New Servo x1 Pos: ");
  //Serial.println(new_servo_x1_pos);
  double constrained_servo_x1_pos = constrain(new_servo_x1_pos, servo_base_ujoint - servo_arm_length, servo_base_ujoint + servo_arm_length);
  //Serial.print("Constrained Servo x1 Pos: ");
  //Serial.println(constrained_servo_x1_pos);
  double mapped_servo_x1_pos = map(constrained_servo_x1_pos, servo_base_ujoint - servo_arm_length, servo_base_ujoint + servo_arm_length, 0, 180);
  //Serial.print("Mapped Servo x1 Pos: ");
  //Serial.println(mapped_servo_x1_pos);
  
  return mapped_servo_x1_pos;
}

void setServoPosition(int angle_x2, int angle_x3) {
  //Serial.print("Got new Orientation Data: ");
  //Serial.print(angle_x2);
  //Serial.print(", ");
  //Serial.println(angle_x3);
  //Serial.println("Angles in Radians: ");
  double angle_x2_rad = (angle_x2 * 1000.0) / 57296.0;
  //Serial.print(angle_x2_rad);
  //Serial.print(", ");
  double angle_x3_rad = (angle_x3 * 1000.0) / 57296.0;
  //Serial.println(angle_x3_rad);
  int servoOnePos = calcServoOnePos(angle_x2_rad, angle_x3_rad);
  int servoTwoPos = calcServoTwoPos(angle_x2_rad, angle_x3_rad);
  //Serial.print("Got new Servo Position Data: ");
  //Serial.print(servoOnePos);
  //Serial.print(", ");
  //Serial.println(servoTwoPos);
  servo1.setAngle(servoOnePos);
  servo2.setAngle(servoTwoPos);
}


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

  /* 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);

  //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() {
  //Take the battery status
  float batteryVoltage = (float)battery.getConverted();

  //Reset to the default values if the battery level is lower than MIN_BAT_V
  if (batteryVoltage < MIN_BAT_V) 
  {
    Serial.println(" ");
    Serial.println("WARNING: LOW BATTERY");
    Serial.println("ALL SYSTEMS DOWN");
    while (batteryVoltage < MIN_BAT_V) {
      batteryVoltage = (float)battery.getConverted();
      delay(100);
    }
  }

  while (Serial.available() > 0) {
    receive_config = Serial.parseInt();
    // int pitch_angle = Serial.parseInt();
    // int yaw_angle = Serial.parseInt();

    if (Serial.read() == '\n') {
      // setServoPosition(pitch_angle, yaw_angle);
      //int servo_one_angle = constrain(pitch_angle, 0, 180);
      //int servo_two_angle = constrain(yaw_angle, 0, 180);
      //servo1.setAngle(servo_one_angle);
      //servo2.setAngle(servo_two_angle);
    }
  }

  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);

  /* Display the floating point data */
  if(receive_config == 0) {
    Serial.print("X: ");
    Serial.print(euler.x());
    Serial.print(" Y: ");
    Serial.print(euler.y());
    Serial.print(" Z: ");
    Serial.print(euler.z());
    Serial.println("\t\t");
  } else 
  if(receive_config == 1) {
    pitch_offset = euler.z();
    yaw_offset = euler.x();
    receive_config = 2;
  } else
  if(receive_config == 2) {
    Serial.print("New Servo Pos: ");
    Serial.print(euler.z() - pitch_offset);
    Serial.print(", ");
    Serial.println(euler.x() - yaw_offset);
  } else if (receive_config == 3) {
    setServoPosition(-1*(euler.z() - pitch_offset), (euler.x() - yaw_offset));
  }


  //Keep active the communication between MKR board & MKR Motor Carrier
  //Ping the SAMD11
  controller.ping();
  //wait
  delay(BNO055_SAMPLERATE_DELAY_MS);

}