Detecting Rover Motion Using the Accelerometer and Returning It to the Original Position

Produced by Kim Si-hoo

4 90 Degree PVC Sockets

2 T-sockets

4 PVC pipes

4 bearings

5 M6 nuts

1 M6 bolt

1 Arduino Uno

Plenty of jumper lines

1 L298N motor driver

4 TT geared motors

1 MPU6050 accelerometer

1 Li-ion battery (7.4V)

Plenty of cable ties

The parts needed here are nuts, bolts, bearings, and PVC pipes.

First, use 4 bearings, 5 bolts, and 5 nuts to make the axis of rotation in the first picture.

Then, as shown in the second picture, use PVC pipes to make a suspension structure on one side.

Make two of them and connect the T-shaped pipes on the suspension to either side of the first shaft of rotation.

The necessary parts are Arduino Uno, L298N motor driver, lithium-ion battery (7.4V), MPU6050 accelerometer, jumper wires, and cable ties.

First, connect the circuits as shown in the schematic above.

Each component is then fastened to the rover with cable ties (the accelerometer must be fastened to the rotating shaft in the center of the rover).

#include <Arduino.h>
#include "I2Cdev.h"
#include "MPU6050.h"


#define motor_IN1 3
#define motor_IN2 4
#define motor_IN3 5
#define motor_IN4 6


// Accelerometer
MPU6050 accelgyro;


// About Kalman Filters
class KalmanFilter {
public:
    KalmanFilter() {
        // Initialize Variables
        x_hat = 0;
        P = 1;
        Q = 0.0001;
        R = 0.01;
    }


    float updateEstimate(float z) {
        // Forecast Updates
        x_hat_minus = x_hat; // Filtering Signal Prediction = Advance Forecast
        P_minus = P + Q; // Quadratic Prediction Error = Forecast Error + Process Noise

        // Measurement Updates
        K = P_minus / (P_minus + R); // Kalman sensor final value = 2nd prediction error / (2nd prediction error + measurement noise)
        x_hat = x_hat_minus + K * (z - x_hat_minus); // Filtering signal prediction = pre-predicted value + Kalman sensor final value * (z-axis - pre-forecast)
        P = (1 - K) * P_minus; // Prediction error = (1 - Kalman sensor final value) * 2nd prediction error
        return x_hat; // Return filtered values


    }


private:
    float x_hat;    // Filtering Signal Prediction
    float P;        // Forecast error
    float Q;        // Process Noise 
    float R;        // Measurement Noise
    float x_hat_minus; // Advance Prediction
    float P_minus;  // Secondary Prediction Error
    float K;        // Kalman Sensor Final Value
};


//  Array to store accelerometer values
const int numSamples = 10;
int16_t axSamples[numSamples];
int16_t aySamples[numSamples];
int16_t azSamples[numSamples];


// Array to store gyro sensor values
int16_t gxSamples[numSamples];
int16_t gySamples[numSamples];
int16_t gzSamples[numSamples];


// Moving Average Value Variable
int16_t axAvg, ayAvg, azAvg;
int16_t gxAvg, gyAvg, gzAvg;


void setup() {
    Serial.begin(9600);
    Wire.begin();
    accelgyro.initialize();
    pinMode(motor_IN1, OUTPUT);
    pinMode(motor_IN2, OUTPUT);
    pinMode(motor_IN3, OUTPUT);
    pinMode(motor_IN4, OUTPUT);
}


void loop() {
    // Read all accelerometer values and store them in a sample array
    for (int i = 0; i < numSamples; i++) {
        int16_t ax, ay, az;
        accelgyro.getAcceleration(&ax, &ay, &az);
        axSamples[i] = ax;
        aySamples[i] = ay;
        azSamples[i] = az;


        int16_t gx, gy, gz;
        accelgyro.getRotation(&gx, &gy, &gz);
        gxSamples[i] = gx;
        gySamples[i] = gy;
        gzSamples[i] = gz;


        delay(1); // Adjust the sampling interval if necessary
        
    }


    // Use a Kalman filter to filter the acceleration values of each axis
    float filteredAx = applyKalmanFilter(axSamples, numSamples);
    float filteredAy = applyKalmanFilter(aySamples, numSamples);
    float filteredAz = applyKalmanFilter(azSamples, numSamples);
    
    // Use a Kalman filter to filter the gyro values on each axis
    float filteredGx = applyKalmanFilter(gxSamples, numSamples);
    float filteredGy = applyKalmanFilter(gySamples, numSamples);
    float filteredGz = applyKalmanFilter(gzSamples, numSamples);
    
    // Output filtered acceleration values to the serial monitor (output on a single line)
    Serial.print("Filtered Ax: ");
    Serial.print(filteredAx);
    Serial.print("   Filtered Ay: ");
    Serial.print(filteredAy);
    Serial.print("   Filtered Az: ");
    Serial.print(filteredAz);


    // Output filtered gyro values to the serial monitor (output on a single line)
    Serial.print("   Filtered Gx: ");
    Serial.print(filteredGx);
    Serial.print("   Filtered Gy: ");
    Serial.print(filteredGy);
    Serial.print("   Filtered Gz: ");
    Serial.println(filteredGz);
  int ax1 = filteredAx;
  int ay1 = filteredAy;
  int az1 = filteredAz;
  int speed = random(256);
  analogWrite(motor_IN1, speed);
  analogWrite(motor_IN2, 0);
  analogWrite(motor_IN3, speed);
  analogWrite(motor_IN4, 0);
  delay(3000);


  while (ax1 > filteredAx && ay1 > filteredAy && az1 > filteredAz) {
  analogWrite(motor_IN1, 0);
  analogWrite(motor_IN2, 255);
  analogWrite(motor_IN3, 0);
  analogWrite(motor_IN4, 255);
  }
}


float applyKalmanFilter(int16_t* samples, int numSamples) {
    KalmanFilter filter;
    float filteredValue = 0;


    // Apply each sample to a Kalman filter to calculate the filtered value
    for (int i = 0; i < numSamples; i++) {
        filteredValue = filter.updateEstimate(samples[i]);
    }
    return filteredValue;
}