mirror of
https://gitlab.waag.org/make/fablab/interns/2025/sam.git
synced 2025-08-03 11:54:58 +00:00
It finally outputs data
now need to get it parsed correctly
This commit is contained in:
@@ -1,5 +1,6 @@
|
||||
#include <Arduino.h>
|
||||
#include <SparkFun_BNO080_Arduino_Library.h>
|
||||
#include <Wire.h>
|
||||
#include "SparkFun_BNO080_Arduino_Library.h"
|
||||
#include <ESP32Servo.h>
|
||||
#include <esp_now.h>
|
||||
#include <WiFi.h>
|
||||
@@ -53,17 +54,16 @@ static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to ma
|
||||
// #define GYRO_2000DPS
|
||||
|
||||
// Uncomment only one full scale accelerometer range (G's)
|
||||
// #define ACCEL_2G // Default
|
||||
#define ACCEL_2G // Default
|
||||
// #define ACCEL_4G
|
||||
#define ACCEL_8G
|
||||
// #define ACCEL_8G
|
||||
// #define ACCEL_16G
|
||||
|
||||
//========================================================================================================================//
|
||||
|
||||
// REQUIRED LIBRARIES (included with download in main sketch folder)
|
||||
|
||||
#include <Wire.h> //I2c communication
|
||||
#include <SPI.h> //SPI communication
|
||||
#include <Wire.h> //I2c communication
|
||||
|
||||
#if defined USE_SBUS_RX
|
||||
#include "src/SBUS/SBUS.h" //sBus interface
|
||||
@@ -77,6 +77,7 @@ static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to ma
|
||||
#include "src/MPU6050/MPU6050.h"
|
||||
MPU6050 mpu6050;
|
||||
#elif defined USE_MPU9250_SPI
|
||||
#include <SPI.h> //SPI communication
|
||||
#include "src/MPU9250/MPU9250.h"
|
||||
MPU9250 mpu9250(SPI2, 36);
|
||||
#elif defined USE_BNO085_I2C
|
||||
@@ -109,14 +110,14 @@ BNO080 myIMU;
|
||||
#define ACCEL_FS_SEL_8 mpu9250.ACCEL_RANGE_8G
|
||||
#define ACCEL_FS_SEL_16 mpu9250.ACCEL_RANGE_16G
|
||||
#elif defined USE_BNO085_I2C
|
||||
#define GYRO_FS_SEL_250 MPU6050_GYRO_FS_250
|
||||
#define GYRO_FS_SEL_500 MPU6050_GYRO_FS_500
|
||||
#define GYRO_FS_SEL_1000 MPU6050_GYRO_FS_1000
|
||||
#define GYRO_FS_SEL_2000 MPU6050_GYRO_FS_2000
|
||||
#define ACCEL_FS_SEL_2 MPU6050_ACCEL_FS_2
|
||||
#define ACCEL_FS_SEL_4 MPU6050_ACCEL_FS_4
|
||||
#define ACCEL_FS_SEL_8 MPU6050_ACCEL_FS_8
|
||||
#define ACCEL_FS_SEL_16 MPU6050_ACCEL_FS_16
|
||||
#define GYRO_FS_SEL_250 0
|
||||
#define GYRO_FS_SEL_500 1
|
||||
#define GYRO_FS_SEL_1000 2
|
||||
#define GYRO_FS_SEL_2000 3
|
||||
#define ACCEL_FS_SEL_2 0
|
||||
#define ACCEL_FS_SEL_4 1
|
||||
#define ACCEL_FS_SEL_8 2
|
||||
#define ACCEL_FS_SEL_16 3
|
||||
#endif
|
||||
|
||||
#if defined GYRO_250DPS
|
||||
@@ -220,7 +221,7 @@ const int ch3Pin = 17; // ele
|
||||
const int ch4Pin = 20; // rudd
|
||||
const int ch5Pin = D7; // gear (throttle cut)
|
||||
const int ch6Pin = D7; // aux1 (free aux channel)
|
||||
const int PPM_Pin = 23;
|
||||
const int PPM_Pin = D7;
|
||||
// OneShot125 ESC pin outputs:
|
||||
const int m1Pin = D0;
|
||||
const int m2Pin = D1;
|
||||
@@ -366,7 +367,11 @@ void setup()
|
||||
{
|
||||
Serial.begin(9600); // USB serial
|
||||
Serial.println("Serial started");
|
||||
delay(10000);
|
||||
|
||||
// Initialize IMU communication
|
||||
Serial.println("Initiating IMU");
|
||||
IMUinit();
|
||||
|
||||
Serial.println("Initiating pins");
|
||||
// Initialize all pins
|
||||
// pinMode(13, OUTPUT); // Pin 13 LED blinker on board, do not modify
|
||||
@@ -408,9 +413,6 @@ void setup()
|
||||
channel_5_pwm = channel_5_fs;
|
||||
channel_6_pwm = channel_6_fs;
|
||||
|
||||
// Initialize IMU communication
|
||||
IMUinit();
|
||||
|
||||
delay(5);
|
||||
|
||||
// Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level when powered up
|
||||
@@ -463,10 +465,10 @@ void loop()
|
||||
loopBlink(); // Indicate we are in main loop with short blink every 1.5 seconds
|
||||
|
||||
// Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE:
|
||||
printRadioData(); //Prints radio pwm values (expected: 1000 to 2000)
|
||||
printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle)
|
||||
// printRadioData(); //Prints radio pwm values (expected: 1000 to 2000)
|
||||
// printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle)
|
||||
// printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest)
|
||||
// printAccelData(); //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level)
|
||||
printAccelData(); //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level)
|
||||
// printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300)
|
||||
// printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level)
|
||||
// printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1)
|
||||
@@ -614,12 +616,21 @@ void IMUinit()
|
||||
mpu9250.setSrd(0); // sets gyro and accel read to 1khz, magnetometer read to 100hz
|
||||
#elif defined USE_BNO085_I2C
|
||||
Wire.begin();
|
||||
myIMU.begin();
|
||||
if (myIMU.begin() == false) // from sparkfun example
|
||||
{
|
||||
Serial.println("BNO080 not detected at default I2C address. Check your jumpers and the hookup guide. Freezing...");
|
||||
while (1)
|
||||
;
|
||||
}
|
||||
Wire.setClock(400000); // Increase I2C data rate to 400kHz
|
||||
Serial.println("IMU initialized");
|
||||
myIMU.enableMagnetometer(50);
|
||||
myIMU.enableGyro(50);
|
||||
delay(100);
|
||||
myIMU.enableAccelerometer(50);
|
||||
delay(100);
|
||||
myIMU.enableMagnetometer(50);
|
||||
delay(500);
|
||||
|
||||
|
||||
#endif
|
||||
}
|
||||
@@ -636,12 +647,18 @@ void getIMUdata()
|
||||
* the constant errors found in calculate_IMU_error() on startup are subtracted from the accelerometer and gyro readings.
|
||||
*/
|
||||
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ, MgX, MgY, MgZ;
|
||||
|
||||
#if defined USE_MPU6050_I2C
|
||||
mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ);
|
||||
#elif defined USE_MPU9250_SPI
|
||||
mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ);
|
||||
#elif defined USE_BNO085_I2C
|
||||
|
||||
// Keep waiting until data becomes available
|
||||
while (!myIMU.dataAvailable())
|
||||
{
|
||||
delay(10); // Small delay to avoid hammering the I2C bus too hard
|
||||
}
|
||||
|
||||
// BNO magnetometer
|
||||
MgX = myIMU.getMagX();
|
||||
MgY = myIMU.getMagY();
|
||||
@@ -652,10 +669,11 @@ void getIMUdata()
|
||||
GyY = myIMU.getGyroY() * 180 / PI;
|
||||
GyZ = myIMU.getGyroZ() * 180 / PI;
|
||||
|
||||
// Acelleration (dont know if I need linear or normal. One way to find out)
|
||||
// Acceleration
|
||||
AcX = myIMU.getAccelX();
|
||||
AcY = myIMU.getAccelY();
|
||||
AcZ = myIMU.getAccelZ();
|
||||
|
||||
#endif
|
||||
|
||||
// Accelerometer
|
||||
@@ -732,6 +750,13 @@ void calculate_IMU_error()
|
||||
#elif defined USE_MPU9250_SPI
|
||||
mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ);
|
||||
#elif defined USE_BNO085_I2C
|
||||
|
||||
while (!myIMU.dataAvailable())
|
||||
{
|
||||
delay(10); // Small delay to avoid hammering the I2C bus too hard
|
||||
}
|
||||
|
||||
|
||||
// bno magnetometer
|
||||
MgX = myIMU.getMagX();
|
||||
MgY = myIMU.getMagY();
|
||||
@@ -741,13 +766,13 @@ void calculate_IMU_error()
|
||||
GyX = myIMU.getGyroX() * 180 / PI;
|
||||
GyY = myIMU.getGyroY() * 180 / PI;
|
||||
GyZ = myIMU.getGyroZ() * 180 / PI;
|
||||
Serial.println(GyZ);
|
||||
|
||||
|
||||
// Acelleration (dont know if I need linear or normal. One way to find out)
|
||||
AcX = myIMU.getAccelX();
|
||||
AcY = myIMU.getAccelY();
|
||||
AcZ = myIMU.getAccelZ();
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
AccX = AcX / ACCEL_SCALE_FACTOR;
|
||||
|
||||
Reference in New Issue
Block a user