Homework answers / question archive /       I require a C++ Master code for my ESP32 that completes the above task

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

Computer Science

Share With

---

 

 

 

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);   

  }

}