Servo Arm Issue

Question

I built a robotic arm with Arduino uno and a Servo shield. The arm is controlled by C# visual studio with USB. There are currently 6 servos attached to the arm and a power supply is used with 5V,2A. I can control the arm by sending data to it. The problem is after few data is sent, the servo arm lose control and move randomly.

*UPDATE I use a 5V,5A power supply and reduce the number of servo to 3. The robot still misbehave/randomly move after few data is send to the servo.(the robot misbehave for a few second and back to the position i want itself.) (I have 6 servos, I tried 3 servos each and i have the same problem.)

This is my arduino code

#include <Servo.h>  
Servo myservoA;  
Servo myservoB;
Servo myservoC;
Servo myservoD;
Servo myservoE;
Servo myservoF;
int i,pos,myspeed,myshow,COM0,MOVE=0;
int sea,seb,sec,sed,see,sef;
static int v=0;
byte Readbytes [10];
byte byte0, byte1,byte2,byte3,byte4,byte5,byte6,byte7;
byte newpos0, newpos1,newpos2,newpos3,newpos4,newpos5,newpos6,newpos7;
byte oldpos0, oldpos1,oldpos2,oldpos3,oldpos4,oldpos5,oldpos7,oldpos8;

String mycommand="";    /// Serial capture   #auto: automatic operation  
#com: computer serial port control  #stop: standstill
static int mycomflag=2; // #auto：2 automatic operation  , #com： 1  computer 
serial port control    #stop：0  standstill 

void setup() 
{ 
  pinMode(13,INPUT);
  pinMode(12,INPUT);  
  Serial.begin(9600);
  myshow=0;
  mycomflag=2;         // the  ARM default  state: 2 automatic operation
                   //                          1 pc
  myservoA.attach(3);     
  myservoB.attach(5);    
  myservoC.attach(6);  
  myservoD.attach(9);  
  myservoE.attach(10); 
  myservoF.attach(11); 

  myservoA.write(10);   //0A Control wrist rotation
  myservoB.write(20);   //14
  myservoC.write(10);   //0A
  myservoD.write(20);   //3C
  myservoE.write(30);  //64
 myservoF.write(5);    //4B Control waist

oldpos0=10;oldpos1=20;oldpos2=10;oldpos3=20;oldpos4=30;oldpos5=5;


newpos0=10;newpos1=20;newpos2=10;newpos3=20;newpos4=30;newpos5=5;


}

void loop() 
{ 
   if (Serial.available()>0) 
  {
  i=0;int bufferLimit=9;
  while(Serial.available()>0 && i < bufferLimit) 
  {      
    Readbytes[i]= Serial.read();
    i++;       
  }
    newpos0=(Readbytes[0]);
    newpos1=(Readbytes[1]);
    newpos2=(Readbytes[2]);
    newpos3=(Readbytes[3]);
    newpos4=(Readbytes[4]);
    newpos5=(Readbytes[5]);
    newpos6=(Readbytes[6]);
    newpos7=(Readbytes[7]);
  COM0=1;
  }


  delay(100);//Required for all bytes to be read at SP2 before sending
        //at SP2 again
  if (COM0==1&&(newpos5==2||newpos5==24||newpos5==80))
  {
  //Serial.write(Readbytes,i);//To Freezer
  //Serial.write(Readbytes[0]);
  //Serial.write(Readbytes[1]);
  Serial.write(newpos0);
  Serial.write(newpos1);
  Serial.write(newpos2);
  Serial.write(newpos3);
  Serial.write(newpos4);
  Serial.write(newpos5);
  Serial.write(newpos6);
  Serial.write(newpos7);

  COM0=0;
  MOVE=1;
  }

  if(MOVE==1)
  {
  myspeed=800;
  for(pos = 0; pos <=myspeed; pos += 1)  
  {                                
   myservoA.write(int(map(pos,1,myspeed,oldpos0,newpos0))); 
   myservoB.write(int(map(pos,1,myspeed,oldpos1,newpos1))); 
   myservoC.write(int(map(pos,1,myspeed,oldpos2,newpos2))); 
   myservoD.write(int(map(pos,1,myspeed,oldpos3,newpos3))); 
   myservoE.write(int(map(pos,1,myspeed,oldpos4,newpos4))); 
   myservoF.write(int(map(pos,1,myspeed,oldpos5,newpos5)));
   delay(1);                       
  }
  MOVE=0;
  oldpos0=newpos0;
  oldpos1=newpos1;
  oldpos2=newpos2;
  oldpos3=newpos3;
  oldpos4=newpos4;
  oldpos5=newpos5;
 }

This is my Visual Studio WINFORM Code

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace RoboAnimate
{
public partial class Form1 : Form
{
    string RxString;
    string c1, c2, c3, c4, c5, c6, c7, c8, c9, c10, c11, c12, c13, c14, c15, 
    c16, c17;
    string fourbytesHexStr;
    UInt32 fourbytesHex = 0;
    byte A, B, C, D, E, F,timer1val;

    bool do1=false, do2 = false, do3 = false, do4 = false, do5 = false, do6 
    = false, sequence1 = true;

    private void timer1_Tick(object sender, EventArgs e)
    {
        timer1val++;
        textBox4.Text = timer1val.ToString();
        if (timer1val == 2) Do1();
        if (timer1val == 5) Do2();
        if (timer1val == 7) Do3();
        if (timer1val == 9) Do4();
        if (timer1val == 11) Do5();
        if (timer1val == 13) Do6();
        if (timer1val == 15) Do7();
        if (timer1val == 17) Do8();
        if (timer1val == 19) Do7();
        if (timer1val == 21) Do6();
        if (timer1val == 23) Do5();
        if (timer1val == 25) Do4();
        if (timer1val == 27) Do3();
        if (timer1val == 29) Do2();
        if (timer1val == 32)
        {
            Do1();
            sequence1 = false;
            timer1.Enabled = false;
            timer1val = 0;
        }
    }

    byte whichdo;

    private void Form1_Load(object sender, EventArgs e)
    {

    }

    private void Pos2_Click(object sender, EventArgs e)
    {
        //                                 Gripper
        //                                    A   B   C   D   E   F     
        byte[] M1bytesToSend = new byte[8] { 160, 30, 10, 20, 30, 24, 0x91, 
        0xCA };
        serialPort1.Write(M1bytesToSend, 0, 8);
    }

    private void Pos3_Click(object sender, EventArgs e)
    {
        //                                 Gripper
        //                                    A   B   C   D   E   F     
        byte[] M1bytesToSend = new byte[8] { 160, 10, 10, 20, 30, 24, 0x91, 
        0xCA };
        serialPort1.Write(M1bytesToSend, 0, 8);
    }

    private void Pos4_Click(object sender, EventArgs e)
    {
        //                                 Gripper
        //                                    A   B   C   D   E   F     
        byte[] M1bytesToSend = new byte[8] { 160, 10, 30, 60, 40, 24, 0x91, 
        0xCA };
        serialPort1.Write(M1bytesToSend, 0, 8);
    }

    private void Pos5_Click(object sender, EventArgs e)
    {
        //                                 Gripper
        //                                    A   B   C   D   E   F     
        byte[] M1bytesToSend = new byte[8] { 160, 60, 30, 60, 40, 24, 0x91, 
         0xCA };
        serialPort1.Write(M1bytesToSend, 0, 8);
    }

    private void Pos6_Click(object sender, EventArgs e)
    {
        //                                 Gripper
        //                                    A   B   C   D   E   F     
        byte[] M1bytesToSend = new byte[8] { 90, 60, 30, 60, 40, 24, 0x91, 
     0xCA };
        serialPort1.Write(M1bytesToSend, 0, 8);
    }

    private void Pos7_Click(object sender, EventArgs e)
    {
        //                                 Gripper
        //                                    A   B   C   D   E   F     
        byte[] M1bytesToSend = new byte[8] { 90, 100, 60, 100, 40, 80, 0x91, 
      0xCA };
        serialPort1.Write(M1bytesToSend, 0, 8);
    }

    private void Pos8_Click(object sender, EventArgs e)
    {
        //                                 Gripper
        //                                    A   B   C   D   E   F     
        byte[] M1bytesToSend = new byte[8] { 95, 90, 130, 100, 40, 80, 0x91, 
         0xCA };
        serialPort1.Write(M1bytesToSend, 0, 8);
    }

    private void ScrollA_Scroll(object sender, ScrollEventArgs e)
    {
        ServoA.Text = ScrollA.Value.ToString();
    }

    private void ScrollB_Scroll(object sender, ScrollEventArgs e)
    {
        ServoB.Text = ScrollB.Value.ToString();
    }

    private void ScrollC_Scroll(object sender, ScrollEventArgs e)
    {
        ServoC.Text = ScrollC.Value.ToString();
    }

    private void ScrollD_Scroll(object sender, ScrollEventArgs e)
    {
        ServoD.Text = ScrollD.Value.ToString();
    }

    private void ScrollE_Scroll(object sender, ScrollEventArgs e)
    {
        ServoE.Text = ScrollE.Value.ToString();
    }

    private void ScrollF_Scroll(object sender, ScrollEventArgs e)
    {
        ServoF.Text = ScrollF.Value.ToString();
    }

    private void Send_Click(object sender, EventArgs e)
    {
        A = Convert.ToByte(ServoA.Text);
        B = Convert.ToByte(ServoB.Text);
        C = Convert.ToByte(ServoC.Text);
        D = Convert.ToByte(ServoD.Text);
        E = Convert.ToByte(ServoE.Text);
        F = Convert.ToByte(ServoF.Text);
        //                                    A     B     C     D     E     

        //byte[] M1bytesToSend = new byte[8] { 0x4A, 0x14, 0x0A, 0x3C, 0x64, 
        0x4B, 0x91, 0xCA };   
        byte[] M1bytesToSend = new byte[8] { A, B, C, D, E, F, 0x91, 0xCA };
        serialPort1.Write(M1bytesToSend, 0, 8);
    }

    private void serialPort1_DataReceived(object sender, 
     SerialDataReceivedEventArgs e)
    {
        Thread.Sleep(50);
        int i;
        int bytes = serialPort1.BytesToRead;
        byte[] buffer = new byte[bytes];
        string[] hex = new string[bytes];
        serialPort1.Read(buffer, 0, bytes);


        RxString = ByteArrayToHexString(buffer);
        for (i = 0; i < bytes; ++i)
        {
            hex[i] = buffer[i].ToString("X");
        }
        textBox2.Text = RxString;
        textBox3.Text = bytes.ToString();

        if (bytes == 9)
        {
            c1 = (hex[0]).PadLeft(2, '0');
            c2 = (hex[1]).PadLeft(2, '0');
            c3 = (hex[2]).PadLeft(2, '0');
            c4 = (hex[3]).PadLeft(2, '0');
            c5 = (hex[4]).PadLeft(2, '0');
            c6 = (hex[5]).PadLeft(2, '0');
            c7 = (hex[6]).PadLeft(2, '0');
            c8 = (hex[7]).PadLeft(2, '0');
            c9 = (hex[8]).PadLeft(2, '0');

            fourbytesHexStr = string.Concat(c1, c2, c3, c4, c5, c6, c7, c8, 
            c9);
            fourbytesHexStr = fourbytesHexStr.PadRight(8, '0');
            textBox1.Text = fourbytesHexStr;
            textBox3.Text = bytes.ToString();
            serialPort1.DiscardInBuffer();
        }

        if (bytes == 8)
        {
            c1 = (hex[0]).PadLeft(2, '0');
            c2 = (hex[1]).PadLeft(2, '0');
            c3 = (hex[2]).PadLeft(2, '0');
            c4 = (hex[3]).PadLeft(2, '0');
            c5 = (hex[4]).PadLeft(2, '0');
            c6 = (hex[5]).PadLeft(2, '0');
            c7 = (hex[6]).PadLeft(2, '0');
            c8 = (hex[7]).PadLeft(2, '0');


            fourbytesHexStr = string.Concat(c1, c2, c3, c4, c5, c6, c7, c8);
            fourbytesHexStr = fourbytesHexStr.PadRight(8, '0');
            textBox1.Text = fourbytesHexStr;
            textBox3.Text = bytes.ToString();
            serialPort1.DiscardInBuffer();
        }

        if (bytes == 7)
        {
            c1 = (hex[0]).PadLeft(2, '0');
            c2 = (hex[1]).PadLeft(2, '0');
            c3 = (hex[2]).PadLeft(2, '0');
            c4 = (hex[3]).PadLeft(2, '0');
            c5 = (hex[4]).PadLeft(2, '0');
            c6 = (hex[5]).PadLeft(2, '0');
            c7 = (hex[6]).PadLeft(2, '0');



            fourbytesHexStr = string.Concat(c1, c2, c3, c4, c5, c6, c7);
            fourbytesHexStr = fourbytesHexStr.PadRight(8, '0');
            textBox1.Text = fourbytesHexStr;
            textBox3.Text = bytes.ToString();
            serialPort1.DiscardInBuffer();
        }
    }

    private string ByteArrayToHexString(byte[] buffer)
    {
        StringBuilder sb = new StringBuilder(buffer.Length * 3);
        foreach (byte b in buffer)
            sb.Append(Convert.ToString(b, 16).PadLeft(2, '0').PadRight(3, ' 
       '));
        return sb.ToString().ToUpper();
    }

    private void Pos1_Click(object sender, EventArgs e)
    {
        //                                 Gripper
        //                                    A   B   C   D   E   F     
        byte[] M1bytesToSend = new byte[8] { 115, 95, 80, 21, 31, 2, 0x91, 
     0xCA };
        serialPort1.Write(M1bytesToSend, 0, 8);
    }

    private void Open_Click(object sender, EventArgs e)
    {
        serialPort1.PortName = "COM6";
        serialPort1.BaudRate = 9600;
        serialPort1.DataBits = 8;
        serialPort1.Parity = Parity.None;
        serialPort1.StopBits = StopBits.One;
        serialPort1.Open();

        if (serialPort1.IsOpen) PortStatus.Text = "PORT Opened...Click to 
        Start DAQ";
        Close.Enabled = true;
        Open.Enabled = false;
    }

    public Form1()
    {
        InitializeComponent();
        whichdo = 100;
    }

    private void Animate1_Click(object sender, EventArgs e)
    {
        timer1.Enabled = true;
    }


    private void Do1()
    {
        byte[] M1bytesToSend = new byte[8] { 115, 95, 80, 21, 31, 2, 0x91, 
    0xCA };
        serialPort1.Write(M1bytesToSend, 0, 8);
    }

    private void Do2()
    {
        byte[] M2bytesToSend = new byte[8] { 160, 30, 10, 20, 30, 24, 0x91, 
       0xCA };
        serialPort1.Write(M2bytesToSend, 0, 8);
    }

    private void Do3()
    {
        byte[] M3bytesToSend = new byte[8] { 160, 10, 10, 20, 30, 24, 0x91, 
      0xCA };
        serialPort1.Write(M3bytesToSend, 0, 8);
    }

    private void Do4()
    {
        byte[] M4bytesToSend = new byte[8] { 160, 10, 30, 60, 40, 24, 0x91, 
     0xCA };
        serialPort1.Write(M4bytesToSend, 0, 8);
    }

    private void Do5()
    {
        byte[] M5bytesToSend = new byte[8] { 160, 60, 30, 60, 40, 24, 0x91, 
     0xCA };
        serialPort1.Write(M5bytesToSend, 0, 8);
    }

    private void Do6()
    {
        byte[] M6bytesToSend = new byte[8] { 90, 60, 30, 60, 40, 24, 0x91, 
      0xCA };
        serialPort1.Write(M6bytesToSend, 0, 8);
    }

    private void Do7()
    {
        byte[] M7bytesToSend = new byte[8] { 90, 100, 60, 100, 40, 80, 0x91, 
      0xCA };
        serialPort1.Write(M7bytesToSend, 0, 8);
    }

    private void Do8()
    {
        byte[] M8bytesToSend = new byte[8] { 95, 90, 130, 100, 40, 80, 0x91, 
     0xCA };
        serialPort1.Write(M8bytesToSend, 0, 8);
    }




}
}

Answer 1

This isn't a C# issue :-)

However, there are a few areas in your posted code that could do with some attention:

 i=0;
 while(Serial.available()>0) 
    {      
        Readbytes[i]= Serial.read();
        i++;       
    }

Not a good idea - you need to limit i so that a stream of garbage coming in on serial doen't wipe out memory. This looks a bit C like, so it would be something like

 i=0;
 while(Serial.available()>0 && i < bufferLimit) 
    {      
        Readbytes[i]= Serial.read();
        i++;       
    }

where bufferLimit is a value that is set to the length of ReadBytes in some way.

In addition, you need some way to frame your comms so that you know where you are. Just assuming the next 8 bytes are correct and plonking them into your servos is a recipe for disaster. You can implement a simple protocol that has a definite lead-in character, data bytes and some sort of checksum or CRC. If you are unsure how to do this please feel free to post a comment and I will make suggestions. In the meantime, please post whatever the code is that is sending data to the code you have posted because that may easily be the culprit.

Edit:

But start with the comment by Aydin Adn because it is #1 contender for causing the problem. My suggestions in this answer could save you grief in the future.

Edit 30th September:

Looking at your C# code, I am more and more of the opinion that something is interfering with your data stream and is putting the PC out of sync with the Arduino, which you cannot recover from because your comms has no framing or error checking.

There are a couple of possibilities:

1. Your serial adapter; I see that you are using COM6 and this points to a USB adapter of some sort. If you are using a cheap and cheerful adapter it is possible that it is inventing data - I had this at work and the only way around it was to only use USB->serial adapters that used the FTDI chipset; anything cheap from China could get up to all sorts of antics, including working absolutely fine for a couple of hours and then suddenly repeating a whole block of data from earlier in the day. The problem is in the drivers rather than the hardware, some of them can be pretty poor.
2. Your data lead/the Arduino board itself; since you have a chunky power supply and some servos drawing current it is entirely possible that there is noise being induced in the comms line and this is being interpreted by the Arduino as data which sits in its receive buffer until you send some more and process it, at which point the bytes you send aren't going to the servo you intend.
3. You are overrunning/racing at some point; I notice Thread.Sleep() and serial discards that are obviously intended to try and frame things. This sort of thing never helps and can make everything very dependent on exact timing of events, whereas you need it to be much more deterministic and time independent.

The only way to be sure will be to implement a simple protocol that can definitely frame a command to move and validate the data to be sure that what was received was what the PC sent. You are only running at 9600 baud which is approximately 1mS per byte, so bumping up the command length from 8 bare bytes will still bring a command in at under 20mS. I would suggest a variant of Intel Hex format which is all in ASCII, you could use something like:

:{sz}{cm}{data}{chk}

: is a colon character and is unique from one command to the next; get a colon and you know what is coming next. Everything from then on is pairs of hexadecimal characaters 0-9/A-F making encoded bytes.

{sz} is the number of bytes in {data} (this is optional but gives some flexibility if you expand your command structure)

{cm} is a command byte (you can split this to give some bits to allow simple retry detection)

{data} is "sz" encoded bytes of data (could be zero but will be 8 for a move command)

{chk} is a negated sum of all raw byte values between the : and the checksum

Decoding this is simplicity and if you add the decoded bytes from sz to chk and get anything other than zero you can chuck the whole lot away and not process it because something is wrong. Similarly, once you receive the colon there can only be 0-9/A-Z, anything else and you can ignore it and let the PC decide what it is going to do about it.

This scheme would allow a status command ":000000" to be sent by the PC to get the current state and a move command something like ":0801A00A1E3C2891CA70" which would be the values from Do4() as an example "M4bytesToSend = new byte[8] { 160, 10, 30, 60, 40, 24, 0x91, 0xCA };" assuming 01 is the move command.

Once the Arduino had decoded the move command above it could return it with a busy bit set to indicate that it had received it and was about to do the move, say ":0861A00A1E3C2891CAD2" and then when it had finished and was ready for the next command it could send ":0841A00A1E3C2891CAB2". In both cases the 0x01 command sent by the PC has extra bits set so that a loopback of its own data wouldn't fool the PC. Using the top bit of the command would allow a retry to be detected; if the PC sends 0x01 twice in succession, the Arduino won't execute the second but will simply send the ":0841A00A1E3C2891CAB2" response to indicate that it has executed that command. The PC must use 0x81 to get a new command accepted and in this way the PC can happily resend a command if it doesn't get a response, safe in the knowledge that it will either be executed if the Arduino didn't get it the first time, or get a response that tells it that command has been done. The PC can of course send :000000 and get the last response back, if it doesn't simply retry the command.

I realise that this seems very long winded when all you want to do is send half a dozen bytes to control an arm but comms of this nature is intrinsically vulnerable, so the only way to get it operating reliably will be to implement some sort of protocol.

I have researched the Arduino methodology and have code that should do exactly this, if you want to try and hack it into a working program (I have no means to compile it so it's a best guess).

Answer 2

The issue isn't your code, it's your power supply... You're drawing too much current to your servos. Either upgrade the power supply or reduce the number of servos, on 2A, 3 x 9g servos would be pushing it imo, i'd give 750mA per servo to stay on the safe side.