Using Arduino with VB.Net to operate 4 servos

Hello all, I am trying to run 4 or more servos on an arduino mega by sending data through a serial port via VB.Net. The data gets sent with a comma separated variable string. My process works with two servos but when I extend my script to add the third and fourth servo everything seems to fall apart. If anyone can take a look at my code and let me know if they have any suggestions that would be awesome!

******** this is my Arduino Code

/*This demo receives a comma separated string over the serial port
from the Grasshopper plugin for Rhino, which ultimately controls a 
pan/tilt servo.


 */

#include <Servo.h>  //includes library for servos

#include <Wire.h>  //includes library for wires

 

#define BAUDRATE 19200

#define BUFFSIZE   12   // buffer one command at a time, 12 bytes is longer than the max length

char buffer[BUFFSIZE];  // this is the double buffer

uint8_t bufferidx = 0;
//********************************************************

uint16_t Servo1Degrees, Servo2Degrees, Servo3Degrees, Servo4Degrees;

 

//********************************************************

Servo RobotOne;  // create variable object to control the Pan servo
Servo RobotTwo;  // create variable object to control the Tilt servo
Servo RobotThree; // crate variable object to control third servo
Servo RobotFour; // crate variable object to control third servo


char *parseptr;

char buffidx;

 

void setup()          

{
//********************************************************

RobotOne.attach(13);  // attaches the servo on pin 9 to the servo o
object
RobotTwo.attach(12);  // attaches the servo on pin 10 to the servo object
RobotThree.attach(11); //attach to pin 11 
RobotFour.attach(10); //attach to pin 10

  Serial.begin(BAUDRATE);   // opens serial port, sets data rate to BAUDRATE

 

}

 

void loop()                    

{

  char c;    // holds one character from the serial port

  if (Serial.available()) {

    c = Serial.read();      // read one character

    buffer[bufferidx] = c;  // add to buffer

    Serial.print(c);//write raw data to serial for testing

   

    if (c == '\n') {   

      buffer[bufferidx+1] = 0; // terminate it

      Serial.println(" eol");    // debug marker

 
//********************************************************
      parseptr = buffer;    // offload the buffer into temp variable

 

      Servo1Degrees = parsedecimal(parseptr);    // parse out the first number, and offload to variable
      parseptr = strchr(parseptr, ',')+1;  // move past the ","

      Servo2Degrees = parsedecimal(parseptr);    // parse the second number
      parseptr = strchr(parseptr, ',')+1;  // move past the ","
      
      Servo3Degrees = parsedecimal(parseptr);
      parseptr = strchr(parseptr, ',')+1;  // move past the ","
      
      Servo4Degrees = parsedecimal(parseptr);
      
      
  //********************************************************
     
      
      RobotOne.write(Servo1Degrees); // tell Pan servo to go to position in variable 'Servo1Degrees'
      RobotTwo.write(Servo2Degrees); // tell Tilt servo to go to position in variable 'Servo2Degrees'
      RobotThree.write(Servo3Degrees); //tell servo three to go to position in variable 'Servo3Degrees'     
      RobotFour.write(Servo4Degrees); //tell servo three to go to position in variable 'Servo4Degrees' 
      
      
      
      
      bufferidx = 0;      // reset the buffer for the next read

      return;        //return so that we don't trigger the index increment below

    }

  

       

        // didn't get newline, need to read more from the buffer

    bufferidx++;    // increment the index for the next character

    if (bufferidx == BUFFSIZE-1) {  //if we get to the end of the buffer reset for safety

      Serial.print('!', BYTE);

      bufferidx = 0;

    }

  }

}

 

uint32_t parsedecimal(char *str) 

{

  uint32_t d = 0;

 

  while (str[0] != 0) {

    if ((str[0] > '9') || (str[0] < '0'))

      return d;

    d *= 10;

    d += str[0] - '0';

    str++;

  }

  return d;

}

And this is my VB.Net code

Option Strict Off
Option Explicit On

'Import SDK and Framework namespaces
Imports RMA.OpenNURBS
Imports RMA.Rhino

Imports Grasshopper
Imports Grasshopper.Kernel
Imports Grasshopper.Kernel.Data

Imports System
Imports System.IO
Imports System.Xml
Imports System.Data
Imports System.Drawing
Imports System.Reflection
Imports System.Collections
Imports System.Windows.Forms
Imports Microsoft.VisualBasic
Imports System.Collections.Generic
Imports System.Runtime.InteropServices



'Code generated by Grasshopper(R) (except for RunScript() content and Additional content)
'Copyright (C) 2009 - Robert McNeel & Associates
<System.Runtime.CompilerServices.CompilerGenerated()> _
Public Class Script_Instance
  Implements IGH_ScriptInstance

#Region "Members"
  ''' <summary>List of error messages. Do not modify this list directly.</summary>
  Private __err As New List(Of String)

  ''' <summary>List of print messages. Do not modify this list directly, use the Print() and Reflect() functions instead.</summary>
  Private __out As New List(Of String)

  ''' <summary>Represents the current Rhino document.</summary>
  Private doc As MRhinoDoc = RMA.Rhino.RhUtil.RhinoApp().ActiveDoc()

  ''' <summary>Represents the current Rhino application.</summary>
  Private app As MRhinoApp = RMA.Rhino.RhUtil.RhinoApp()

  ''' <summary>Represents the Script component which maintains this script.</summary>
  Public owner As Grasshopper.Kernel.IGH_ActiveObject
#End Region

#Region "Utility functions"
  ''' <summary>Print a String to the [Out] Parameter of the Script component.</summary>
  ''' <param name="text">String to print.</param>
  Private Sub Print(ByVal text As String)
    __out.Add(text)
  End Sub

  ''' <summary>Print a formatted String to the [Out] Parameter of the Script component.</summary>
  ''' <param name="format">String format.</param>
  ''' <param name="args">Formatting parameters.</param>
  Private Sub Print(ByVal format As String, ByVal ParamArray args As Object())
    __out.Add(String.Format(format, args))
  End Sub

  ''' <summary>Print useful information about an object instance to the [Out] Parameter of the Script component. </summary>
  ''' <param name="obj">Object instance to parse.</param>
  Private Sub Reflect(ByVal obj As Object)
    __out.Add(GH_ScriptComponentUtilities.ReflectType_VB(obj))
  End Sub

  ''' <summary>Print the signatures of all the overloads of a specific method to the [Out] Parameter of the Script component. </summary>
  ''' <param name="obj">Object instance to parse.</param>
  Private Sub Reflect(ByVal obj As Object, ByVal method_name As String)
    __out.Add(GH_ScriptComponentUtilities.ReflectType_VB(obj, method_name))
  End Sub
#End Region

  ''' <summary>
  ''' This procedure contains the user code. Input parameters are provided as ByVal arguments,
  ''' Output parameter are ByRef arguments. You don't have to assign output parameters,
  ''' they will be null by default.
  ''' </summary>
  Private Sub RunScript(ByVal Servo1Degrees As Integer, ByVal Servo2Degrees As Integer, ByVal StepNumber As Integer, ByVal Servo3Degrees As Integer, ByVal Servo4Degrees As Integer, ByRef A As Object) 

    Select Case StepNumber

      Case 0

        OpenPort()

        Print("Port has been opened")

      Case 1
        '******************************************************************************************************************
        SendDegrees(Servo1Degrees, Servo2Degrees, Servo3Degrees, Servo4Degrees)
        Print(Servo1Degrees & "," & Servo2Degrees & "," & Servo3Degrees & "," & Servo4Degrees)  'actually writes that data to serial port

      Case 2

        ClosePort()

        Print("Port has been closed")

    End Select



  End Sub 

  '<Custom additional code> 

  Private WithEvents CPort As New IO.Ports.SerialPort      'port with events



  Sub OpenPort()   'called early in the app, before the port is ever needed. This attaches the port, and opens it to listen.

    Try

      If CPort.IsOpen = False Then

        CPort = My.Computer.Ports.OpenSerialPort("COM3", 19200)

      End If

    Catch ex As Exception

      MessageBox.Show(ex.ToString)

    End Try

  End Sub


  '*********************************************************
  Sub SendDegrees(ByVal Servo1Degrees As Integer, ByVal Servo2Degrees As Integer, ByVal Servo3Degrees As Integer, ByVal Servo4Degrees As Integer)

    CPort.WriteLine(CStr(Servo1Degrees & "," & Servo2Degrees & "," & Servo3Degrees & "," & Servo4Degrees))

  End Sub



  'should be called when the port is no longer needed.

  Private Sub ClosePort()

    If CPort IsNot Nothing Then

      If CPort.IsOpen = True Then

        CPort.Close()

      End If

      CPort.Dispose()

    End If

  End Sub



  '</Custom additional code> 

End Class

My process works with two servos but when I extend my script to add the third and fourth servo everything seems to fall apart.

It would be easier to help you debug this if you explained just what falls apart, and how.

One thing that pops out is this:

#define BUFFSIZE   12   // buffer one command at a time, 12 bytes is longer than the max length

If the string happened to be 130,120,100,110, that’s 15 bytes.

Are you sure it's a software problem?

How are you powering the servos?

If the string happened to be 130,120,100,110, that's 15 bytes.

the cr and terminating zero are also written into that buffer.

Another approach to the arduino parsing that is more robust and has a smaller footprint is to convert the numeric values on the fly.

You can create an array of four integers to hold the values, here is on approach:

Wait for a header character to indicate the start of a message Set the four elements in your array to zero and set in index counter to 0 (the first element) If the incoming character is a digit, multiply the array[index] by 10 and add the char – '0' If the incoming character is a comma, increment the index If the index exceeds the size of the array or the incoming character is a terminating character (carriage return for example) then you can write the values in the array to the servos