Skip to content

Instantly share code, notes, and snippets.

@benongithub
Created February 2, 2023 16:08
Show Gist options
  • Save benongithub/a22a70a81a9beb208f86baa23f3d7f12 to your computer and use it in GitHub Desktop.
Save benongithub/a22a70a81a9beb208f86baa23f3d7f12 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 = 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);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment