Rover Movement Code:

NEW AND IMPROVED CLUNKERS:

Motor Nano CODE

#include <SoftwareSerial.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_Sensor.h>

Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();

#define LSM9DS1_SCK A5
#define LSM9DS1_MISO 12
#define LSM9DS1_MOSI A4
#define LSM9DS1_XGCS 6
#define LSM9DS1_MCS 5

const int forward[4] = { 1, 0, 1, 0 };
const int backward[4] = { 0, 1, 0, 1 };
const int right[4] = { 1, 0, 0, 1 };
const int left[4] = { 0, 1, 1, 0 };
const int stop[4] = { 0, 0, 0, 0 };
const int brake[4] = { 1, 1, 1, 1 };

SoftwareSerial Serial1(A0, A1);
SoftwareSerial Serial2(A2, A3);

String comms = "";

const int main_pins[4] = { 9, 10, 11, 12 };
const int aux_pins[4] = { 5, 6, 7, 8 };
const int led_pins[4] = { 2, 3, 4, 13 };

void setup() {
  for (int i = 2; i != 14; i++) {
    pinMode(i, OUTPUT);
  }

  Serial.begin(9600);
  Serial1.begin(9600);
  Serial2.begin(9600);

  if (!lsm.begin())
  {
    Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
    while (1);
  }
  Serial.println("Found LSM9DS1 9DOF");

  setupSensor();
}

void loop() {
  read_serial();
  float imu_data[9];
  read_imu(imu_data);
  write_message(imu_data);
}

void basic_motion(const int pin_states[4]) {
  for (int i = 0; i != 4; i++) {
    digitalWrite(main_pins[i], pin_states[i]);
    digitalWrite(aux_pins[i], pin_states[i]);
    digitalWrite(led_pins[i], pin_states[i]);
  }
  delay(50);
}

void read_serial() {
  if (Serial1.available() > 0) {
    comms = "ESP-32";
    char r_packet = Serial1.read();
    processing(r_packet);
  } else if (Serial.available() > 0) {
    comms = "Master HC-05";
    write_message("Espressif down!");
    char r_packet = Serial.read();
    processing(r_packet);
  } else {
    processing('S');
    Serial.println("Malfunction. No transmitter");
  }
}

void write_message(const char* text) {
  if (comms == "ESP-32") {
    if (Serial2.available() > 0) {
      Serial2.write(text);
    } else {
      comms = "Master HC-05";
      write_message("Espressif down");
      Serial.println("Malfunction.Espressif down");
    }
  } else if (comms == "Master HC-05") {
    if (Serial.available() > 0) {
      Serial.write(text);
    } else {
      Serial.println("Malfunction. No receiver");
      processing('S');
    }
  } else {
    processing('S');
    Serial.println("Malfunction. No receiver");
  }
}

void processing(char command) {
  switch(command) {
    case 'F':
      basic_motion(forward);
      break;
    case 'B':
      basic_motion(backward);
      break;
    case 'R':
      basic_motion(right);
      break;
    case 'L':
      basic_motion(left);
      break;
    case 'K':
      basic_motion(brake);
      break;
    case 'S':
      basic_motion(stop);
      break;
    default:
      basic_motion(stop);
      break;
  }
}

void setupSensor() {
  lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G, lsm.LSM9DS1_ACCELDATARATE_10HZ);
  lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
  lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
}

void read_imu(float* msg) {
  lsm.read();

  sensors_event_t a, m, g, temp;

  lsm.getEvent(&a, &m, &g, &temp);

  msg[0] = a.acceleration.x;
  msg[1] = a.acceleration.y;
  msg[2] = a.acceleration.z;
  msg[3] = m.magnetic.x;
  msg[4] = m.magnetic.y;
  msg[5] = m.magnetic.z;
  msg[6] = g.gyro.x;
  msg[7] = g.gyro.y;
  msg[8] = g.gyro.z;
}

Motor Nano Code

#include <SoftwareSerial.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_Sensor.h>

Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();

#define LSM9DS1_SCK A5
#define LSM9DS1_MISO 12
#define LSM9DS1_MOSI A4
#define LSM9DS1_XGCS 6
#define LSM9DS1_MCS 5

const int forward[4] = { 1, 0, 1, 0 };
const int backward[4] = { 0, 1, 0, 1 };
const int right[4] = { 1, 0, 0, 1 };
const int left[4] = { 0, 1, 1, 0 };
const int stop[4] = { 0, 0, 0, 0 };
const int brake[4] = { 1, 1, 1, 1 };

SoftwareSerial Serial1 = SoftwareSerial(A0, A1);
SoftwareSerial Serial2 = SoftwareSerial(A2, A3);

String comms = "";

const int main_pins[4] = { 9, 10, 11, 12 };
const int aux_pins[4] = { 5, 6, 7, 8 };
const int led_pins[4] = { 2, 3, 4, 13 };

void setup() {
  for (int i = 2; i != 14; i++) {
    pinMode(i, OUTPUT);
  }
  if (!lsm.begin())
  {
    Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
    while (1);
  }
  Serial.println("Found LSM9DS1 9DOF");

  setupSensor();
}

void loop() {
  read_serial();
  write_message(read_imu());
}

void basic_motion(int pin_states[4]) {
  for (int i = 0; i != 4; i++) {
    digitalWrite(main_pins[i], pin_states[i]);
    digitalWrite(aux_pins[i], pin_states[i]);
    digitalWrite(led_pins[i], pin_states[i]);
  }

  delay(50);
}

void read_serial() {
  if (Serial1.available() > 0) {
    comms = "ESP-32";
    char r_packet = Serial1.read();
    processing(r_packet);
  } else if (Serial.available() > 0) {
    comms = "Master HC-05";
    write_message('Espressif down!');
    char r_packet = Serial.read();
    processing(r_packet);
  } else {
    processing('Stop');
    Serial.println("Malfunction. No transmitter");
  }
}

void write_message(char text) {
  if (comms = "ESP-32") {
    if (Serial2.available() > 0) {
      Serial2.write(text);
    } else {
      comms = "Master HC-05";
      write_message('Espressif down');
      Serial.println("Malfunction.Espressif down");
    }
  } else if (comms = "Master HC-05") {
    if (Serial.available() > 0) {
      Serial.write(text);
    } else {
      Serial.println("Malfunction. No receiver");
      processing('Stop');
    }
  } else {
    processing('Stop');
    Serial.println("Malfunction. No receiver");
  }
}

void processing(char command) {
  if (command == 'Forward') {
    basic_motion(forward);
  } else if (command == 'Backward') {
    basic_motion(backward);
  } else if (command == 'Right') {
    basic_motion(right);
  } else if (command == 'Left') {
    basic_motion(left);
  } else if (command == 'Brake') {
    basic_motion(brake);
  } else if (command == 'Stop') {
    basic_motion(stop);
  }
}

void setupSensor() {
  // 1.) Set the accelerometer range
  lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G, lsm.LSM9DS1_ACCELDATARATE_10HZ);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G, lsm.LSM9DS1_ACCELDATARATE_119HZ);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G, lsm.LSM9DS1_ACCELDATARATE_476HZ);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G, lsm.LSM9DS1_ACCELDATARATE_952HZ);

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

float* read_imu() {
  lsm.read();

  sensors_event_t a, m, g, temp;

  lsm.getEvent(&a, &m, &g, &temp);

  float msg[9] = {a.acceleration.x, a.acceleration.y, a.acceleration.z, m.magnetic.x, m.magnetic.y, m.magnetic.z, g.gyro.x, g.gyro.y, g.gyro.z };

  return msg;
}

OLD JANK:

#include <SoftwareSerial.h>

// Motor states
const int FORWARD[4] = {1, 0, 1, 0};
const int BACKWARD[4] = {0, 1, 0, 1};
const int RIGHT[4] = {1, 0, 0, 1};
const int LEFT[4] = {0, 1, 1, 0};
const int STOP[4] = {0, 0, 0, 0};
const int BRAKE[4] = {1, 1, 1, 1};

// Serial communication setup
SoftwareSerial espSerial(A0, A1);
SoftwareSerial hcSerial(A2, A3);

String commsSource = "";

// Pin assignments
const int MAIN_PINS[4] = {9, 10, 11, 12};
const int AUX_PINS[4] = {5, 6, 7, 8};
const int LED_PINS[4] = {2, 3, 4, 13};

void setup() {
  for (int pin = 2; pin <= 13; pin++) {
    pinMode(pin, OUTPUT);
  }
  Serial.begin(9600);
  espSerial.begin(9600);
  hcSerial.begin(9600);
}

void loop() {
  readSerialData();
}

void executeMotion(const int* pinStates) {
  for (int i = 0; i < 4; i++) {
    digitalWrite(MAIN_PINS[i], pinStates[i]);
    digitalWrite(AUX_PINS[i], pinStates[i]);
    digitalWrite(LED_PINS[i], pinStates[i]);
  }
  delay(50);
}

void readSerialData() {
  if (espSerial.available() > 0) {
    commsSource = "ESP-32";
    String packet = espSerial.readString();
    processCommand(packet);
  } else if (Serial.available() > 0) {
    commsSource = "Master HC-05";
    sendMessage("Espressif down!");
    String packet = Serial.readString();
    processCommand(packet);
  } else {
    processCommand("Stop");
    Serial.println("Malfunction. No transmitter");
  }
}

void sendMessage(const String& message) {
  if (commsSource == "ESP-32") {
    if (hcSerial.available() > 0) {
      hcSerial.write(message.c_str());
    } else {
      commsSource = "Master HC-05";
      sendMessage("Espressif down");
      Serial.println("Malfunction. Espressif down");
    }
  } else if (commsSource == "Master HC-05") {
    if (Serial.available() > 0) {
      Serial.write(message.c_str());
    } else {
      Serial.println("Malfunction. No receiver");
      processCommand("Stop");
    }
  } else {
    processCommand("Stop");
    Serial.println("Malfunction. No receiver");
  }
}

void processCommand(const String& command) {
  const int* state;

  if (command == "Forward") {
    state = FORWARD;
  } else if (command == "Backward") {
    state = BACKWARD;
  } else if (command == "Right") {
    state = RIGHT;
  } else if (command == "Left") {
    state = LEFT;
  } else if (command == "Brake") {
    state = BRAKE;
  } else {
    state = STOP; // Default to stop for unknown commands
  }

  executeMotion(state);
}

#include <SoftwareSerial.h>

const int forward[4] = { 1, 0, 1, 0 };
const int backward[4] = { 0, 1, 0, 1 };
const int right[4] = { 1, 0, 0, 1 };
const int left[4] = { 0, 1, 1, 0 };
const int stop[4] = { 0, 0, 0, 0 };
const int brake[4] = { 1, 1, 1, 1 };

SoftwareSerial Serial1 = SoftwareSerial(A0, A1);
SoftwareSerial Serial2 = SoftwareSerial(A2, A3);

String comms = "";

const int main_pins[4] = { 9, 10, 11, 12 };
const int aux_pins[4] = { 5, 6, 7, 8 };
const int led_pins[4] = { 2, 3, 4, 13 };

void setup() {
  for (int i = 2; i != 14; i++) {
    pinMode(i, OUTPUT);
  }
}

void loop() {
  read_serial();
  write_message(read_imu());
}

void basic_motion(int pin_states[4]) {
  for (int i = 0; i != 4; i++) {
    digitalWrite(main_pins[i], pin_states[i]);
    digitalWrite(aux_pins[i], pin_states[i]);
    digitalWrite(led_pins[i], pin_states[i]);
  }

  delay(50);
}

void read_serial() {
  if (Serial1.available() > 0) {
    comms = "ESP-32";
    char r_packet = Serial1.read();
    processing(r_packet);
  } else if (Serial.available() > 0) {
    comms = "Master HC-05";
    write_message('Espressif down!');
    char r_packet = Serial.read();
    processing(r_packet);
  } else {
    processing('Stop');
    Serial.println("Malfunction. No transmitter");
  }
}

void write_message(char text) {
  if (comms = "ESP-32") {
    if (Serial2.available() > 0) {
      Serial2.write(text);
    } else {
      comms = "Master HC-05";
      write_message('Espressif down');
      Serial.println("Malfunction.Espressif down");
    }
  } else if (comms = "Master HC-05") {
    if (Serial.available() > 0) {
      Serial.write(text);
    } else {
      Serial.println("Malfunction. No receiver");
      processing('Stop');
    }
  } else {
    processing('Stop');
    Serial.println("Malfunction. No receiver");
  }
}

void processing(char command) {
  if (command == 'Forward') {
    basic_motion(forward);
  } else if (command == 'Backward') {
    basic_motion(backward);
  } else if (command == 'Right') {
    basic_motion(right);
  } else if (command == 'Left') {
    basic_motion(left);
  } else if (command == 'Brake') {
    basic_motion(brake);
  } else if (command == 'Stop') {
    basic_motion(stop);
  }
}

#include <SoftwareSerial.h>

/*const*/ int forward[4] = { 1, 0, 1, 0 };
/*const*/ int backward[4] = { 0, 1, 0, 1 };
/*const*/ int right[4] = { 1, 0, 0, 1 };
/*const*/ int left[4] = { 0, 1, 1, 0 };
/*const*/ int stop[4] = { 0, 0, 0, 0 };
/*const*/ int brake[4] = { 1, 1, 1, 1 };

SoftwareSerial Serial1 = SoftwareSerial(A0, A1);
SoftwareSerial Serial2 = SoftwareSerial(A2, A3);

String comms = "";

const int main_pins[4] = { 9, 10, 11, 12 };
const int aux_pins[4] = { 5, 6, 7, 8 };
const int led_pins[4] = { 2, 3, 4, 13 };

void setup() {
  for (int i = 2; i != 14; i++) {
    pinMode(i, OUTPUT);
  }
  Serial.begin(9600);
  Serial1.begin(9600);
  Serial2.begin(9600);
}

void loop() {
  read_serial();
  // `write_message` should be called only after processing a command
}

void basic_motion(int pin_states[4]) {
  for (int i = 0; i != 4; i++) {
    digitalWrite(main_pins[i], pin_states[i]);
    digitalWrite(aux_pins[i], pin_states[i]);
    digitalWrite(led_pins[i], pin_states[i]);
  }
  delay(50);
}

void read_serial() {
  if (Serial1.available() > 0) {
    comms = "ESP-32";
    String r_packet = Serial1.readString();
    processing(r_packet);
  } else if (Serial.available() > 0) {
    comms = "Master HC-05";
    write_message("Espressif down!");
    String r_packet = Serial.readString();
    processing(r_packet);
  } else {
    processing("Stop");
    Serial.println("Malfunction. No transmitter");
  }
}

void write_message(String text) {
  if (comms == "ESP-32") {
    if (Serial2.available() > 0) {
      Serial2.write(text.c_str());
    } else {
      comms = "Master HC-05";
      write_message("Espressif down");
      Serial.println("Malfunction. Espressif down");
    }
  } else if (comms == "Master HC-05") {
    if (Serial.available() > 0) {
      Serial.write(text.c_str());
    } else {
      Serial.println("Malfunction. No receiver");
      processing("Stop");
    }
  } else {
    processing("Stop");
    Serial.println("Malfunction. No receiver");
  }
}

void processing(String command) {
  int* state;
  if (command == "Forward") {
    state = forward;
  } else if (command == "Backward") {
    state = backward;
  } else if (command == "Right") {
    state = right;
  } else if (command == "Left") {
    state = left;
  } else if (command == "Brake") {
    state = brake;
  } else if (command == "Stop") {
    state = stop;
  } else {
    state = stop; // Default to stop if command is unknown
  }
  basic_motion(state);
}

#include <SoftwareSerial.h>

int forward={1,0,1,0};
int backward={0,1,0,1};
int right={1,0,0,1};
int left={0,1,1,0};
int stop={0,0,0,0};
int brake={1,1,1,1};

SoftwareSerial Serial1 = SoftwareSerial(rxPin, txPin);

void setup(){
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);  
}

void loop(){
  
}

void basic_motion(int main_pins[4], int pin_states[4], int aux_pins[4], int led_pins[4]) {
  for(i=0; i!=4; i++){
    digitalWrite(main_pins[i], pin_states[i]);
    digitalWrite(aux_pins[i], pin_states[i]);
    digitalWrite(led_pins[i], pin_states[i]);
  }

  delay(50);
}


#include <SoftwareSerial.h>

const int mainMotorPins[4][2] = {
  {2, 3},
  {4, 5},
  {6, 7},
  {8, 9}
};

const int auxMotorPins[4][2] = {
  {10, 11},
  {12, 13},
  {A0, A1},
  {A2, A3}
};

const int ledPins[4] = {A4, A5, A6, A7};

SoftwareSerial Serial1 = SoftwareSerial(rxPin, txPin);

void setup() {
  for (int i = 0; i < 4; i++) {
    pinMode(mainMotorPins[i][0], OUTPUT);
    pinMode(mainMotorPins[i][1], OUTPUT);
    pinMode(auxMotorPins[i][0], OUTPUT);
    pinMode(auxMotorPins[i][1], OUTPUT);
    pinMode(ledPins[i], OUTPUT);
  }
}

void basic_motion(int states[4]) {
  for (int i = 0; i < 4; i++) {
    int state = states[i];
    int mainPin1 = mainMotorPins[i][0];
    int mainPin2 = mainMotorPins[i][1];
    int auxPin1 = auxMotorPins[i][0];
    int auxPin2 = auxMotorPins[i][1];
    int ledPin = ledPins[i];
    
    if (state == 1) {
      digitalWrite(mainPin1, HIGH);
      digitalWrite(mainPin2, LOW);
      digitalWrite(auxPin1, HIGH);
      digitalWrite(auxPin2, LOW);
      digitalWrite(ledPin, HIGH);
    } else if (state == -1) {
      digitalWrite(mainPin1, LOW);
      digitalWrite(mainPin2, HIGH);
      digitalWrite(auxPin1, LOW);
      digitalWrite(auxPin2, HIGH);
      digitalWrite(ledPin, HIGH);
    } else {
      digitalWrite(mainPin1, LOW);
      digitalWrite(mainPin2, LOW);
      digitalWrite(auxPin1, LOW);
      digitalWrite(auxPin2, LOW);
      digitalWrite(ledPin, LOW);
    }
  }
  delay(50);
}

void loop() {
  int states[4] = {1, -1, 0, 1};
  basic_motion(states);
  delay(1000);
  
  int stopStates[4] = {0, 0, 0, 0};
  basic_motion(stopStates);
  delay(1000);
}