My master:
// Libraries
#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_Sensor.h> // not used in this demo but required!
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <LiquidCrystal_I2C.h>
// --------------------------------------------------------------------
// Constants
// --------------------------------------------------------------------
// Motor signal channels
// ####################################################################
const int channel4 = 2; // steering (yellow cable)
const int channel2 = 3; // acceleration (purple cable)
const int s1 = 6; // acceleration // Connect this to S1 on the Sabertooth (purple cable)
const int s2 = 7; // steering // Connect this to S2 on the Sabertooth (yellow cable)
// ####################################################################
// Speed limits
// ####################################################################
const int defaultSpeed = 185; // Default motor speed (0-370)
const int upperLimit = 355;
const int lowerLimit = 15;
float deadZone = 10; // Adjust this value for changing the deadzone where the motors act with the default speed
float filteredCh1 = defaultSpeed; // Initial values (adjust as needed)
float filteredCh2 = defaultSpeed;
// ####################################################################
// Remapping Values
// ####################################################################
const int MAX_PPM=1000;
const int MIN_PPM=2000;
int remappedMaxPPM = 370;
int remappedMinPPM = 0;
// ####################################################################
// Control filter constants
// ####################################################################
const float alpha = 0.2; // Smoothing factor (adjust for setting the weight of the current readings) default:0.2
// ####################################################################
// IMU filter constants
// ##########################################################
const float alpha9DOF = 0.15; // Weight for accelerometer data
const float gamma9DOF = 0.2; // Weight for magnetometer data
const float beta9DOF = 0.02; // Weight for gyroscope data
// ##########################################################
// Error constants
// ##########################################################
const float ACC_ANGLE_ERROR_X = 0.01;
const float ACC_ANGLE_ERROR_Y = 0.01;
const float ACC_ERROR_X = 0.13;
const float ACC_ERROR_Y = 0.01;
// ##########################################################
// --------------------------------------------------------------------
// Initialization
// --------------------------------------------------------------------
// IMU initialization
// ##########################################################
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(); //0x1E, 0x6B
float filteredAccelX = 0.0;
float filteredAccelY = 0.0;
float fusedAngleX = 0.0;
float fusedAngleY = 0.0;
// ##########################################################
// Serial communication object through the digital pins
// ##########################################################
TinyGPSPlus gps; // The TinyGPS++ object
static const int gpsRXPin = 8, gpsTXPin = 9; // Communication pins of the gps module
static const int unoRXPin = 10, unoTXPin = 11; // Communication pins between mega and uno
SoftwareSerial ss(gpsRXPin, gpsTXPin);
SoftwareSerial unoSerial(unoRXPin, unoTXPin);
float message=0;
// ##########################################################
// I2C Initialization
// ##########################################################
#define SLAVE_ADDR 9
#define ANSWER_SIZE 26
#define MESSAGE_COUNT 3
// ##########################################################
// Time stamp
// ##########################################################
unsigned long interval = 0;
struct Values {
float readingCh1;
float readingCh2;
float remappedReadingCh1;
float remappedReadingCh2;
float filteredCh1;
float filteredCh2;
float fusedAngleX;
float fusedAngleY;
bool readingGps;
};
Values readingValues ;
Values remappedValues ;
Values filteredValues ;
Values readingImuValues ;
Values readingGps ;
void setupSensor()
{
// 1.) Set the accelerometer range
lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G);
// 2.) Set the magnetometer sensitivity
lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS);
// 3.) Setup the gyroscope
lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
//lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS);
//lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS);
}
void setup() {
Wire.begin();
Serial.begin(9600);
ss.begin(9600);
pinMode(channel4,INPUT);
pinMode(channel2,INPUT);
analogWrite(s1, defaultSpeed);
analogWrite(s2, defaultSpeed);
while (!Serial) {
delay(1); // will pause Zero, Leonardo, etc until serial console opens
}
Serial.println("LSM9DS1 data read demo");
// Try to initialise and warn if we couldn't detect the chip
if (!lsm.begin())
{
Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
while (1);
}
Serial.println("Found LSM9DS1 9DOF");
}
void loop() {
readingValues = readPPM(channel4, channel2);
remappedValues = remap(readingValues.readingCh1, readingValues.readingCh2);
filteredValues = lowPassFilter(remappedValues.remappedReadingCh1, remappedValues.remappedReadingCh2);
writePPM(filteredValues.filteredCh1, filteredValues.filteredCh2);
readingImuValues = readImu();
// Values readingGpsValues = readGps();
// I2C(filteredValues.filteredCh1);
// I2C(filteredValues.filteredCh2);
unsigned long currentMillis = millis();
static unsigned long previousMillis = 0;
unsigned long interval = currentMillis - previousMillis;
// Serial.println(interval);
if (interval > 15000) {
for (float i = 0; i < MESSAGE_COUNT; i++) {
prepareMessage(i);
}
previousMillis = currentMillis; // Update the last time the action was performed
}
// delay(3000);
}
Values readPPM(int channelInput1, int channelInput2) {
int ch1Reading = pulseIn(channelInput1, HIGH);
int ch2Reading = pulseIn(channelInput2, HIGH);
Values result;
result.readingCh1 = ch1Reading;
result.readingCh2 = ch2Reading;
return result;
}
Values remap(int channelInput1Reading, int channelInput2Reading ) {
int ch1RemappedReading = map(channelInput1Reading, MAX_PPM, MIN_PPM, remappedMaxPPM, remappedMinPPM);
int ch2RemappedReading = map(channelInput2Reading, MAX_PPM, MIN_PPM, remappedMaxPPM, remappedMinPPM);
Values result;
result.remappedReadingCh1 = ch1RemappedReading;
result.remappedReadingCh2 = ch2RemappedReading;
return result;
}
Values lowPassFilter(int remappedCh1Input, int remappedCh2Input) {
filteredCh1 = alpha * remappedCh1Input + (1 - alpha) * filteredCh1;
filteredCh2 = alpha * remappedCh2Input + (1 - alpha) * filteredCh2;
// Upper-lower limits
if (filteredCh1 > upperLimit) {
filteredCh1 = upperLimit;
} else if (filteredCh1 < lowerLimit) {
filteredCh1 = lowerLimit;
}
if (filteredCh2 > upperLimit) {
filteredCh2 = upperLimit;
} else if (filteredCh2 <= lowerLimit) {
filteredCh2 = lowerLimit;
}
// Deadzone
if (abs(filteredCh1 - defaultSpeed) < deadZone) {
filteredCh1 = defaultSpeed;
}
if (abs(filteredCh2 - defaultSpeed) < deadZone) {
filteredCh2 = defaultSpeed;
}
Values result;
result.filteredCh1 = filteredCh1;
result.filteredCh2 = filteredCh2;
return result;
}
void writePPM(int filteredCh1Input, int filteredCh2Input) {
analogWrite(s1, filteredCh2Input);
analogWrite(s2, filteredCh1Input);
}
Values readImu() {
lsm.read(); /* ask it to read in the data */
/* Get a new sensor event */
sensors_event_t a, m, g, temp;
lsm.getEvent(&a, &m, &g, &temp);
// Calculate pitch angle from accelerometer data
filteredAccelY = alpha9DOF * a.acceleration.y + (1 - alpha9DOF) * filteredAccelY;
filteredAccelX = alpha9DOF * a.acceleration.x + (1 - alpha9DOF) * filteredAccelX;
float accelAngleX = (atan((filteredAccelY)/sqrt(pow((filteredAccelX + ACC_ERROR_X),2) + pow((a.acceleration.z),2)))*RAD_TO_DEG) - ACC_ANGLE_ERROR_X;
float accelAngleY = (atan(-1*(filteredAccelX)/sqrt(pow((filteredAccelY),2) + pow((a.acceleration.z),2)))*RAD_TO_DEG) - ACC_ANGLE_ERROR_Y;
// Calculate gyroscope-based change in pitch angle
float gyroRateX = g.gyro.x * DEG_TO_RAD;
float gyroRateY = g.gyro.y * DEG_TO_RAD;
// Complementary filter to combine accelerometer and gyroscope data
fusedAngleX = alpha9DOF * (gyroRateX + fusedAngleX) + (1-alpha9DOF) * accelAngleX;
fusedAngleY = alpha9DOF * (gyroRateY + fusedAngleY) + (1-alpha9DOF) * accelAngleY;
// Calculate yaw angle from magnetometer data
float filteredYawAngleY = 0;
float filteredYawAngleX = 0;
filteredYawAngleY = gamma9DOF * m.magnetic.y + (1 - gamma9DOF) * filteredYawAngleY;
filteredYawAngleX = gamma9DOF * m.magnetic.x + (1 - gamma9DOF) * filteredYawAngleX;
float yawAngle = atan2(filteredYawAngleY, filteredYawAngleX) * RAD_TO_DEG;
// Integrate accelerometer data to get velocity (simplified, assuming constant acceleration)
static float velocity = 0.0;
velocity += a.acceleration.x * 0.02; // 0.02 is the time between loops in seconds
Values result;
result.fusedAngleX= fusedAngleX;
result.fusedAngleY= fusedAngleY;
// Serial.println("x=");
// Serial.println(fusedAngleX);
// Serial.println("y=");
// Serial.println(fusedAngleY);
return result;
}
void prepareMessage(float messageID)
{
if(messageID==0){
message=readingImuValues.fusedAngleX;
// message=11;
}
else if(messageID==1){
message=readingImuValues.fusedAngleY;
// message=12;
}
else if(messageID==2){
message=remappedValues.remappedReadingCh2;
// message=13;
}
byte byteArray[sizeof(float)*MESSAGE_COUNT]; // Byte array to hold the converted float
// Convert message ID to byte array
memcpy(byteArray, &messageID, sizeof(messageID));
// Convert message data to byte array
memcpy(byteArray + sizeof(messageID), &message, sizeof(message));
sendWireData(byteArray, messageID, message);
Serial.print("Message sent => ID: ");
Serial.print(messageID);
Serial.print(" Data: ");
Serial.println(message);
}
void sendWireData(byte* byteArray, float messageID, float message){
Wire.beginTransmission(SLAVE_ADDR);
Wire.write(byteArray,sizeof(byteArray)); // Send the message to the slave
int result = Wire.endTransmission();
if (result != 0) {
Serial.print("Error in I2C transmission. Error code: ");
Serial.println(result);
} else {
Serial.print("\nMessage sent with ID: ");
Serial.print(messageID);
Serial.print(" and data: ");
Serial.print(message);
}
}
My slave:
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <WiFiNINA.h>
#include <SoftwareSerial.h>
char ssid[] = "Ey, das tut W-LAN";
char pass[] = "27aa25!82T3006479e6";
String Apikey = "DRFURNGZGF3PDKBJ";
String server = "api.thingspeak.com"; // Only specify the domain name
WiFiClient client;
String field="";
float messageData = 0.0f;
float messageData1 = 0.0f;
float messageData2 = 0.0f;
float messageData3 = 0.0f;
float messageID = 0.0f;
static unsigned long lastUpdateTime = 0;
unsigned long currentTime = 0;
int status = WL_IDLE_STATUS; // Declare wifi status variable
bool uploadingToServer = false;
static const int RXPin = 12, TXPin = 13;
SoftwareSerial megaSerial(RXPin, TXPin); // RX, TX
// Addresses for I2C communication
#define SLAVE_ADDR 9
#define MESSAGE_COUNT 3
// Received message initialization
byte receivedArray[sizeof(float) * MESSAGE_COUNT];
float receivedMessage[MESSAGE_COUNT];
float receivedValue;
void setup() {
Serial.begin(9600);
connectToWiFi();
Wire.begin(SLAVE_ADDR);
Wire.onReceive(receiveEvent);
Wire.onRequest(requestEvent);
Serial.println("Slave setup complete");
delay(1000);
}
void connectToWiFi() {
// Attempt to connect to Wi-Fi
while (status != WL_CONNECTED) {
Serial.print("Attempting to connect to WiFi...");
status = WiFi.begin(ssid, pass);
delay(5000);
}
Serial.println("Connected to WiFi");
delay(1000);
}
void connectToThingSpeak() {
if (client.connect(server.c_str(), 80)) {
Serial.println("Connected to ThingSpeak server");
} else {
Serial.println("Connection to ThingSpeak server failed");
}
}
void receiveEvent() {
Serial.println("\nMessage received");
Wire.readBytes(receivedArray, sizeof(receivedArray));
// Extract message ID
memcpy(&messageID, receivedArray, sizeof(messageID));
// Extract message data
memcpy(&messageData, receivedArray + sizeof(messageID), sizeof(messageData));
// Display or process the received message
displayReceivedMessage(messageID, messageData);
// currentTime = millis();
// unsigned long interval = currentTime - lastUpdateTime;
// Serial.println(interval);
}
void requestEvent() {
Serial.print("Request received\n");
String acknowledgementMessage = "slave received the message";
Wire.write(acknowledgementMessage.c_str(), acknowledgementMessage.length());
}
void displayReceivedMessage(float messageID, float messageData) {
// Display or process the received message
// You can customize this part based on your requirements
if (messageID == 0) {
Serial.print("fused X: ");
// field = "&field1=";
messageData1=messageData;
} else if (messageID == 1) {
Serial.print("fused Y: ");
// field = "&field2=";
messageData2=messageData;
} else if (messageID == 2) {
Serial.print("Acceleration input: ");
// field = "&field3=";
messageData3=messageData;
}
Serial.print(messageData);
Serial.println(); // Add a newline for clarity
}
void loop() {
if(status != WL_CONNECTED) {
Serial.print("Wifi connection failed. Attempting to reconnect...");
connectToWiFi();
delay(5000);
} else {
Serial.println("Connected to WiFi\n");
// Create HTTP request
float trialData = 100;
String http_str = "GET /update?api_key=" + Apikey +
"&field1=" + String(messageData1) +
"&field2=" + String(messageData2) +
"&field3=" + String(messageData3) +
" HTTP/1.1\r\n";
http_str += "Host: " + server + "\r\n";
http_str += "Connection: close\r\n\r\n";
// Connect to the ThingSpeak server
connectToThingSpeak();
// Make the HTTP request
client.print(http_str);
client.stop();
delay(3000);
}
}```
