From 35402b9ee7e1188eb5f47682b8c581fd9f0e85dc Mon Sep 17 00:00:00 2001
From: Aaron John Sabu <aaronjohnsabu1999@gmail.com>
Date: Tue, 26 Oct 2021 21:49:26 -0700
Subject: [PATCH] Added basic IMU code
---
.../Laptop_Code/ESP32_slave/ESP32_slave.ino | 17 +-
.../ESP32_master/ESP32_master.ino | 8 +-
Code/Control/Laptop_Code/main_keyboard.py | 2 +-
Code/IMU_Code/IMU_Code.ino | 295 ++++++++++++++++++
4 files changed, 313 insertions(+), 9 deletions(-)
create mode 100644 Code/IMU_Code/IMU_Code.ino
diff --git a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
index a6e02a9..63ee808 100644
--- a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
@@ -11,8 +11,12 @@
// MAC Address of the receiver (MASTER)
//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}; // #2
-uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
-// uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
+//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
+uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x9B, 0x04, 0x98}; // #8
String success;
// Define variables to store incoming readings
@@ -93,6 +97,7 @@ void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
// Callback when data is received
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
memcpy(&receivedData, incomingData, sizeof(receivedData));
+ Serial.println("OnDataRecv");
// the data format: 225 225 225 225 + + - -
// + up / forward
// - down / backward
@@ -108,7 +113,7 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
count = 0;
print_count=0;
change_count = 0;
- // control_motion();
+ control_motion();
}
// =============================== All the setup ===========================================
@@ -135,7 +140,7 @@ void setup() {
pinMode(LED, OUTPUT);
digitalWrite(LED, HIGH);
digitalWrite(LED, LOW);
-
+ Serial.println("Preliminary Setup done");
// -------------- lidar part --------------------
if (distanceSensor.begin() == 0){
Serial.println("Sensor online!");
@@ -228,12 +233,14 @@ void loop()
print_received_Data();
//-------------------------------------------------------------------------------------
- control_motion();
+ // control_motion();
//-------------------------------------------------------------------------------------
}
// ================================== ^ Main loop ^ ===================================================
void control_motion(){
// vertical motor
+ Serial.println("control_motion");
+ Serial.println(abs(Rec_pwm1));
motorVertical_L->setSpeed(abs(Rec_pwm1));
motorVertical_R->setSpeed(abs(Rec_pwm2));
diff --git a/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino b/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino
index 1b97e0d..446b250 100644
--- a/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino
+++ b/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino
@@ -5,11 +5,13 @@
// ==================================== broadcast address ============================================
// MAC Address of the receiver (SLAVE)
//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
-// uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}; // #2
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}; // #2
//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
-// uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
-uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
+uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
+//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x9B, 0x04, 0x98}; // #8
// ==================================== global data =================================================
String success;
// Define variables to store BME280 readings to be sent
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index 757c838..d2efd77 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -462,7 +462,7 @@ def variables_change_once():
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM22" # Based on your own serial port number
+ PORT = "COM3" # Based on your own serial port number
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
diff --git a/Code/IMU_Code/IMU_Code.ino b/Code/IMU_Code/IMU_Code.ino
new file mode 100644
index 0000000..47031f2
--- /dev/null
+++ b/Code/IMU_Code/IMU_Code.ino
@@ -0,0 +1,295 @@
+// Basic demo for magnetometer readings from Adafruit LIS3MDL
+
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_LIS3MDL.h>
+#include <Adafruit_LSM6DSOX.h>
+
+Adafruit_LIS3MDL lis3mdl;
+Adafruit_LSM6DSOX sox;
+
+#define LSM_CS 10
+#define LSM_MOSI 11
+#define LSM_MISO 12
+#define LSM_SCK 13
+
+#define LIS3MDL_CS 10
+#define LIS3MDL_MOSI 11
+#define LIS3MDL_MISO 12
+#define LIS3MDL_CLK 13
+
+double posX = 0.00;
+double posY = 0.00;
+double posZ = 0.00;
+double velX = 0.00;
+double velY = 0.00;
+double velZ = 0.00;
+double accX = 0.00;
+double accY = 0.00;
+double accZ = 0.00;
+double gX = 0.00;
+double gY = 0.00;
+double gZ = 10.00;
+double prevTime = 0.00;
+
+void setup(void) {
+ Serial.begin(115200);
+ while (!Serial) delay(10); // will pause Zero, Leonardo, etc until serial console opens
+
+
+ // Try to initialize!
+ if (!sox.begin_I2C()) {
+ // if (!sox.begin_SPI(LSM_CS)) {
+ // if (!sox.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) {
+ Serial.println("Failed to find LSM6DSOX chip");
+ while (1) {
+ delay(10);
+ }
+ }
+ if (!lis3mdl.begin_I2C()) { // hardware I2C mode, can pass in address & alt Wire
+ //if (!lis3mdl.begin_SPI(LIS3MDL_CS)) { // hardware SPI mode
+ //if (!lis3mdl.begin_SPI(LIS3MDL_CS, LIS3MDL_CLK, LIS3MDL_MISO, LIS3MDL_MOSI)) { // soft SPI
+ Serial.println("Failed to find LIS3MDL chip");
+ while (1) { delay(10); }
+ }
+ Serial.println("LIS3MDL Found!");
+
+ // sox.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
+ Serial.print("Accelerometer range set to: ");
+ switch (sox.getAccelRange()) {
+ case LSM6DS_ACCEL_RANGE_2_G:
+ Serial.println("+-2G");
+ break;
+ case LSM6DS_ACCEL_RANGE_4_G:
+ Serial.println("+-4G");
+ break;
+ case LSM6DS_ACCEL_RANGE_8_G:
+ Serial.println("+-8G");
+ break;
+ case LSM6DS_ACCEL_RANGE_16_G:
+ Serial.println("+-16G");
+ break;
+ }
+
+ // sox.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS );
+ Serial.print("Gyro range set to: ");
+ switch (sox.getGyroRange()) {
+ case LSM6DS_GYRO_RANGE_125_DPS:
+ Serial.println("125 degrees/s");
+ break;
+ case LSM6DS_GYRO_RANGE_250_DPS:
+ Serial.println("250 degrees/s");
+ break;
+ case LSM6DS_GYRO_RANGE_500_DPS:
+ Serial.println("500 degrees/s");
+ break;
+ case LSM6DS_GYRO_RANGE_1000_DPS:
+ Serial.println("1000 degrees/s");
+ break;
+ case LSM6DS_GYRO_RANGE_2000_DPS:
+ Serial.println("2000 degrees/s");
+ break;
+ case ISM330DHCX_GYRO_RANGE_4000_DPS:
+ break; // unsupported range for the DSOX
+ }
+
+ // sox.setAccelDataRate(LSM6DS_RATE_12_5_HZ);
+ Serial.print("Accelerometer data rate set to: ");
+ switch (sox.getAccelDataRate()) {
+ case LSM6DS_RATE_SHUTDOWN:
+ Serial.println("0 Hz");
+ break;
+ case LSM6DS_RATE_12_5_HZ:
+ Serial.println("12.5 Hz");
+ break;
+ case LSM6DS_RATE_26_HZ:
+ Serial.println("26 Hz");
+ break;
+ case LSM6DS_RATE_52_HZ:
+ Serial.println("52 Hz");
+ break;
+ case LSM6DS_RATE_104_HZ:
+ Serial.println("104 Hz");
+ break;
+ case LSM6DS_RATE_208_HZ:
+ Serial.println("208 Hz");
+ break;
+ case LSM6DS_RATE_416_HZ:
+ Serial.println("416 Hz");
+ break;
+ case LSM6DS_RATE_833_HZ:
+ Serial.println("833 Hz");
+ break;
+ case LSM6DS_RATE_1_66K_HZ:
+ Serial.println("1.66 KHz");
+ break;
+ case LSM6DS_RATE_3_33K_HZ:
+ Serial.println("3.33 KHz");
+ break;
+ case LSM6DS_RATE_6_66K_HZ:
+ Serial.println("6.66 KHz");
+ break;
+ }
+
+ // sox.setGyroDataRate(LSM6DS_RATE_12_5_HZ);
+ Serial.print("Gyro data rate set to: ");
+ switch (sox.getGyroDataRate()) {
+ case LSM6DS_RATE_SHUTDOWN:
+ Serial.println("0 Hz");
+ break;
+ case LSM6DS_RATE_12_5_HZ:
+ Serial.println("12.5 Hz");
+ break;
+ case LSM6DS_RATE_26_HZ:
+ Serial.println("26 Hz");
+ break;
+ case LSM6DS_RATE_52_HZ:
+ Serial.println("52 Hz");
+ break;
+ case LSM6DS_RATE_104_HZ:
+ Serial.println("104 Hz");
+ break;
+ case LSM6DS_RATE_208_HZ:
+ Serial.println("208 Hz");
+ break;
+ case LSM6DS_RATE_416_HZ:
+ Serial.println("416 Hz");
+ break;
+ case LSM6DS_RATE_833_HZ:
+ Serial.println("833 Hz");
+ break;
+ case LSM6DS_RATE_1_66K_HZ:
+ Serial.println("1.66 KHz");
+ break;
+ case LSM6DS_RATE_3_33K_HZ:
+ Serial.println("3.33 KHz");
+ break;
+ case LSM6DS_RATE_6_66K_HZ:
+ Serial.println("6.66 KHz");
+ break;
+ }
+
+ lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
+ Serial.print("Performance mode set to: ");
+ switch (lis3mdl.getPerformanceMode()) {
+ case LIS3MDL_LOWPOWERMODE: Serial.println("Low"); break;
+ case LIS3MDL_MEDIUMMODE: Serial.println("Medium"); break;
+ case LIS3MDL_HIGHMODE: Serial.println("High"); break;
+ case LIS3MDL_ULTRAHIGHMODE: Serial.println("Ultra-High"); break;
+ }
+
+ lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
+ Serial.print("Operation mode set to: ");
+ // Single shot mode will complete conversion and go into power down
+ switch (lis3mdl.getOperationMode()) {
+ case LIS3MDL_CONTINUOUSMODE: Serial.println("Continuous"); break;
+ case LIS3MDL_SINGLEMODE: Serial.println("Single mode"); break;
+ case LIS3MDL_POWERDOWNMODE: Serial.println("Power-down"); break;
+ }
+
+ lis3mdl.setDataRate(LIS3MDL_DATARATE_155_HZ);
+ // You can check the datarate by looking at the frequency of the DRDY pin
+ Serial.print("Data rate set to: ");
+ switch (lis3mdl.getDataRate()) {
+ case LIS3MDL_DATARATE_0_625_HZ: Serial.println("0.625 Hz"); break;
+ case LIS3MDL_DATARATE_1_25_HZ: Serial.println("1.25 Hz"); break;
+ case LIS3MDL_DATARATE_2_5_HZ: Serial.println("2.5 Hz"); break;
+ case LIS3MDL_DATARATE_5_HZ: Serial.println("5 Hz"); break;
+ case LIS3MDL_DATARATE_10_HZ: Serial.println("10 Hz"); break;
+ case LIS3MDL_DATARATE_20_HZ: Serial.println("20 Hz"); break;
+ case LIS3MDL_DATARATE_40_HZ: Serial.println("40 Hz"); break;
+ case LIS3MDL_DATARATE_80_HZ: Serial.println("80 Hz"); break;
+ case LIS3MDL_DATARATE_155_HZ: Serial.println("155 Hz"); break;
+ case LIS3MDL_DATARATE_300_HZ: Serial.println("300 Hz"); break;
+ case LIS3MDL_DATARATE_560_HZ: Serial.println("560 Hz"); break;
+ case LIS3MDL_DATARATE_1000_HZ: Serial.println("1000 Hz"); break;
+ }
+
+ lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS);
+ Serial.print("Range set to: ");
+ switch (lis3mdl.getRange()) {
+ case LIS3MDL_RANGE_4_GAUSS: Serial.println("+-4 gauss"); break;
+ case LIS3MDL_RANGE_8_GAUSS: Serial.println("+-8 gauss"); break;
+ case LIS3MDL_RANGE_12_GAUSS: Serial.println("+-12 gauss"); break;
+ case LIS3MDL_RANGE_16_GAUSS: Serial.println("+-16 gauss"); break;
+ }
+
+ lis3mdl.setIntThreshold(500);
+ lis3mdl.configInterrupt(false, false, true, // enable z axis
+ true, // polarity
+ false, // don't latch
+ true); // enabled!
+
+ prevTime = millis();
+ Serial.print("prevTime = ");
+ Serial.println(prevTime);
+}
+
+void loop() {
+ // /* Get a new normalized sensor event */
+ sensors_event_t accel;
+ sensors_event_t gyro;
+ sensors_event_t temp;
+ sox.getEvent(&accel, &gyro, &temp);
+
+// Serial.print("\t\tTemperature ");
+// Serial.print(temp.temperature);
+// Serial.println(" deg C");
+
+ /* Display the results (distance is measured in m) */
+ double currTime = millis();
+ double diffTime = (currTime - prevTime)/1000.0;
+ prevTime = currTime;
+
+ Serial.println();
+ Serial.println(currTime);
+ Serial.println(diffTime);
+
+ accX = accel.acceleration.x - gX;
+ accY = accel.acceleration.y - gY;
+ accZ = accel.acceleration.z - gZ;
+ velX = velX + accX*diffTime;
+ velY = velY + accY*diffTime;
+ velZ = velZ + accZ*diffTime;
+ posX = posX + velX*diffTime - 0.5*accX*diffTime*diffTime;
+ posY = posY + velY*diffTime - 0.5*accY*diffTime*diffTime;
+ posZ = posZ + velZ*diffTime - 0.5*accZ*diffTime*diffTime;
+
+ Serial.print(" \tX: ");
+ Serial.print(posX);
+ Serial.print(" \tY: ");
+ Serial.print(posY);
+ Serial.print(" \tZ: ");
+ Serial.print(posZ);
+// Serial.println(" m/s^2 ");
+
+ /* Display the results (rotation is measured in rad/s) */
+// Serial.print("\t\tGyro X: ");
+// Serial.print(gyro.gyro.x);
+// Serial.print(" \tY: ");
+// Serial.print(gyro.gyro.y);
+// Serial.print(" \tZ: ");
+// Serial.print(gyro.gyro.z);
+// Serial.println(" radians/s ");
+// Serial.println();
+
+// delay(100);
+
+ lis3mdl.read(); // get X Y and Z data at once
+ // Then print out the raw data
+// Serial.print("\nX: "); Serial.print(lis3mdl.x);
+// Serial.print(" \tY: "); Serial.print(lis3mdl.y);
+// Serial.print(" \tZ: "); Serial.println(lis3mdl.z);
+
+ /* Or....get a new sensor event, normalized to uTesla */
+ sensors_event_t event;
+ lis3mdl.getEvent(&event);
+ /* Display the results (magnetic field is measured in uTesla) */
+// Serial.print("\tX: "); Serial.print(event.magnetic.x);
+// Serial.print(" \tY: "); Serial.print(event.magnetic.y);
+// Serial.print(" \tZ: "); Serial.print(event.magnetic.z);
+// Serial.println(" uTesla ");
+
+// delay(100);
+ Serial.println();
+}
--
GitLab