/* Sweep

by BARRAGAN <http://barraganstudio.com>
This example code is in the public domain.

modified 8 Nov 2013

by Scott Fitzgerald
https://www.arduino.cc/en/Tutorial/LibraryExamples/Sweep
*/

#include <Servo.h>

Servo myservo; // create servo object to control a servo

// twelve servo objects can be created on most boards

int pos = 0; // variable to store the servo position

void setup() {
myservo.attach(10); // attaches the servo on pin 9 to the servo object
myservo.write(10);
}

void loop() {
for (pos = 10; pos <= 170; pos += 5) { // goes from 0 degrees to 180 degrees
// in steps of 1 degree
myservo.write(pos); // tell servo to go to position in variable
'pos'
delay(250); // waits 15 ms for the servo to reach the
position
}
for (pos = 10; pos >= 10; pos -= 5) { // goes from 180 degrees to 0 degrees
myservo.write(pos); // tell servo to go to position in variable
'pos'
delay(250); // waits 15 ms for the servo to reach the
position
}
}

#include <Servo.h>
Servo Garra;
Servo SM_6;
Servo SM_5;
Servo SM_4;
Servo SM_3;
Servo SM_2;
/////////////////////////////////////////////
const int Max_Garra = 90, Min_Garra = 10;
int anguloRobot[6] = {0, 0, 0, 0, 0, 0}; ///{Garra,SM5,SM4,SM3,SM2,SM1}
const int incremento = 10;
String mensaje = "000000", AngulosRobots = "0";
//////////////////////////////////////////
void setup() {
Serial.begin(9600);
 Garra.attach(11);
SM_2.attach(10);
SM_3.attach(9);
SM_4.attach(6);
SM_5.attach(5);
SM_6.attach(3);
}

void loop() {
float MovGarra = map(anguloRobot[0], 10, 87, 10, 90);
float MovSM_2 = map(anguloRobot[1], 10, 87, 10, 90);
float MovSM_3 = map(anguloRobot[2], 0, 180, 10, 110);
float MovSM_4 = map(anguloRobot[3], 0, 180, 10, 110);
float MovSM_5 = map(anguloRobot[4], 0, 180, 10, 110);
float MovSM_6 = map(anguloRobot[5], 0, 180, 10, 110);

Garra.write(MovGarra);
SM_2.write(MovSM_2);
SM_3.write(MovSM_3);
SM_4.write(MovSM_4);
SM_5.write(MovSM_5);
SM_6.write(MovSM_6);
}
void serialEvent() {
if (Serial.available())
{
mensaje = Serial.readString();
actualizarAngulo();
AngulosRobots = "";
for (int i = 0; i < 6; i++)
{
AngulosRobots = AngulosRobots + " / " + String(anguloRobot[i]) ;
}
Serial.println(AngulosRobots);
}
}
void actualizarAngulo()
{
for (int i = 0; i < 6; i++)
{
switch (mensaje[i])
{
case 'a':
anguloRobot[i] = anguloRobot[i] + incremento; // incrementar angulo
break;
case 'b': // disminuir angulo
anguloRobot[i] = anguloRobot[i] - incremento;
break;
default :
break;
}
}
}