Skip to content

Commit

Permalink
improved imu support
Browse files Browse the repository at this point in the history
  • Loading branch information
SaintSampo committed Oct 31, 2024
1 parent fc31701 commit 6424e0c
Show file tree
Hide file tree
Showing 9 changed files with 289 additions and 265 deletions.
90 changes: 90 additions & 0 deletions examples/9DOF_IMU_Robot/9DOF_IMU_Robot.ino
Original file line number Diff line number Diff line change
@@ -0,0 +1,90 @@
#include <Alfredo_NoU3.h>

int interruptPinLSM6 = 48;
int interruptPinMMC5 = 47;

float acceleration_x, acceleration_y, acceleration_z;
float gyroscope_x, gyroscope_y, gyroscope_z;
float magnetometer_X, magnetometer_Y, magnetometer_Z;

volatile bool newDataAvailableLSM6 = true;
volatile bool newDataAvailableMMC5 = true;

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

// Initialize LSM6
if (LSM6.begin(Wire1) == false) {
Serial.println("LSM6 did not respond - check your wiring. Freezing.");
while (true) {};
}
pinMode(interruptPinLSM6, INPUT);
attachInterrupt(digitalPinToInterrupt(interruptPinLSM6), interruptRoutineLSM6, RISING);
LSM6.enableInterrupt();

// Initialize MMC5
if (MMC5.begin(Wire1) == false) {
Serial.println("MMC5983MA did not respond - check your wiring. Freezing.");
while (true) {};
}
MMC5.softReset();
MMC5.setFilterBandwidth(800);
MMC5.setContinuousModeFrequency(100);
MMC5.enableAutomaticSetReset();
MMC5.enableContinuousMode();
pinMode(interruptPinMMC5, INPUT);
attachInterrupt(digitalPinToInterrupt(interruptPinMMC5), interruptRoutineMMC5, RISING);
MMC5.enableInterrupt();

Serial.println();
Serial.println("Accel_X\tAccel_Y\tAccel_Z\tGyro_X\tGyro_Y\tGyro_Z\tMag_X\tMag_Y\tMag_Z");
}

void loop() {
// Check LSM6 for new data
if (newDataAvailableLSM6) {
newDataAvailableLSM6 = false;

if (LSM6.accelerationAvailable()) {
LSM6.readAcceleration(&acceleration_x, &acceleration_y, &acceleration_z);
}

if (LSM6.gyroscopeAvailable()) {
LSM6.readGyroscope(&gyroscope_x, &gyroscope_y, &gyroscope_z);
}
}

// Check MMC5983MA for new data
if (newDataAvailableMMC5) {
newDataAvailableMMC5 = false;
MMC5.clearMeasDoneInterrupt();

MMC5.readAccelerometer(&magnetometer_X, &magnetometer_Y, &magnetometer_Z); // Results in µT (microteslas).
}

formatPrint(acceleration_x);
formatPrint(acceleration_y);
formatPrint(acceleration_z);
formatPrint(gyroscope_x);
formatPrint(gyroscope_y);
formatPrint(gyroscope_z);
formatPrint(magnetometer_X);
formatPrint(magnetometer_Y);
formatPrint(magnetometer_Z);
Serial.println('\t');
}

void interruptRoutineLSM6() {
newDataAvailableLSM6 = true;
}

void interruptRoutineMMC5() {
newDataAvailableMMC5 = true;
}

void formatPrint(float num) {
char buffer[11];
snprintf(buffer, sizeof(buffer), "%7.3f ", num);
Serial.print(buffer);
}
Original file line number Diff line number Diff line change
@@ -1,51 +1,63 @@
/*
Arduino LSM6DSOX - Simple Accelerometer
This example reads the acceleration values from the LSM6DSOX
sensor and continuously prints them to the Serial Monitor
or Serial Plotter.
#include <Alfredo_NoU3.h>

The circuit:
- Arduino Nano RP2040 Connect
int interruptPinLSM6 = 48;

created 10 May 2021
by Arturo Guadalupi
float acceleration_x, acceleration_y, acceleration_z;
float gyroscope_x, gyroscope_y, gyroscope_z;

This example code is in the public domain.
*/

#include <Alfredo_NoU3.h>
volatile bool newDataAvailable = true;

void setup() {
Wire1.setPins(35, 36);

Serial.begin(9600);
while (!Serial);

if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
NoU3.begin();
Serial.begin(115200);

while (1);
if (LSM6.begin(Wire1) == false) {
Serial.println("LSM6 did not respond - check your wiring. Freezing.");
while (true) {};
}
pinMode(interruptPinLSM6, INPUT);
attachInterrupt(digitalPinToInterrupt(interruptPinLSM6), interruptRoutine, RISING);
LSM6.enableInterrupt();

Serial.print("Accelerometer sample rate = ");
Serial.print(IMU.accelerationSampleRate());
Serial.print(LSM6.accelerationSampleRate());
Serial.println(" Hz");
Serial.println();
Serial.println("Acceleration in g's");
Serial.println("X\tY\tZ");
Serial.println("X\tY\tZ\tX\tY\tZ");

newDataAvailable = true;
}

void loop() {
float x, y, z;
if (newDataAvailable == true) {
newDataAvailable = false;
if (LSM6.accelerationAvailable()) {
LSM6.readAcceleration(&acceleration_x, &acceleration_y, &acceleration_z); // Results are in g (earth gravity).
}

if (IMU.accelerationAvailable()) {
IMU.readAcceleration(x, y, z);
if (LSM6.gyroscopeAvailable()) {
LSM6.readGyroscope(&gyroscope_x, &gyroscope_y, &gyroscope_z); // Results are in degrees/second.
}

Serial.print(x);
Serial.print('\t');
Serial.print(y);
Serial.print('\t');
Serial.println(z);
printState();
}
}

void interruptRoutine() {
newDataAvailable = true;
}

void printState() {
Serial.print(acceleration_x);
Serial.print('\t');
Serial.print(acceleration_y);
Serial.print('\t');
Serial.print(acceleration_z);
Serial.print('\t');
Serial.print(gyroscope_x);
Serial.print('\t');
Serial.print(gyroscope_y);
Serial.print('\t');
Serial.println(gyroscope_z);
}
59 changes: 20 additions & 39 deletions examples/MMC59_Wire1_SimpleMag/MMC59_Wire1_SimpleMag.ino
Original file line number Diff line number Diff line change
@@ -1,40 +1,28 @@

#include <Alfredo_NoU3.h>

SFE_MMC5983MA myMag;
int interruptPinMMC5 = 47;

int interruptPin = 47;
float magnetometer_X = 0, magnetometer_Y = 0, magnetometer_Z = 0;

volatile bool newDataAvailable = true;
uint32_t rawValueX = 0;
uint32_t rawValueY = 0;
uint32_t rawValueZ = 0;
double scaledX = 0;
double scaledY = 0;
double scaledZ = 0;
double heading = 0;

void setup()
{
NoU3.begin();
Serial.begin(115200);
Serial.println("MMC5983MA Example");

Wire1.begin(35, 36);

pinMode(interruptPin, INPUT);
attachInterrupt(digitalPinToInterrupt(interruptPin), interruptRoutine, RISING);

if (myMag.begin(Wire1) == false){
if (MMC5.begin(Wire1) == false){
Serial.println("MMC5983MA did not respond - check your wiring. Freezing.");
while (true);
while (true){};
}

myMag.softReset();
myMag.setFilterBandwidth(800);
myMag.setContinuousModeFrequency(100);
myMag.enableAutomaticSetReset();
myMag.enableContinuousMode();
myMag.enableInterrupt();
MMC5.softReset();
MMC5.setFilterBandwidth(800);
MMC5.setContinuousModeFrequency(100);
MMC5.enableAutomaticSetReset();
MMC5.enableContinuousMode();
pinMode(interruptPinMMC5, INPUT);
attachInterrupt(digitalPinToInterrupt(interruptPinMMC5), interruptRoutine, RISING);
MMC5.enableInterrupt();

newDataAvailable = true;
}
Expand All @@ -43,30 +31,23 @@ void loop()
{
if (newDataAvailable == true){
newDataAvailable = false;
myMag.clearMeasDoneInterrupt();

myMag.readFieldsXYZ(&rawValueX, &rawValueY, &rawValueZ);
MMC5.clearMeasDoneInterrupt();

scaledX = (double)rawValueX - 131072.0;
scaledX /= 131072.0;

scaledY = (double)rawValueY - 131072.0;
scaledY /= 131072.0;

scaledZ = (double)rawValueZ - 131072.0;
scaledZ /= 131072.0;
MMC5.readAccelerometer(&magnetometer_X, &magnetometer_Y, &magnetometer_Z); // Results are in uT (microteslas).

Serial.print("X: ");
Serial.print(scaledX * 800.0, 3);
Serial.print(magnetometer_X, 3);

Serial.print(" Y: ");
Serial.print(scaledY * 800.0, 3);
Serial.print(magnetometer_Y, 3);

Serial.print(" Z: ");
Serial.println(scaledZ * 800.0, 3);
Serial.println(magnetometer_Z, 3);
}
}

void interruptRoutine() {
newDataAvailable = true;
}


46 changes: 25 additions & 21 deletions src/Alfredo_NoU3.cpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,10 @@
#include "esp32-hal-ledc.h"
#include "Arduino.h"

#include "Alfredo_NoU3.h"

#include "Alfredo_NoU3_LSM6.h"
#include "Alfredo_NoU3_MMC5.h"
#include "Alfredo_NoU3_PCA9.h"

#if !defined(NO_GLOBAL_INSTANCES) && !defined(NO_GLOBAL_NOU3)
Expand All @@ -13,6 +17,12 @@ float fmap(float val, float in_min, float in_max, float out_min, float out_max)
return (val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void NoU_Agent::begin()
{
NoU3_PCA9_Begin();
Wire1.begin(35, 36, 400000);
}

NoU_Motor::NoU_Motor(uint8_t motorPort)
{
this->motorPort = motorPort;
Expand Down Expand Up @@ -131,14 +141,12 @@ float NoU_Drivetrain::applyInputCurve(float input) {
}

void NoU_Drivetrain::setMotors(float frontLeftPower, float frontRightPower, float rearLeftPower, float rearRightPower) {
switch (drivetrainType) {
case FOUR_MOTORS:
rearLeftMotor->set(rearLeftPower);
rearRightMotor->set(rearRightPower);
case TWO_MOTORS:
frontLeftMotor->set(frontLeftPower);
frontRightMotor->set(frontRightPower);
if (drivetrainType == FOUR_MOTORS) {
rearLeftMotor->set(rearLeftPower);
rearRightMotor->set(rearRightPower);
}
frontLeftMotor->set(frontLeftPower);
frontRightMotor->set(frontRightPower);
}

void NoU_Drivetrain::tankDrive(float leftPower, float rightPower) {
Expand Down Expand Up @@ -247,25 +255,21 @@ void NoU_Drivetrain::holonomicDrive(float xVelocity, float yVelocity, float rota
}

void NoU_Drivetrain::setMinimumOutput(float minimumOutput) {
switch (drivetrainType) {
case FOUR_MOTORS:
rearLeftMotor->setMinimumOutput(minimumOutput);
rearRightMotor->setMinimumOutput(minimumOutput);
case TWO_MOTORS:
frontLeftMotor->setMinimumOutput(minimumOutput);
frontRightMotor->setMinimumOutput(minimumOutput);
if (drivetrainType == FOUR_MOTORS) {
rearLeftMotor->setMinimumOutput(minimumOutput);
rearRightMotor->setMinimumOutput(minimumOutput);
}
frontLeftMotor->setMinimumOutput(minimumOutput);
frontRightMotor->setMinimumOutput(minimumOutput);
}

void NoU_Drivetrain::setMaximumOutput(float maximumOutput) {
switch (drivetrainType) {
case FOUR_MOTORS:
rearLeftMotor->setMaximumOutput(maximumOutput);
rearRightMotor->setMaximumOutput(maximumOutput);
case TWO_MOTORS:
frontLeftMotor->setMaximumOutput(maximumOutput);
frontRightMotor->setMaximumOutput(maximumOutput);
if (drivetrainType == FOUR_MOTORS) {
rearLeftMotor->setMaximumOutput(maximumOutput);
rearRightMotor->setMaximumOutput(maximumOutput);
}
frontLeftMotor->setMaximumOutput(maximumOutput);
frontRightMotor->setMaximumOutput(maximumOutput);
}

void NoU_Drivetrain::setInputExponent(float inputExponent) {
Expand Down
9 changes: 4 additions & 5 deletions src/Alfredo_NoU3.h
Original file line number Diff line number Diff line change
@@ -1,12 +1,11 @@
#ifndef ALFREDO_NOU3_H
#define ALFREDO_NOU3_H

#include "Alfredo_NoU3_PCA9.h"
#include "Alfredo_NoU3_Encoder.h"
#include <inttypes.h>

#include "Alfredo_NoU3_LSM6.h"
#include "Alfredo_NoU3_MMC5.h"

#include <inttypes.h>
#include "Alfredo_NoU3_PCA9.h"

#define PIN_SNS_VERSION 1
#define PIN_SNS_VIN 2
Expand Down Expand Up @@ -46,7 +45,7 @@ const int RSL_PWM_FREQ = 1000; // Hz

class NoU_Agent {
public:
void begin(){ NoU3_PCA9_Begin(); };
void begin();
float getBatteryVoltage(){ return analogReadMilliVolts(PIN_SNS_VIN) * 0.001 * 7.818; };
float getVersionVoltage(){ return analogReadMilliVolts(PIN_SNS_VERSION) * 0.001 ; };
};
Expand Down
Loading

0 comments on commit 6424e0c

Please sign in to comment.