I Need To Complete The Master Code To Complete The Autonomous Reverse Parking

I Need To Complete The Master Code To Complete The Autonomous Reverse Parking. I Require A C++ Master Code For My ESP32 That Completes The Above Task. I Have Got Two Sensors On My Car Which Are The HC-SR04 And The MPU6500 Which Must Be Used To C Omplete The Task. We Must Use I2C Communication Between The Devices. The Arduino Nano Is Used To Power The Two Motors And The Steering And Receives It Commands From The ESP32(MASTER) The ESP32 Is The Master For The Arduino Nano And The Two Sensors And Must Be Used To Communicate With The Slave Sensors To Complete The Task. The Slave Code Is Completed But I Just Need To Finish Writing The Master Code. Ive Attached The Slave Code Also For Reference If You Need.
 

 

 

 

I require a C++ Master code for my ESP32 that completes the above task.

I have got two sensors on my car which are the HC-SR04 and the MPU6500 which must be used to complete the task. We must use I2C communication between the two devices. The slave code is completed but I just need to finish writing the master code. Ive attached the slave code also for reference if you need.

 

Here’s the Master code i’ve tried writing so far

 

#include <HCSR04.h>

#include <Wire.h>  // include Wire library

#include <MPU6050_tockn.h>

#define MPU6050_ADDR 0x68

 

#define I2C_Arduino_ADDR 0x04  // 4 in hexadecimal

 

MPU6050 mpu6050(Wire);

HCSR04 hc(5, 18);  //initialisation class HCSR04 (trig pin , echo pin)

 

long timer = 0;

 

void setup() {

 

  Serial.begin(9600);

  Wire.begin();  // join i2c bus (address optional for master)

  mpu6050.begin();

  mpu6050.calcGyroOffsets(true);

}

 

int stop_left = 0;

int stop_right = 0;

int left_back = -250;

int right_back = -250;

int left_fwd = 250;

int left_fwd_2 = 160;

int right_fwd_2 = 160;

int right_fwd = 250;

int steering = 20;

int steering_2 = 80;

int mid_steering = 52.7;

 

 

 

void loop() {

 

      mpu6050.update();

 

  if (millis() - timer > 1000) {

    Serial.print("angleX : ");

    Serial.print(mpu6050.getAngleX());

    Serial.print("\tangleY : ");

    Serial.print(mpu6050.getAngleY());

    Serial.print("\tangleZ : ");

    Serial.println(mpu6050.getAngleZ());

    timer = millis();

  }

 

 

  Serial.println(hc.dist());  // return curent distance in serial

  delay(60); 

 

 

  Wire.beginTransmission(I2C_Arduino_ADDR);

  Wire.write((byte)((left_fwd & 0x0000FF00) >> 8));      // first byte of y, containing bits 16 to 9

  Wire.write((byte)(left_fwd & 0x000000FF));             // second byte of y, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((right_fwd & 0x0000FF00) >> 8));     // first byte of x, containing bits 16 to 9

  Wire.write((byte)(right_fwd & 0x000000FF));            // second byte of x, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((mid_steering & 0x0000FF00) >> 8));  // first byte of y, containing bits 16 to 9

  Wire.write((byte)(mid_steering & 0x000000FF));         // second byte of y, containing the 8 LSB - bits 8 to 1

  delay(1000);

  Wire.endTransmission();  // stop transmitting

 

  Wire.beginTransmission(I2C_Arduino_ADDR);

  Wire.write((byte)((left_fwd_2 & 0x0000FF00) >> 8));    // first byte of y, containing bits 16 to 9

  Wire.write((byte)(left_fwd_2 & 0x000000FF));           // second byte of y, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((right_fwd & 0x0000FF00) >> 8));  // first byte of x, containing bits 16 to 9

  Wire.write((byte)(right_fwd & 0x000000FF));         // second byte of x, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((steering & 0x0000FF00) >> 8));    // first byte of y, containing bits 16 to 9

  Wire.write((byte)(steering & 0x000000FF));           // second byte of y, containing the 8 LSB - bits 8 to 1

  delay(1000);

  Wire.endTransmission();  // stop transmitting

                           //if(mpu6050.getAngleZ() >= 180 || mpu6050.getAngleZ() <= -180  )

                           // {

  Wire.beginTransmission(I2C_Arduino_ADDR);

  Wire.write((byte)((left_back & 0x0000FF00) >> 8));     // first byte of y, containing bits 16 to 9

  Wire.write((byte)(left_back & 0x000000FF));            // second byte of y, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((right_back & 0x0000FF00) >> 8));    // first byte of x, containing bits 16 to 9

  Wire.write((byte)(right_back & 0x000000FF));           // second byte of x, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((mid_steering & 0x0000FF00) >> 8));  // first byte of y, containing bits 16 to 9

  Wire.write((byte)(mid_steering & 0x000000FF));

  delay(1000);

  Wire.endTransmission();  // stop transmitting

                           // }

 

 

  Wire.beginTransmission(I2C_Arduino_ADDR);

  Wire.write((byte)((left_fwd & 0x0000FF00) >> 8));  // first byte of y, containing bits 16 to 9

  Wire.write((byte)(left_fwd & 0x000000FF));         // second byte of y, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((right_fwd_2 & 0x0000FF00) >> 8));      // first byte of x, containing bits 16 to 9

  Wire.write((byte)(right_fwd_2 & 0x000000FF));             // second byte of x, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((steering_2 & 0x0000FF00) >> 8));     // first byte of y, containing bits 16 to 9

  Wire.write((byte)(steering_2 & 0x000000FF));            // second byte of y, containing the 8 LSB - bits 8 to 1

  delay(1000);

  Wire.endTransmission();  // stop transmitting

 

  Wire.beginTransmission(I2C_Arduino_ADDR);

  Wire.write((byte)((left_back & 0x0000FF00) >> 8));     // first byte of y, containing bits 16 to 9

  Wire.write((byte)(left_back & 0x000000FF));            // second byte of y, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((right_back & 0x0000FF00) >> 8));    // first byte of x, containing bits 16 to 9

  Wire.write((byte)(right_back & 0x000000FF));           // second byte of x, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((mid_steering & 0x0000FF00) >> 8));  // first byte of y, containing bits 16 to 9

  Wire.write((byte)(mid_steering & 0x000000FF));

  delay(1000);

  Wire.endTransmission();  // stop transmitting

 

 

 

  //Wire.beginTransmission(I2C_Arduino_ADDR);

 

  // Wire.write((byte)((left_back & 0x0000FF00) >> 8));    // first byte of y, containing bits 16 to 9

  // Wire.write((byte)(left_back & 0x000000FF));           // second byte of y, containing the 8 LSB - bits 8 to 1

 

 

 

  // Wire.write((byte)((right_back & 0x0000FF00) >> 8));    // first byte of x, containing bits 16 to 9

  // Wire.write((byte)(right_back & 0x000000FF));           // second byte of x, containing the 8 LSB - bits 8 to 1

 

 

  // Wire.write((byte)((mid_steering & 0x0000FF00) >> 8));    // first byte of y, containing bits 16 to 9

  // Wire.write((byte)(mid_steering & 0x000000FF));           // second byte of y, containing the 8 LSB - bits 8 to 1

 

 

  //delay(1000);

  // Wire.endTransmission();// stop transmitting

 

 

  //if(hc.dist() <= 10)

 

  Wire.beginTransmission(I2C_Arduino_ADDR);

  Wire.write((byte)((stop_left & 0x0000FF00) >> 8));  // first byte of y, containing bits 16 to 9

  Wire.write((byte)(stop_left & 0x000000FF));         // second byte of y, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((stop_right & 0x0000FF00) >> 8));  // first byte of x, containing bits 16 to 9

  Wire.write((byte)(stop_right & 0x000000FF));         // second byte of x, containing the 8 LSB - bits 8 to 1

  Wire.write((byte)((mid_steering & 0x0000FF00) >> 8));  // first byte of y, containing bits 16 to 9

  Wire.write((byte)(mid_steering & 0x000000FF));         // second byte of y, containing the 8 LSB - bits 8 to 1

  delay(1000);

  Wire.endTransmission();  // stop transmitting

 

  // }

}

 

 

Here’s the Arduino Nano SLAVE CODE

 

// DO NOT modify or edit any of this code - for Session 2, the Arduino NANO code is provided for you

// the only exception is modify the pin numbers for the motors (if the motors do not spin the correct way)

 

#include <Servo.h>

#include <Encoder.h>

#include <Wire.h>

 

#define servoPin 4

 

Servo myservo;        // create servo object to control a servo

float steeringAngle;  // variable to store the servo position

 

// either change your wire connections to the two encoders or change the pin numbers in the code

// to change whether the count increments or decrements when turning in a particular direction

Encoder enc1(2, 11);  // create an encoder object for encoder 1

Encoder enc2(3, 12);  // create an encoder object for encoder 2

long oldPos_enc1 = -999; 

long oldPos_enc2 = -999;

long enc1_count;

long enc2_count;

 

#define enA 5   // EnableA command line - should be a PWM pin

#define enB 6   // EnableB command line - should be a PWM pin

 

// name the motor control pins - replace the ** with your pin number, digital pins do not need the 'D' prefix whereas analogue pins need the 'A' prefix

#define INa A0  // Channel A direction

#define INb A1  // Channel A direction

#define INc A2  // Channel B direction

#define INd A3  // Channel B direction

 

 

void setup() {

  Wire.begin(4);                // join I2C bus with address #4

  Wire.onReceive(receiveEvent); // register event

  Wire.onRequest(requestEvent); // register event

  myservo.attach(servoPin);  // attach our servo object to pin D4

  // the Servo library takes care of defining the PinMode declaration (libraries/Servo/src/avr/Servo.cpp line 240)

 

  // configure the motor control pins as outputs

  pinMode(INa, OUTPUT);

  pinMode(INb, OUTPUT);

  pinMode(INc, OUTPUT);

  pinMode(INd, OUTPUT);

  pinMode(enA, OUTPUT);

  pinMode(enB, OUTPUT);  

 

  // initialise serial communication

  Serial.begin(9600);

  Serial.println("Arduino Nano is Running"); // sanity check

}

 

 

void loop() {

  // update the two encoder values

  enc1_count = enc1.read();

  enc2_count = enc2.read();

  if (enc1_count != oldPos_enc1) {

    oldPos_enc1 = enc1_count;

  }

  if (enc2_count != oldPos_enc2) {

    oldPos_enc2 = enc2_count;

  }

}

 

// this function executes when data is requested from the master device

void requestEvent(void)

{

  Wire.write(enc1_count);

  Wire.write(enc2_count);

}

 

// this function executes whenever data is received from the master device

void receiveEvent(int howMany)

{

  if(howMany != 6)  // for 3 16-bit numbers, the data will be 6 bytes long - anything else is an error

  {

    emptyBuffer();

    return;

  }

 

  int16_t leftMotor_speed = 0;

  int16_t rightMotor_speed = 0;

  int16_t servoAngle = 0;

 

  uint8_t leftMotor_speed16_9 = Wire.read();  // receive bits 16 to 9 of x (one byte)

  uint8_t leftMotor_speed8_1 = Wire.read();   // receive bits 8 to 1 of x (one byte)

  uint8_t rightMotor_speed16_9 = Wire.read();   // receive bits 16 to 9 of y (one byte)

  uint8_t rightMotor_speed8_1 = Wire.read();   // receive bits 8 to 1 of y (one byte)

  uint8_t servoAngle16_9 = Wire.read();   // receive bits 16 to 9 of z (one byte)

  uint8_t servoAngle8_1 = Wire.read();   // receive bits 8 to 1 of z (one byte)

 

  leftMotor_speed = (leftMotor_speed16_9 << 8) | leftMotor_speed8_1; // combine the two bytes into a 16 bit number

  rightMotor_speed = (rightMotor_speed16_9 << 8) | rightMotor_speed8_1; // combine the two bytes into a 16 bit number

  servoAngle = (servoAngle16_9 << 8) | servoAngle8_1; // combine the two bytes into a 16 bit number

 

  // verify that the correct values are received via the serial monitor

  Serial.print("Left Motor: ");

  Serial.print(leftMotor_speed);

  Serial.print("\t");

  Serial.print("Right Motor: ");

  Serial.print(rightMotor_speed);

  Serial.print("\t");

  Serial.print("Servo: ");

  Serial.println(servoAngle);

 

  setSteeringAngle(servoAngle);

  runMotors(leftMotor_speed, rightMotor_speed);

}

 

 

// function to clear the I2C buffer

void emptyBuffer(void)

{

  Serial.println("Error: I2C Byte Size Mismatch"); // i.e. an incorrect number of bytes has been received

  while(Wire.available())

  {

    Wire.read();

  }

}

 

 

// function to set the steering angle

void setSteeringAngle(int servoAngle){

  servoAngle = constrain(servoAngle, 0, 180); // prevents the servo being set to 'impossible' angles

  myservo.write(servoAngle);

}

 

 

// function to run the motors

void runMotors(int leftMotor_speed, int rightMotor_speed){

  // limit the speed value between -255 and 255 as the PWM value can only be between 0 and 255 - the negative is handled below

  leftMotor_speed = constrain(leftMotor_speed, -255, 255);

  rightMotor_speed = constrain(rightMotor_speed, -255, 255);

 

  // vary the motor speeds - use the absolute value to remove the negative

  analogWrite(enA, abs(leftMotor_speed));

  analogWrite(enB, abs(rightMotor_speed));

 

  // if the speed value is negative, run the motor backwards

  if (leftMotor_speed < 0) {

    digitalWrite(INa, LOW);

    digitalWrite(INb, HIGH);

  }

  // else, run the motor forwards

  else {

    digitalWrite(INa, HIGH);

    digitalWrite(INb, LOW);   

  }

 

  // if the speed value is negative, run the motor backwards

  if (rightMotor_speed < 0) {

    digitalWrite(INc, LOW);

    digitalWrite(INd, HIGH);

  }

  // else run the motor forwards

  else {

    digitalWrite(INc, HIGH);

    digitalWrite(INd, LOW);   

  }

}