Pages: [1]   Go Down
Author Topic: Receiving data for multiple servos  (Read 710 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 11
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

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;
  }
}
Logged

Global Moderator
Netherlands
Offline Offline
Shannon Member
*****
Karma: 170
Posts: 12483
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset


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
Logged

Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

Global Moderator
Netherlands
Offline Offline
Shannon Member
*****
Karma: 170
Posts: 12483
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

putting this thought in code, not tested but just to get it:
Code:
#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)

Logged

Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

Offline Offline
Newbie
*
Karma: 0
Posts: 11
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

0
Offline Offline
Tesla Member
***
Karma: 118
Posts: 8962
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Consider the daffodil. And while you're doing that, I'll be over here, looking through your stuff.   smiley-cool

Offline Offline
Newbie
*
Karma: 0
Posts: 11
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Just tested it, works like a charm. Thanks!
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 11
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.

Code:
#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;
  }
}
Logged

London
Offline Offline
Faraday Member
**
Karma: 8
Posts: 6240
Have fun!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Pages: [1]   Go Up
Jump to: