/* Made by Jefferson Sandoval for the Final project of FabAcademy 2021.
 *  
 * This is the official code for my Fab-OmniBot.
 * 
 * Uploaded to my own board that uses an ATtiny3216 microcontroller, a 
 * nRF24 radio transceiver module (as receiver), a laser distance sensor 
 * and two TB6612FNG motor drivers.
 * 
 * Documentation:
 * http://fabacademy.org/2021/labs/kamplintfort/students/jefferson-sandoval/project/04_Robot/
 */

//Library for math functions
#include <math.h> 

//Libraries for Communication nRF24
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

RF24 radio(4, 5); //Create radio object with and CE & CSN pins
const byte address[6] = "00111"; //Receiver Communication address

//Define pins for Motor W1
const int pinPWMA = 9; //Speed
const int pinAIN1 = 11; //Direction
const int pinAIN2 = 10; //Direction

//Define pins for Motor W2
const int pinPWMB = 8; //Speed
const int pinBIN1 = 3; //Direction
const int pinBIN2 = 2; //Direction

//Define pins for Motor W3
const int pinPWMC = 1; //Speed
const int pinCIN1 = 12; //Direction
const int pinCIN2 = 13; //Direction

const double AngWheels[3] = {(30*PI/180), (150*PI/180), (270*PI/180)}; //Angle in rads of each 
//wheel with respect to Y-axis of the robot (they correspond to 300º, 60º
//and 180º)

void setup() {
  Serial.begin(9600); //Begin Serial communication at 9600 bauds

  radio.begin(); //Initialize radio object
  radio.setPALevel(RF24_PA_HIGH); //Set radio range
  radio.openReadingPipe(0, address); //Set address to transmitter
  radio.startListening(); //Set module as receiver

  //Set Pin Modes for the motors
  pinMode(pinPWMA, OUTPUT);
  pinMode(pinAIN1, OUTPUT);
  pinMode(pinAIN2, OUTPUT);

  pinMode(pinPWMB, OUTPUT);
  pinMode(pinBIN1, OUTPUT);
  pinMode(pinBIN2, OUTPUT);
  
  pinMode(pinPWMC, OUTPUT);
  pinMode(pinCIN1, OUTPUT);
  pinMode(pinCIN2, OUTPUT);
}

void loop() 
{
  int Values[3]; //Declare variable for data to receive

  if (radio.available()) {
    radio.read(&Values, sizeof(Values)); //Read array of Values from Transmitter
    
      if (Values[2]<=-20 || Values[2]>=20) //Detect if the Joystick_Left is activated
      { int speed = map(Values[2],-100,100,-125,125); //Read the percentage values and 
      //convert them to speed values for the motors

        //Give direction of rotation to each wheel, depending on the direction the we 
        //push the Joystick
        if (Values[2]>20) { 
          digitalWrite(pinAIN1, LOW);
          digitalWrite(pinAIN2, HIGH);
          analogWrite(pinPWMA, abs(speed));
          digitalWrite(pinBIN1, LOW);
          digitalWrite(pinBIN2, HIGH);
          analogWrite(pinPWMB, abs(speed));
          digitalWrite(pinCIN1, LOW);
          digitalWrite(pinCIN2, HIGH);
          analogWrite(pinPWMC, abs(speed));
          Serial.println("CCW rotation");
        }
        if (Values[2]<-20) { 
          digitalWrite(pinAIN1, HIGH);
          digitalWrite(pinAIN2, LOW);
          analogWrite(pinPWMA, abs(speed));
          digitalWrite(pinBIN1, HIGH);
          digitalWrite(pinBIN2, LOW);
          analogWrite(pinPWMB, abs(speed));
          digitalWrite(pinCIN1, HIGH);
          digitalWrite(pinCIN2, LOW);
          analogWrite(pinPWMC, abs(speed));
          Serial.println("CW rotation");
        }
        Serial.print("Speed: ");
        Serial.println(speed);
      } else if ((Values[0]<=-25 || Values[0]>=25) || (Values[1]<=-25 || Values[1]>=25))
        {
            //Calculate angle of movement of robot (and Read the percentaje values and 
            //convert them to speed values for the motors)
            double Vx = 0;
            if (Values[0]<=-25 || Values[0]>=25) {Vx=map(Values[0],-100,100,-230,230);}            
            double Vy = 0;
            if (Values[1]<=-25 || Values[1]>=25) {Vy=map(Values[1],-100,100,-230,230);}

            //Calculate speed for every wheel, using the formula holonomic drive for 
            //3W omnidirectional robots:
            double SpeedM1 = 0.58*Vx - 0.33*Vy;
            double SpeedM2 = -0.58*Vx - 0.33*Vy;
            double SpeedM3 = 0.67*Vy;
            
            //Give direction of rotation to each wheel, depending on the signal of 
            //their speed
            if (SpeedM1<0){ 
              digitalWrite(pinAIN1, HIGH);
              digitalWrite(pinAIN2, LOW);
              analogWrite(pinPWMA, abs(SpeedM1));
            }
            else if (SpeedM1>0){
              digitalWrite(pinAIN1, LOW);
              digitalWrite(pinAIN2, HIGH);
              analogWrite(pinPWMA, abs(SpeedM1));
            }
            
            if (SpeedM2<0){ 
              digitalWrite(pinBIN1,HIGH);
              digitalWrite(pinBIN2, LOW);
              analogWrite(pinPWMB, abs(SpeedM2));
            }
            else if (SpeedM2>0){ 
              digitalWrite(pinBIN1, LOW);
              digitalWrite(pinBIN2, HIGH);
              analogWrite(pinPWMB, abs(SpeedM2));
            }
            
            if (SpeedM3<0){ 
              digitalWrite(pinCIN1, HIGH);
              digitalWrite(pinCIN2, LOW);
              analogWrite(pinPWMC, abs(SpeedM3));}
            else if (SpeedM3>0){ 
              digitalWrite(pinCIN1, LOW);
              digitalWrite(pinCIN2, HIGH);
              analogWrite(pinPWMC, abs(SpeedM3));
            }

            //Print in Serial monitor to check

            Serial.print("ValueX: ");
            Serial.println(Values[1]);
            Serial.print("ValueY: ");
            Serial.println(Values[0]);

            if (Values[1]<-20) {Serial.println("Moving to the Left");}
            if (Values[1]>20) {Serial.println("Moving to the Right");}
            if (Values[0]<-20) {Serial.println("Moving Backward");}
            if (Values[0]>20) {Serial.println("Moving Forward");}
            Serial.print("M1: ");
            Serial.println(SpeedM1);
            Serial.print("M2: ");
            Serial.println(SpeedM2);
            Serial.print("M3: ");
            Serial.println(SpeedM3);
            Serial.println("");        
        } else 
        { //Deactivate motors
          analogWrite(pinPWMA, 0);
          digitalWrite(pinCIN1, LOW);
          digitalWrite(pinCIN2, LOW);
          analogWrite(pinPWMB, 0);
          digitalWrite(pinCIN1, LOW);
          digitalWrite(pinCIN2, LOW);
          analogWrite(pinPWMC, 0); 
          digitalWrite(pinCIN1, LOW);
          digitalWrite(pinCIN2, LOW);
        }           
  }
  delay(100);
}