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