Skip to content

Instantly share code, notes, and snippets.

@benongithub
Created April 4, 2023 09:30
Show Gist options
  • Save benongithub/828f2e9f7c0bf40d171542bd7872aecd to your computer and use it in GitHub Desktop.
Save benongithub/828f2e9f7c0bf40d171542bd7872aecd to your computer and use it in GitHub Desktop.
#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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment