EEEE4008: Projectile Shooter Auxiliary Platform: Arduino UNO (Slave) Code

// *** Projectile Shooter Tri-Omniwheel Robot with Variable Controller Translation & Rotation Inputs
// *** Including Switch & Route Following Code Segments

//include libraries
#include <SPI.h>
#include <Wire.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <AccelStepper.h>
#include <MultiStepper.h>
#include <Servo.h>

//initialise servo
Servo s1;

//define the stepper motor pin connections
const int step1Pin = 8;
const int dir1Pin = 7;
const int step2Pin = 6;
const int dir2Pin = 5;
const int step3Pin = 4;
const int dir3Pin = 3;

int s1Pos = 0;

bool toggleSpinners;
bool toggleFire;

//note these will both be empty and added to in code
String extracted_route_str[] = {"U","80","R","30","U","30","R","30","U","50","R","20","D","10"};
char extracted_route_char[] = {'U','80','R','30','U','30','R','30','U','50','R','20','D','10'};
int routeLength = sizeof(extracted_route_char);

int pathDistance = 0;

int motor1Count = 0;
int motor2Count = 0;
int motor3Count = 0;

bool followedRoute = false;


RF24 radio(9, 10);   // nRF24L01 (CE, CSN)

//configure the stepper motors
AccelStepper motor1(1,step1Pin, dir1Pin); //1 is a driver interface
AccelStepper motor2(1,step2Pin, dir2Pin);
AccelStepper motor3(1,step3Pin, dir3Pin);

MultiStepper motors;

long positions[3];

const byte address[6] = "00001"; //NRF24L01 address

//timer variables
unsigned long lastReceiveTime = 0;
unsigned long currentTime = 0;

//max size of this structure is 32 bytes - NRF24L01 buffer limit
struct Data_Package {
  byte jLValX;
  byte jLValY;
  byte jRValX;
  byte jRValY;
  byte toggleButton;
  byte switchButton;
  byte fireButton;
  byte grabButton;
};

Data_Package data; //create a variable with the above structure

//input matrix elements
float a = cos((240*3.14159)/180);
float b = cos((120*3.14159)/180);
float c = cos(0);
float d = sin((240*3.14159)/180);
float e = sin((120*3.14159)/180);
float f = sin(0);
float g = 1;
float h = 1;
float i = 1;

//calculate determinant
float det = (a*e*i)+(b*f*g)+(c*d*h)-(c*e*g)-(a*f*h)-(b*d*i);

//calculate inverse elements
float a_inverse = ((e*i)-(f*h))/det;
float b_inverse = ((h*c)-(i*b))/det;
float c_inverse = ((b*f)-(c*e))/det;
float d_inverse = ((g*f)-(d*i))/det;
float e_inverse = ((a*i)-(g*c))/det;
float f_inverse = ((d*c)-(a*f))/det;
float g_inverse = ((d*h)-(g*e))/det;
float h_inverse = ((g*b)-(a*h))/det;
float i_inverse = ((a*e)-(d*b))/det;

//default normalised controller variables
float jLValX_Normalised = 0;
float jLValY_Normalised = 0;
float jRValX_Normalised = 0;

//default motor speeds (set to OFF)
float motor1Speed = 0;
float motor2Speed = 0;
float motor3Speed = 0;

bool received = false;

void receiveEvent(long x) {
  while (Wire.available()) { // loop through all but the last
    extracted_route_str[] = x;
    extracted_route_char[] = x;
    received = true;

    //uncomment for I2C transmission back to Raspberry Pi
    //Wire.beginTransmission(4);
    //Wire.write(route);
    //Wire.endTransmission();
  }
}

void setup() {
  Serial.begin(9600); //serial monitor channel

  //I2C setup
  Wire.begin(0x8);
  Wire.onReceive(receiveEvent);

  s1.attach(5);

  //setup the NRF24L01 module
  pinMode(9, OUTPUT);
  radio.begin();
  radio.openReadingPipe(0, address);
  radio.setAutoAck(false);
  radio.setDataRate(RF24_250KBPS);
  radio.setPALevel(RF24_PA_LOW);
  radio.startListening(); //set the module as receiver
  resetData(); //refresh data

  toggleDrive = true;

  //configure motor max speed and acceleration
  motor1.setMaxSpeed(5000);
  motor2.setMaxSpeed(5000);
  motor3.setMaxSpeed(5000);
  motor1.setAcceleration(1000);
  motor2.setAcceleration(1000);
  motor3.setAcceleration(1000);
  motor1.setSpeed(0);
  motor2.setSpeed(0);
  motor3.setSpeed(0);

  motors.addStepper(motor1);
  motors.addStepper(motor2);
  motors.addStepper(motor3);
}

void loop() {
  if (!followedRoute && received){
    for (int i = 0; i < routeLength; i++){
      if (extracted_route_char[i] == 'U' || extracted_route_char[i] == 'D' || extracted_route_char[i] == 'L' || extracted_route_char[i] == 'R'){
        String tmp = extracted_route_str[i+1];
        pathDistance = tmp.toInt();
        char dir = extracted_route_char[i];
        switch (dir) {
          case 'U':
            translateUp(pathDistance);
            break;
          case 'D':
            translateDown(pathDistance);
            break;
          case 'L':
            translateLeft(pathDistance);
            break;
          case 'R':
            translateRight(pathDistance);
            break;
        }
      }
      else {
        //nothing
      }
    }
    followedRoute = true;
  }

  else if (received){

    readData();
    delay(100);

    if (data.toggleButton == 0){
      toggleSpinners = !toggleSpinners;
    }

    if (data.fireButton == 0){
      toggleFire = true;
    }

    if (toggleSpinners == true) {
      Serial.println("Spinners On");
      digitalWrite(2, HIGH);
    }
    else {
      digitalWrite(2, LOW);
    }

    if ((toggleFire == true) and (toggleSpinners == true)){
      Serial.println("Fire");
      delay(100);
      s1.write(90); //move to open position
      delay(200);
      s1.write(0); //move to closed position
      toggleFire = false;
    }
    else {
      toggleFire = false;
    }

    //insert drive code
    convertJoystickVal();
    //run the motors at the calculated speed
    motor1.runSpeed();
    motor2.runSpeed();
    motor3.runSpeed();

    /*Serial.print(data.jLValX);
    Serial.print(":");
    Serial.print(data.jLValY);
    Serial.print(":");
    Serial.print(data.jRValX);
    Serial.print(":");
    Serial.println(data.jRValY);*/
  }
}

void resetData() {
  //reset the values when there is no radio connection - set initial default values
  data.jLValX = 127; //map value from 0-1023 to 0-255 (byte)
  data.jLValY = 127;
  data.jRValX = 127;
  data.jRValY = 127;
  data.toggleButton = 1; //connected to GND so 0 when pressed
  data.switchButton = 1;
  data.fireButton = 1;
  data.grabButton = 1;
}

void readData() {
      // Check whether there is data to be received
  if (radio.available()) {
    radio.read(&data, sizeof(Data_Package)); // Read the whole data and store it into the 'data' structure
    lastReceiveTime = millis(); // At this moment we have received the data
  }
  // Check whether we keep receving data, or we have a connection between the two modules
  currentTime = millis();
  if ( currentTime - lastReceiveTime > 1000 ) { // If current time is more then 1 second since we have recived the last data, that means we have lost connection
    resetData(); // If connection is lost, reset the data. It prevents unwanted behavior, for example if a drone has a throttle up and we lose connection, it can keep flying unless we reset the values
  }
}

void convertJoystickVal(){
  //check whether there is data to be received
  if (radio.available()) {
    radio.read(&data, sizeof(Data_Package)); //read the whole data and store it into the 'data' structure
    lastReceiveTime = millis(); //at this moment the data is received
  }
  //check whether to keep receving data or if there is a connection between the two modules
  currentTime = millis();
  if ( currentTime - lastReceiveTime > 1000 ) { //if the current time is >1 second since the last data was recived then connection is lost
    resetData(); //if connection is lost then reset the data - prevents unwanted behavior
  }

  //left joystick x axis conversion
  jLValX_Normalised = map(data.jLValX, 255, 0, 127, -127); //map(minInput, maxInput, minOutput, maxOutput)
  if ((jLValX_Normalised < 5) && (jLValX_Normalised > -5)){
    jLValX_Normalised = 0;
  }

  //left joystick y axis conversion
  jLValY_Normalised = map(data.jLValY, 255, 0, -127, 127);
  if ((jLValY_Normalised < 5) && (jLValY_Normalised > -5)){
    jLValY_Normalised = 0;
  }

  //right joystick conversion
  jRValX_Normalised = map(data.jRValX, 0, 255, 127, -127);
  if ((jRValX_Normalised < 5) && (jRValX_Normalised > -5)){
    jRValX_Normalised = 0;
  }

  //calculate motor speeds
  motor1Speed = ((a_inverse*jLValX_Normalised)+(b_inverse*jLValY_Normalised)+(c_inverse*jRValX_Normalised))*5;
  motor2Speed = ((d_inverse*jLValX_Normalised)+(e_inverse*jLValY_Normalised)+(f_inverse*jRValX_Normalised))*5;
  motor3Speed = ((g_inverse*jLValX_Normalised)+(h_inverse*jLValY_Normalised)+(i_inverse*jRValX_Normalised))*5;

  /*
  Serial.print("M1: ");
  Serial.println(motor1Speed);
  Serial.print("M2: ");
  Serial.println(motor2Speed);
  Serial.print("M3: ");
  Serial.println(motor3Speed);
  */

  //run motors at the calculated speed
  motor1.setSpeed(motor1Speed);
  motor2.setSpeed(motor2Speed);
  motor3.setSpeed(motor3Speed);
}

void translateUp(int pathDistance){
  positions[0] = int(-1.1547*pathDistance);
  positions[1] = int(1.1547*pathDistance);
  positions[2] = 0;
  Serial.print("Up: ");
  Serial.print(positions[0]);
  Serial.print(", ");
  Serial.print(positions[1]);
  Serial.print(",");
  Serial.println(positions[2]);
  motor1.setCurrentPosition(0);
  motor2.setCurrentPosition(0);
  motor3.setCurrentPosition(0);
  motors.moveTo(positions);
}

void translateDown(int pathDistance){
  positions[0] = int(1.1547*pathDistance);
  positions[1] = int(-1.1547*pathDistance);
  positions[2] = 0;
  Serial.print("Down: ");
  Serial.print(positions[0]);
  Serial.print(", ");
  Serial.print(positions[1]);
  Serial.print(",");
  Serial.println(positions[2]);
  motor1.setCurrentPosition(0);
  motor2.setCurrentPosition(0);
  motor3.setCurrentPosition(0);
  motors.moveTo(positions);
}

void translateLeft(int pathDistance){
  positions[0] = int(pathDistance/2);
  positions[1] = int(pathDistance/2);
  positions[2] = -(pathDistance);
  Serial.print("Left: ");
  Serial.print(positions[0]);
  Serial.print(", ");
  Serial.print(positions[1]);
  Serial.print(",");
  Serial.println(positions[2]);
  motor1.setCurrentPosition(0);
  motor2.setCurrentPosition(0);
  motor3.setCurrentPosition(0);
  motors.moveTo(positions);
}

void translateRight(int pathDistance){
  positions[0] = int(-pathDistance/2);
  positions[1] = int(-pathDistance/2);
  positions[2] = pathDistance;
  Serial.print("Right: ");
  Serial.print(positions[0]);
  Serial.print(", ");
  Serial.print(positions[1]);
  Serial.print(",");
  Serial.println(positions[2]);
  motor1.setCurrentPosition(0);
  motor2.setCurrentPosition(0);
  motor3.setCurrentPosition(0);
  motors.moveTo(positions);
}