EEEE4008: Object Manipulator Auxiliary Robot Code with Omitted Switch Functionality

// *** Object Manipulator Tri-Omniwheel Robot with Variable Controller Translation & Rotation Inputs
// This code does not include the 'switch' button operation

//include RF  & stepper  motor libraries
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <AccelStepper.h>
#include <Servo.h>

Servo s1;
Servo s2;
Servo s3;
Servo s4;

//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;
int s1PosNext = 0;
int s2Pos = 0;
int s2PosNext = 0;
int s3Pos = 0;
int s3PosNext = 0;
int s4Pos = 0;
int s4PosNext = 0;

bool toggleDrive;

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

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;

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

  s1.attach(5);
  s2.attach(6);
  s3.attach(7);
  s4.attach(8);

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

void loop() {
  readData();
  delay(100);

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

  if (toggleDrive == true) {
    Serial.println("Drive Mode");

    readData();
    delay(100);
    if (data.toggleButton == 0){
      toggleDrive = !toggleDrive;
    }

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

  else {
    //object manipulator code
    Serial.println("Functionality Mode");

    s1Pos = s1.read();
    s2Pos = s2.read();
    s3Pos = s3.read();
    s4Pos = s4.read();

    // elbow - joint 1 - base
    if (data.jLValX >= 150) {
      //move CC
      if (s1Pos > 50){
        s1PosNext = s1Pos - 1;
        s1.write(s1PosNext);
      }
      else {
      }
    }

    if (data.jLValX < 100) {
      //move C
      if (s1Pos <= 170){
        s1PosNext = s1Pos + 1;
        s1.write(s1PosNext);
      }
      else {
      }
    }

    // elbow - joint 2 - middle
    if (data.jLValY >= 150) {
      //move CC
      if (s2Pos > 90){
        s2PosNext = s2Pos - 1;
        s2.write(s2PosNext);
      }
      else {
      }
    }

    if (data.jLValY < 100) {
      //move C
      if (s2Pos <= 170){
        s2PosNext = s2Pos + 1;
        s2.write(s2PosNext);
      }
      else {
      }
    }

    // wrist - joint 3 - top
    if (data.jRValX >= 150) {
      //move CC
      if (s3Pos > 10){
        s3PosNext = s3Pos - 1;
        s3.write(s3PosNext);
      }
      else {
      }
    }

    if (data.jRValX < 100) {
      //move C
      if (s3Pos <= 170){
        s3PosNext = s3Pos + 1;
        s3.write(s3PosNext);
      }
      else {
      }
    }

    // grabber
    if (data.jRValY >= 150) {
      //move CC
      if (s4Pos < 105){
        s4PosNext = s4Pos + 1;
        s4.write(s4PosNext);
      }
      else {
      }
    }

    if (data.jRValY < 100) {
      //move C
      if (s4Pos > 20){
        s4PosNext = s4Pos - 1;
        s4.write(s4PosNext);
      }
      else {
      }
    }
  }

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