/***********************************************************
File name: AdeeptArmDrive.ino
Description:
After the robotic arm is powered on, you can control it by two methods:
1)By potentiometers on the Arm Drive board
You can rotate the 4 potentiometers on the Adeept Arm Drive board to make the arm turn left/right, move up/down and forward/backward, and grab objects.
2)By remote control
Pull the left stick to the left and the robotic arm will turn clockwise in a range of 0-180 degrees; push it to the right, the arm will turn counterclockwise in the same range.
Push the left stick forward and the arm will stretch forward; pull it backward, the arm will retract back.
Push forward the stick on the right, the arm will reach up; pull it backward, the arm will draw back.
Push the right stick to the left, the arm will pinch the grippers; pull it to the right, the grippers will open.
Website: www.adeept.com
E-mail: support@adeept.com
Author: Tom
Date: 2017/06/06
***********************************************************/
#include <SPI.h>
#include "RF24.h"
#include <Servo.h>

RF24 radio(8, 10);                // define the object to control NRF24L01
byte addresses[5] = "00006";      // define communication address which should correspond to remote control
int data[9]={512, 512, 0, 0, 1, 1, 512, 512, 512};  // define array used to save the communication data
int mode[1];

Servo servo1;//create servo object to control a servo
Servo servo2;//create servo object to control a servo
Servo servo3;//create servo object to control a servo
Servo servo4;//create servo object to control a servo


//The following can be modified according to your specific needs
int dataServo1 = 90; // Servo 1 rotation range(dataServo1=15~165)
int dataServo2 = 45; // Servo 2 rotation range(dataServo2=15~75)
int dataServo3 = 60; // Servo 3 rotation range(dataServo3=15~110)
int dataServo4 = 0;  // Servo 4 rotation range(dataServo4= 0~50)


float dirServo1Offset = 0;    // define a variable for deviation(degree) of the servo
float dirServo2Offset = 0;    // define a variable for deviation(degree) of the servo
float dirServo3Offset = 0;    // define a variable for deviation(degree) of the servo
float dirServo4Offset = 0;    // define a variable for deviation(degree) of the servo

int automatic = 0;

int val1;
int val2;
int val3;
int val4;
int motorPin = 7;  // define pin for motor

void setup()
{
    servo1.attach(9);//attachs the servo on pin 5 to servo object
    servo2.attach(6);//attachs the servo on pin 5 to servo object
    servo3.attach(5);//attachs the servo on pin 9 to servo object
    servo4.attach(3);//attachs the servo on pin 10 to servo object

  radio.begin();                      // initialize RF24
  radio.setRetries(0, 15);            // set retries times
  radio.setPALevel(RF24_PA_LOW);      // set power
  radio.openReadingPipe(1, addresses);// open delivery channel
  radio.startListening();             // start monitoring
  pinMode(motorPin, OUTPUT);          // set buzzerPin to output mode
}

void loop()
{
  servo1.write(dataServo1+dirServo1Offset);//goes to dataServo1 degrees
  servo2.write(dataServo2+dirServo2Offset);//goes to dataServo2 degrees
  servo3.write(dataServo3+dirServo3Offset);//goes to dataServo3 degrees
  servo4.write(dataServo4+dirServo4Offset);//goes to dataServo4 degrees
  receiveData();
  if(automatic == 0){ //Operating mode 1: Potentiometer control on the drive board
      mode[0]=0;
      val1 = map(analogRead(0), 0, 1023, 15, 165);
      val2 = map(analogRead(1), 0, 1023, 15, 75);
      val3 = map(analogRead(2), 0, 1023, 15, 110);
      val4 = map(analogRead(3), 0, 1000, 0, 50);
  }
  if(automatic == 1){//Operating mode 2: Remote control
      mode[0]=1;
      if(data[0]<300){val1++;if(val1>=165){val1=165;}}//Control the arm turn right(Servo1)
      if(data[0]>600){val1--;if(val1<=15){val1=15;}}  //Control the arm turn left(Servo1)

      if(data[1]>600){val3++;if(val3>=110){val3=110;}}//Control the movement of the arm forward(Servo3)
      if(data[1]<300){val3--;if(val3<=15){val3=15;}}  //Control the arm to move backwards(Servo3)

      if(data[8]>600){val4++;if(val4>=50){val4=50;}}  //The control the arm(Servo4)
      if(data[8]<300){val4--;if(val4<1){val4=0;}}     //The control the arm (Servo4)

      if(data[7]<300){val2++;if(val2>=75){val2=75;}}  //The control the arm moves downward(Servo2)
      if(data[7]>600){val2--;if(val2<=15){val2=15;}}  //The control the arm moves upward(Servo2)
  }
  if(dataServo1>val1){dataServo1--;  }
  if(dataServo1<val1){dataServo1++;  }
  if(dataServo1>165) {dataServo1=165;}
  if(dataServo1<15)  {dataServo1=15; }

  if(dataServo2>val2){dataServo2--; }
  if(dataServo2<val2){dataServo2++; }
  if(dataServo2>75)  {dataServo2=75;}
  if(dataServo2<15)  {dataServo2=15;}

  if(dataServo3>val3){dataServo3--; }
  if(dataServo3<val3){dataServo3++; }
  if(dataServo3>110){dataServo3=110;}
  if(dataServo3<15) {dataServo3=15; }

  if(dataServo4>val4){dataServo4--; }
  if(dataServo4<val4){dataServo4++; }
  if(dataServo4>50)  {dataServo4=50;}
  if(dataServo4<1)   {dataServo4=0; }


  delay(50);//wait for a second.33

}
void receiveData(){
   if ( radio.available()) {             // if receive the data
    while (radio.available()) {          // read all the data
      radio.read( data, sizeof(data) );  // read data
    }
    if(!data[3]){automatic = 0;}
    if(!data[4]){automatic = 1;}
    if(!data[5])// control the motor
      digitalWrite(motorPin, LOW);
    else
      digitalWrite(motorPin, HIGH);

   }
}









/***********************************************************
File name:  AdeeptRemoteControl.ino
Description:
After the robotic arm is powered on, you can control it by two methods:
1)By potentiometers on the Arm Drive board
2)By remote control
You can switch the status of the arm by the remote control.
The remote control can communicate with the robotic arm, the LED1 on the remote will blink.
Press the button B on the remote, so the robotic arm will be controlled in the first method, and the LED2
lights up when LED3 dims at the same time. Press button C to enter the second mode of control; meanwhile
the LED3 will light up when LED2 will go out.
Website: www.adeept.com
E-mail: support@adeept.com
Author: Tom
Date: 2017/06/06
***********************************************************/
#include <SPI.h>
#include "RF24.h"
RF24 radio(9, 10);            // define the object to control NRF24L01
byte addresses[5] = "00006";  // define communication address which should correspond to remote control
int data[9];                  // define array used to save the communication data
int mode = 0;
const int pot6Pin = 5;        // define R6
const int pot5Pin = 4;        // define R1
const int led1Pin = 6;        // define pin for LED1 which is close to NRF24L01 and used to indicate the state of NRF24L01
const int led2Pin = 7;        // define pin for LED2 which is the mode is displayed in the robotic arm remote control mode
const int led3Pin = 8;        // define pin for LED3 which is the mode is displayed in the robotic arm auto mode
const int APin = 2;           // define pin for D2
const int BPin = 3;           // define pin for D3
const int CPin = 4;           // define pin for D4
const int DPin = 5;           // define pin for D5

const int u1XPin = 0;      // define pin for direction X of joystick U1
const int u1YPin = 1;      // define pin for direction Y of joystick U1
const int u2XPin = 2;      // define pin for direction X of joystick U2
const int u2YPin = 3;      // define pin for direction Y of joystick U2

void setup() {

  radio.begin();                      // initialize RF24
  radio.setRetries(0, 15);            // set retries times
  radio.setPALevel(RF24_PA_LOW);      // set power
  radio.openWritingPipe(addresses);   // open delivery channel
  radio.stopListening();              // stop monitoring
  pinMode(led1Pin, OUTPUT);           // set led1Pin to output mode
  pinMode(led2Pin, OUTPUT);           // set led2Pin to output mode
  pinMode(led3Pin, OUTPUT);           // set led3Pin to output mode
  pinMode(APin, INPUT_PULLUP);        // set APin to output mode
  pinMode(BPin, INPUT_PULLUP);        // set BPin to output mode
  pinMode(CPin, INPUT_PULLUP);        // set CPin to output mode
  pinMode(DPin, INPUT_PULLUP);        // set DPin to output mode
  data[3] = 0;
  data[4] = 1;
}

void loop() {
  // put the values of rocker, switch and potentiometer into the array
  data[0] = analogRead(u1XPin);
  data[1] = analogRead(u1YPin);
  if(digitalRead(APin)==LOW){
    delay(100);
    data[2] = digitalRead(APin);
    }else{
      data[2] = HIGH;
      }
  if( digitalRead(BPin)==LOW){//Switch the working mode
    mode = 0;
    data[3] = 0;
    data[4] = 1;
  }
  if(digitalRead(CPin)==LOW){//Switch the working mode
    mode = 1;
    data[3] = 1;
    data[4] = 0;
  }
  data[5] = digitalRead(DPin);
  data[6] = analogRead(pot5Pin);
  data[7] = analogRead(u2YPin);
  data[8] = analogRead(u2XPin);
  // send array data. If the sending succeeds, open signal LED
  if (radio.write( data, sizeof(data) ))
  digitalWrite(led1Pin,HIGH);

  // delay for a period of time, then turn off the signal LED for next sending
  delay(2);
  digitalWrite(led1Pin,LOW);

  if(mode==0){//LED display status
    digitalWrite(led2Pin,HIGH);
    digitalWrite(led3Pin,LOW);
  }
  if(mode==1){//LED display status
      digitalWrite(led2Pin,LOW);
      digitalWrite(led3Pin,HIGH);
  }
}