//What is the starting angle of the arm

float j0_angle = 0;
float j1_angle = 0;
float j2_angle = 0;
float j3_angle = 0;
float j4_angle = 0;


//Define Slider
int l1 = 3;
int l2 = 4;
int l3 = 5;
int l4 = 6;
int l5 = 7;
//Define Rotative
float r1 = A0;
float r2 = A1;
float r3 = A2;
float r4 = A3;
float r5 = A4;

void setup() {
  Serial.begin(115200);  // activate Serial Communication
  //Serial.begin(9600);
  //Define DIGITAL
  /*The Sliders will be consider Digital components. 
  This way their funtion will be  just to turn on or off of each arm */
  pinMode(3, INPUT);
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  pinMode(6, INPUT);
  pinMode(7, INPUT);

  //Define Analog
  /*In this case the rotatives will have the function of moving the arm, so they will be analog */
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);

}

void loop() {
  
  int valorl1 = digitalRead(l1);
  int valorl2 = digitalRead(l2);
  int valorl3 = digitalRead(l3);
  int valorl4 = digitalRead(l4);
   int valorl5 = digitalRead(l5);
  // Serial.println(valorl1); //This is in case you want to know what value is making.

  /*For each arm, we set up  an slider an a rotative. If the slider is ON (1), 
the rotative will have current to function. If it is not that case, meaning 
the slider is OFF(any other number but 1), then it does not work, it just print in the serial monitor "axis not activated"  */

 /*Due to each arm can rotate different degrees, we have used the function map. With this
 function we stablish that the lowest degree of the arm will be also for the rotative potenciometer. 
 And for the higher degree of the arm will be also the higher one for the rotative potenciometer*/

  if (valorl1 == 1) {

    Serial.println(valorl1);
    float valorr1 = analogRead(r1);
    valorr1 = map(valorr1, 0, 1023, 0, 360);
    Serial.println(valorr1);
    j0_angle = valorr1;
    Serial.println("J0_" + String(j0_angle, 3));  // Send Angle data. Format is "A_123.456"
    Serial.flush();                               // wait for messages to be sent
    delay(200);                                   // wait for 5 seconds

  } else {
    Serial.println("axis no activated");
  }
  if (valorl2 == 1) {

    Serial.println(valorl2);
    float valorr2 = analogRead(r2);
    valorr2 = map(valorr2, 0, 1023, -120, 120);
    Serial.println(valorr2);
    j1_angle = valorr2;
    Serial.println("J1_" + String(j1_angle, 3));  // Send Angle data. Format is "A_123.456"
    Serial.flush();                               // wait for messages to be sent
    delay(200);                                   // wait for 5 seconds

  } else {
    Serial.println("axis no activated");
  }

  if (valorl3 == 1) {

    Serial.println(valorl3);
    float valorr3 = analogRead(r3);
    valorr3 = map(valorr3, 0, 1023, -120, 120);
    Serial.println(valorr3);
    j2_angle = valorr3;
    Serial.println("J2_" + String(j2_angle, 3));  // Send Angle data. Format is "A_123.456"
    Serial.flush();                               // wait for messages to be sent
    delay(200);                                   // wait for 5 seconds

  } else {
    Serial.println("axis no activated");
  }
  if (valorl4 == 1) {

    Serial.println(valorl4);
    float valorr4 = analogRead(r4);
    valorr4 = map(valorr4, 0, 1023, 0, 360);
    Serial.println(valorr4);
    j3_angle = valorr4;
    Serial.println("J3_" + String(j3_angle, 3));  // Send Angle data. Format is "A_123.456"
    Serial.flush();                               // wait for messages to be sent
    delay(200);                                   // wait for 5 seconds

  } else {
    Serial.println("axis no activated");
  }
  if (valorl5== 1) {

    Serial.println(valorl5);
    float valorr5 = analogRead(r5);
    valorr5 = map(valorr5, 0, 1023, -120, 120);
    Serial.println(valorr5);
    j4_angle = valorr5;
    Serial.println("J4_" + String(j4_angle, 3));  // Send Angle data. Format is "A_123.456"
    Serial.flush();                               // wait for messages to be sent
    delay(200);                                   // wait for 5 seconds

  } else {
    Serial.println("axis no activated");
  }
}