EEEE4008: Object Manipulator Auxiliary Platform: Arduino UNO (Slave) Code

// *** Object Manipulator 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 servos
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;

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

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

  //servo pin assignment
  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);

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

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