Receiving data for multiple servos

Hi, I am very new to arduino and electronics, I have this issue with a project of mine.

I have a flash blob detection program that sends data to Processing which then passes to an Arduino mega 2560. The Arduino then gives out orders to 36 Futaba S3001 servos. The servos will need to point at specific degrees based on the data received, which is within 0-180.

For 18 of the servos, it just needs to swing back and forth, I just need to send it an on/off, but the other 18 servos I need to send 3-digit numbers to give them a specific angle to point at. Which means I need to send around 18x3 (3-digit number each) plus 18x1 (1-digit for on/off as 'T' or 'F') which is 72 characters each time.

I tried it out with 10 servos, but to prevent data drop, I had to give it a pretty large delay for it to pack up the characters before it does anything, so the feedback of the servos were really choppy. It even just stopped working after a while.

The servos have an external DC power source of 6V.

Would really appreciate any kind of help.

Here is the code

#include <Servo.h>

//_______________________________
//---- store data from processing
char inData[5];
char inChar = -1;
byte index = 0;
char temp[3];
//===============================

//_______________________________
//---------------------- cat data
const int catNum = 5;
int currCat;
int maxHandPos = 170;
int minHandPos = 10;
boolean realData;
//===============================

//_______________________________
//-------------------- servo data
Servo assServo[catNum];
int assServoPin[catNum];
int assPosition[catNum];
// - - - - - - - - - - - - - - - 
boolean handShakeBool[catNum];
Servo handServo[catNum];
int handServoPin[catNum];
int handPosition[catNum];
int handSpeed = 1;
//===============================

//_______________________________
//----------------------- testing
int ledPin[catNum];
//===============================


// FUNCTION ___________________________________________
//-----------------------------------------------------
void setup(){
 Serial.begin(57600);
 currCat = -1;
 realData = false;
 // - - - - - - - - - - - - - - - - - - - - -
 /*for(int i=0; i<catNum; i++){
   assServoPin[i] = i+13;
   assServo[i].attach(assServoPin[i]);
   assPosition[i] = 90;
   // - - - - - - - - - - - - - - - -
   handShakeBool[i] = false;
   handServoPin[i] = i+1;
   handServo[i].attach(handServoPin[i]);
   handPosition[i] = 0;
 }
 // - - - - - - - - - - - - - - - - - - - - -
 shakeHandFunc();*/
 for(int i=0; i<catNum; i++){
   ledPin[i] = i+3;
   pinMode(ledPin[i], OUTPUT);
   digitalWrite(ledPin[i], LOW);
 }
}

void loop(){
  readData();
  if(strlen(inData) == 5){
    if(inData[4] == 'T' || inData[4] == 'F'){
      // - - - - - - - - - - - - - - - - - - - - -
      switch(inData[0]){
        case 'A':
        currCat = 0;
        realData = true;
        break;
        // - - - - - - - - -
        case 'B':
        currCat = 1;
        realData = true;
        break;
        // - - - - - - - - -
        case 'C':
        currCat = 2;
        realData = true;
        break;
        // - - - - - - - - -
        case 'D':
        currCat = 3;
        realData = true;
        break;
        // - - - - - - - - -
        case 'E':
        currCat = 4;
        realData = true;
        break;
      }
      // - - - - - - - - - - - - - - - - - - - - -
      if(realData && currCat != -1){
        realData = false;
        assPosition[currCat] = setPos(inData);
        if(inData[4] == 'T'){
          handShakeBool[currCat] = true;
        }else{
          handShakeBool[currCat] = false;
        }
        
        assServo[currCat].write(assPosition[currCat]);
        currCat = -1;
      }
      // - - - - - - - - - - - - - - - - - - - - -
    }
  }
 clearChar(inData);
 delay(100);
}

void shakeHandFunc(){
  for(int i=0; i<catNum; i++){
    if(handShakeBool[i]){
      if(handPosition[i] > maxHandPos){
        handPosition[i]-=handSpeed;
      }
      if(handPosition[i] < minHandPos){
        handPosition[i]+=handSpeed;
      }
    }else{
      handPosition[i] = 90;
    }
    handServo[i].write(handPosition[i]);
  }
  delay(100);
}

void readData(){
  while(Serial.available() > 0)
  {
   if(index < 5)
   {
     inChar = Serial.read();
     inData[index] = inChar;
     index++;
     inData[index] = '\0';
   }
 }
}

int setPos(char* tempChar){
  for(int i=0; i<3; i++){
    temp[i] = tempChar[i+1];
  }
  return (atoi(temp));
  clearChar(temp);
}

void clearChar(char* charTemp){
  for(int i=0; i<strlen(charTemp); i++){
    charTemp[i] = 0;
  }
}

Why not encoding the angles in bytes: 0..180 fits perfectly in one byte, so iso 18x3 you need 18x1

the 18 on /off can be merged into 3 bytes (leaving free 6 bits for checksum or so)

that makes: 18 x1 + 3 = 21 bytes iso 72 each time

Set the serial speed to 115200 and it would double .

together this would decrease the communication approx a factor 6.

hope this helps,
Rob

putting this thought in code, not tested but just to get it:

#include <Servo.h>

int servocounter = 0;
// FUNCTION 
//-----------------------------------------------------
void setup()
{
 Serial.begin(115200);
 Serial.println("start");
}

void loop()
{
  if (Serial.Available())
  {
    uint8_t v = Serial.read();
    if (servoCounter<18)  // servo 0..17
    {
      servo[servoCounter].write(v);
      servoCounter++;
    } 
    else  // servo 18..35
    {
      // 6 bit in one byte
      for (int i = 0; i< 6; i++)
      {
        servo[servoCounter+i].write(bitRead(v,i));
      }
    servoCounter +=6;
    }
  }
  if (servoCounter >=36) servoCounter = 0;
}

You might need sending a byte with the value 0xFF/255 to indicate start of a new sequence (servoCounter = 0)

Thanks for the reply.

I'll try it out once I have time to set it up again, takes a bit of time.

So I'm guessing I would have to encode the data on the Processing side first, and then send it to Arduino?

If you have control of the way you will send the servo data, then you may be able to make the reading of the data easier.

Just tested it, works like a charm. Thanks!

Hi, I ran into another issue in the same project.

I got the first 16 (we changed it from 18 to 16) servos to work with no problem, however the other 16 servos are not working the way they should, they just kind of wobble a bit randomly and I can't figure what seems to be the problem.

They are supposed to sweep from 30 degrees to 160 degrees if I give them a "1" and just stop if I give them a "0", but either just shaking a bit at random times or just flat out not move at all.

Any suggestions are very much appreciated.

Here is the code I have so far.

#include <Servo.h>

//________________________________
//----------------------- cat data
const int catNum = 16;
int minHandPos = 30;
int maxHandPos = 160;
int shakeSpeed = 10;
//================================

//________________________________
//--------------------- servo data
Servo assServo[catNum];
Servo handServo[catNum];
int assServoPin[catNum] = {3, 6, 9, 12, 22, 26, 28, 32, 34, 38, 40, 44, 46, 50, 52, A0};
int handServoPin[catNum] = {4, 7, 10, 13, 23, 27, 29, 33, 35, 39, 41, 45, 47, 51, 53, A1};
boolean shakeBool[catNum];
int assPos[catNum];
int handPos[catNum];
int servoCount = 0;
//================================

// FUNCTION_________________________________________
//--------------------------------------------------
void setup(){
  Serial.begin(115200);
  // set ass servo
  for(int i=0; i<catNum; i++){
    assServo[i].attach(assServoPin[i]);
    handServo[i].attach(handServoPin[i]);
    
    assPos[i] = 90;
    assServo[i].write(assPos[i]);
    
    handPos[i] = minHandPos;
    handServo[i].write(handPos[i]);
    shakeBool[i] = false;
  }
}

void loop(){
  if(Serial.available()){
    uint8_t v = Serial.read();
    if(v == 255){      //Marker for next loop
      servoCount = 0;
    }else{
      if(servoCount<catNum){
        assServo[servoCount].write(v);
        servoCount++;
      }
      else
      {
        for(int i=0; i<8; i++){
          if(bitRead(v, i) == 1){            // "ON"
            if(handPos[(servoCount-catNum)] <= minHandPos){
              shakeBool[(servoCount-catNum)] = true;
              // If it is under the minimum angle
            }
            else if(handPos[(servoCount-catNum)]>=maxHandPos){
              shakeBool[(servoCount-catNum)] = false;
              //If it is over the maximum angle
            }
            if(shakeBool[(servoCount-catNum)]){
              handPos[(servoCount-catNum)]+=shakeSpeed;
              handServo[(servoCount-catNum)].write(handPos[(servoCount-catNum)]);
              // +angle and send to servo
            }else{
              handPos[(servoCount-catNum)]-=shakeSpeed;
              handServo[(servoCount-catNum)].write(handPos[(servoCount-catNum)]);
              // - angle and send to servo
            }
            delay(30);
          }else{                              // "OFF"
            //Do nothing
          }
          servoCount++;
        }
      }
    }
    if(servoCount >= ((catNum*2)-1)) servoCount = 0;
  }
}

One way to manage this kind of problem is to separate at the message handling from the control logic and servo control. That would make it easier to print debug messages to see if you are sending the right commands to the servos.