Hi there, I'm new to this and this is my first time posting on this forum.
I'm trying to control a robotic arm that has 5 servos using the Serial Monitor input. To control the robot, I would have to give an input such as c45, where c is to choose servo C and 45 would turn the servo to its 45 degree position. Inputs with angle less than 10 and more than 170 would not be taken as it is the servos' limit.
I tested the code with input a1 and got a proper output of the angle is too low. But when I entered e45 it would move servo A to angle of 45. Any following command that chooses a servo to move to a specified angle will result in moving servo A to that specified angle. How can I rectify this?
I have not completely connected the arduino board to the robot arm as it is not ready. I am using an Arduino Uno.
Thank you in advance for taking the time to look at this.
#include <Servo.h>
#include <string.h>
Servo servoA, servoB, servoC, servoD, servoE;
int pos = 0;
const unsigned int MAX_MANUAL_INPUT=12;
void setup()
{
Serial.begin(9600);
while (!Serial);
Serial.println("-------------------------");
Serial.println("Robo is loading....");
delay(1000);
Serial.println("Robo loaded succesfully");
Serial.println("-------------------------");
servoA.attach(3);
servoB.attach(5);
servoC.attach(6);
servoD.attach(8);
servoE.attach(9);
Serial.println("calibrating servo..."); //Calibrate servos
for (pos = 0; pos <= 180; pos += 1) //was told it was important for debugging
servoA.write(0);
delay(1000);
servoA.write(180);
delay(1000);
servoA.write(90);
delay(1000);
servoB.write(0);
delay(1000);
servoB.write(180);
delay(1000);
servoB.write(90);
delay(1000);
servoC.write(0);
delay(1000);
servoC.write(180);
delay(1000);
servoC.write(90);
delay(1000);
servoD.write(0);
delay(1000);
servoD.write(180);
delay(1000);
servoD.write(90);
delay(1000);
servoE.write(0);
delay(1000);
servoE.write(180);
delay(1000);
servoE.write(90);
delay(1000);
Serial.println("servo calibrated");
Serial.println("-------------------------");
Serial.println("Comand input online, write command to perform action");
Serial.println("-------------------------");
}
void loop()
{
for (pos = 0; pos <= 180; pos += 1) //was told it was important for debugging
static char servoinput[MAX_MANUAL_INPUT];
static char angleinput [4];
static unsigned int input_pos = 0;
while (Serial.available() > 0)
{
static char servoinput[MAX_MANUAL_INPUT];
static char angleinput [4];
static unsigned int input_pos = 0;
char inByte = Serial.read();
if ( inByte != '\n' && (input_pos < MAX_MANUAL_INPUT - 1) ) //store serial input
{
servoinput[input_pos] = inByte;
if ( input_pos>=1)
{
angleinput[input_pos-1] = servoinput[input_pos];
}
input_pos++;
}
else
{
int angle = atoi(angleinput); //change input to numbers
Serial.println(angleinput[0]); //Check variable contents
Serial.println(angleinput[1]);
Serial.println(angleinput[2]);
Serial.println(servoinput);
Serial.println(angle);
static char servochoose [1];
servochoose[0]= servoinput[0];
Serial.println(servochoose[0]);
servochoose[1]='\n';
Serial.println(servochoose[1]); //ensure servochoose doesnt have anything in [1]
if (angle < 10)
{
Serial.print(">");
Serial.println(angle);
Serial.println("cannost execute command, too low number");
servochoose[0] = '\n';
}
else if (angle >= 10 && angle < 170 && strcmp(servochoose, 'a') != 0)
{
Serial.print(">");
Serial.println(angle);
Serial.print("turning servo A to ");
Serial.print(angle);
Serial.println(" degrees");
servoA.write(angle);
delay(30);
servochoose[0] = '\n';
}
else if(angle >= 10 && angle < 170 && strcmp(servochoose, 'b') != 0)
{
Serial.print(">");
Serial.println(angle);
Serial.print("turning servo B to ");
Serial.print(angle);
Serial.println(" degrees");
servoB.write(angle);
delay(30);
servochoose[0] = '\n';
}
else if(angle >= 10 && angle < 170 && strcmp(servochoose, 'c') != 0)
{
Serial.print(">");
Serial.println(angle);
Serial.print("turning servo C to ");
Serial.print(angle);
Serial.println(" degrees");
servoC.write(angle);
delay(30);
servochoose[0] = '\n';
}
else if(angle >= 10 && angle < 170 && strcmp(servochoose, 'd') != 0)
{
Serial.print(">");
Serial.println(angle);
Serial.print("turning servo D to ");
Serial.print(angle);
Serial.println(" degrees");
servoD.write(angle);
delay(30);
servochoose[0] = '\n';
}
else if(angle >= 10 && angle < 170 && strcmp(servochoose, 'e') != 0)
{
Serial.print(">");
Serial.println(angle);
Serial.print("turning servo E to ");
Serial.print(angle);
Serial.println(" degrees");
servoE.write(angle);
delay(30);
servochoose[0] = '\n';
}
input_pos = 0; //resets all required values
if ( servoinput != '\n' && (input_pos < MAX_MANUAL_INPUT - 1) )
{
servoinput[input_pos] = '\n';
if ( input_pos>=1)
{
angleinput[input_pos-1] = '\n';
}
input_pos++;
}
input_pos = 0;
servochoose[0] = '\n';
}
}
}