Hello, I'm working on a little project in which I try to control 5 continuous servomotors in real time using Flex Sensors and sending the sensors' values through an XBee module from one Arduino to the other.
Wireless communication between the Arduinos work fine, sending the data I need to make the servo motors move at the specified "angles". Problem is, as they are continuous servo motors, I set their "angles" using time delays for the servo motors' rotation; in other words, I write the rotation that I need to a servomotor, wait the required time to position it at the angle where I need it, and then make it stop. Doing this however, makes controlling the servomotors at the same time pretty impossible, since each servo must wait their specified time to reach its position, stopping the program and continuing once the servo has stopped and the program moves into the next servo.
Previously I tried using one delay for every servomotor, making them able to work at the same time with each other, but this gave me the nuisance that I can't give the servomotors more than two possible "angles".
Is there any way that I can make the rotation of each servo and its delay separate from each other and make the program continue its execution so I don't wait for each servo to finish its rotation?
Here is my code, hope that it helps:
#include <Servo.h>
char val1;
char val2;
char val3;
char val4;
char val5;
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;
Servo motor5;
char state1;
char state2;
char state3;
char state4;
char state5;
void setup() {
// initialize serial communication:
Serial.begin(9600);
motor1.attach(11);
motor2.attach(10);
motor3.attach(9);
motor4.attach(6);
motor5.attach(5);
motor1.write(90);
motor2.write(90);
motor3.write(90);
motor4.write(90);
motor5.write(90);
state1 = 'A';
state2 = 'F';
state3 = 'K';
state4 = 'P';
state5 = 'U';
}
void loop() {
// see if there's incoming serial data:
if (Serial.available() >= 5) {
char val1 = Serial.read();
char val2 = Serial.read();
char val3 = Serial.read();
char val4 = Serial.read();
char val5 = Serial.read();
Serial.print("Primer Servo ");
Serial.println(val1);
Serial.print("Segundo Servo ");
Serial.println(val2);
Serial.print("Tercer Servo ");
Serial.println(val3);
Serial.print("Cuarto Servo ");
Serial.println(val4);
Serial.print("Quinto Servo ");
Serial.println(val5);
//---------SERVO 1---------------
if (val1 == 'A') {
motor1.write(90);
delay(100);
}
if (val1 == 'B') {
if(state1 == 'A' || state1 == 'E')
{
motor1.write(180);
delay(300);
state1 = 'B';
}
}
if (val1 == 'C') {
if(state1=='A' || state1 == 'E')
{
motor1.write(180);
delay(600);
state1 = 'C';
}
if(state1 == 'B' || state1 == 'D')
{
motor1.write(180);
delay(300);
state1 = 'C';
}
}
if (val1 == 'D') {
motor1.write(0);
delay(300);
state1 = 'D';
}
if (val1 == 'E') {
if(state1 == 'C')
{
motor1.write(0);
delay(600);
state1 = 'E';
}
if(state1 =='D'|| state1 == 'B')
{
motor1.write(0);
delay(300);
state1 = 'E' ;
}
}
motor1.write(90);
//--------------------END OF MOTOR 1-------------------------------------------------
//---------SERVO 2---------------
if (val2 == 'F') {
motor2.write(90);
delay(100);
}
if (val2 == 'G') {
if(state2 == 'F' || state2 == 'J')
{
motor2.write(180);
delay(300);
state2 = 'G';
}
}
if (val2 == 'H') {
if(state2=='F' || state2 == 'J')
{
motor2.write(180);
delay(600);
state2 = 'H';
}
if(state2 == 'G' || state2 == 'I')
{
motor2.write(180);
delay(300);
state2 = 'H';
}
}
if (val2 == 'I') {
motor2.write(0);
delay(300);
state2 = 'I';
}
if (val2 == 'J') {
if(state2 == 'H')
{
motor2.write(0);
delay(600);
state2 = 'J';
}
if(state2 =='I'|| state2 == 'G')
{
motor2.write(0);
delay(300);
state2 = 'J' ;
}
}
motor2.write(90);
//--------------------END OF MOTOR 2-------------------------------------------------
//---------SERVO 3---------------
if (val3 == 'K') {
motor3.write(90);
delay(100);
}
if (val3 == 'L') {
if(state3 == 'K' || state3 == 'O')
{
motor3.write(180);
delay(300);
state3 = 'L';
}
}
if (val3 == 'M') {
if(state3=='K' || state3 == 'O')
{
motor3.write(180);
delay(600);
state3 = 'M';
}
if(state3 == 'L' || state3 == 'N')
{
motor3.write(180);
delay(300);
state3 = 'M';
}
}
if (val3 == 'N') {
motor3.write(0);
delay(300);
state3 = 'N';
}
if (val3 == 'O') {
if(state3 == 'M')
{
motor3.write(0);
delay(600);
state3 = 'O';
}
if(state3 =='N'|| state3 == 'L')
{
motor3.write(0);
delay(300);
state3 = 'O' ;
}
}
motor3.write(90);
//--------------------END OF MOTOR 3-------------------------------------------------
//---------SERVO 4---------------
if (val4 == 'P') {
motor4.write(90);
delay(100);
}
if (val4 == 'Q') {
if(state4 == 'P' || state4 == 'T')
{
motor4.write(180);
delay(300);
state4 = 'Q';
}
}
if (val4 == 'R') {
if(state4=='P' || state4 == 'T')
{
motor4.write(180);
delay(600);
state4 = 'R';
}
if(state4 == 'Q' || state4 == 'S')
{
motor4.write(180);
delay(300);
state4 = 'R';
}
}
if (val4 == 'S') {
motor4.write(0);
delay(300);
state4 = 'S';
}
if (val4 == 'T') {
if(state4 == 'R')
{
motor4.write(0);
delay(600);
state4 = 'T';
}
if(state4 =='S'|| state4 == 'Q')
{
motor4.write(0);
delay(300);
state4 = 'T' ;
}
}
motor4.write(90);
//--------------------END OF MOTOR 4-------------------------------------------------
//---------SERVO 5---------------
if (val5 == 'U') {
motor5.write(90);
delay(100);
}
if (val5 == 'V') {
if(state5 == 'U' || state5 == 'Y')
{
motor5.write(180);
delay(300);
state5 = 'V';
}
}
if (val5 == 'W') {
if(state5=='U' || state5 == 'Y')
{
motor5.write(180);
delay(600);
state5 = 'W';
}
if(state5 == 'V' || state5 == 'X')
{
motor5.write(180);
delay(300);
state5 = 'W';
}
}
if (val5 == 'X') {
motor5.write(0);
delay(300);
state5 = 'X';
}
if (val5 == 'Y') {
if(state5 == 'W')
{
motor5.write(0);
delay(600);
state5 = 'Y';
}
if(state5 =='X'|| state5 == 'V')
{
motor5.write(0);
delay(300);
state5 = 'Y' ;
}
}
motor5.write(90);
//--------------------END OF MOTOR 5-------------------------------------------------
}
}
Thank you for your help.