Page 1 of 1

Controlling 4 motors with AdvancedServo

Posted: Mon Jul 23, 2012 9:03 pm
by viccos
Hi....

I am working in a form that helps me test displacement of a mobile robot with omnidirectional wheels for a few seconds. What I did was to modify the advance servo full. The velocity of the servos is giving by the change in position (since I am using parallax HB-25).

My only concern is that motor cero works well, but motors 1, 2 and 3 don´t. And I guess I am misisng something when attaching the other motors.

I am sorry if you find a lot of spanish terms here. Thanks a lot....

Here is my Code:

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using Phidgets; //for the servo class and the exceptions class
using Phidgets.Events; //for the event handling classes

namespace AdvancedServo_full
{

public partial class Form1 : Form
{
int segundos;
int tmax;
int positionMotor = 90;
int NumeroServos;
bool StartStop = true;
bool AdelanteAtrasSeleccionado1;
bool AdelanteAtrasSeleccionado2;
bool AdelanteAtrasSeleccionado3;
bool AdelanteAtrasSeleccionado4;
bool VelM1Seleccionado;
bool VelM2Seleccionado;
bool VelM3Seleccionado;
bool VelM4Seleccionado;
string Errormessage1 = @" Por favor elija todos los
parámetros de los motores";
string TempDir = "";
string TempVel = "";
private AdvancedServo advServo; //Declare an advancedservo object
private ErrorEventBox errorBox;

public Form1()
{
InitializeComponent();
errorBox = new ErrorEventBox();
}

//Initialization code
private void Form1_Load(object sender, EventArgs e)
{
advServo = new AdvancedServo();

advServo.Attach += new AttachEventHandler(advServo_Attach);
advServo.Detach += new DetachEventHandler(advServo_Detach);
advServo.Error += new ErrorEventHandler(advServo_Error);


//This assumes that if there is a command line argument, it is a serial number
//and we try to open that specific device. Otherwise, open any device.
openCmdLine(advServo);
}

//PhidgetServo attach event handling code
void advServo_Attach(object sender, AttachEventArgs e)
{
AdvancedServo attached = (AdvancedServo)sender;

ConectadoTxt.Text = attached.Attached.ToString();
nameTxt.Text = attached.Name;
serialTxt.Text = attached.SerialNumber.ToString();
versiontxt.Text = attached.Version.ToString();
numServosTxt.Text = attached.servos.Count.ToString();

NumeroServos = attached.servos.Count;



ArranqueParo.Enabled = true;


//Set the default servo type to the one Phidgets sells
foreach (AdvancedServoServo motor in attached.servos)
motor.Type = ServoServo.ServoType.HITEC_HS322HD;

foreach(String servoType in System.Enum.GetNames(typeof(ServoServo.ServoType)))
{
//stop here
if (servoType.Equals(ServoServo.ServoType.USER_DEFINED.ToString()))
break;
}

initAllValues();

}

//PhidgetServo detach event handling code
void advServo_Detach(object sender, DetachEventArgs e)
{
AdvancedServo detached = (AdvancedServo)sender;

ConectadoTxt.Text = detached.Attached.ToString();
nameTxt.Clear();
serialTxt.Clear();
versiontxt.Clear();
numServosTxt.Clear();


ArranqueParo.Enabled = false;

}

void advServo_Error(object sender, ErrorEventArgs e)
{
Phidget phid = (Phidget)sender;
DialogResult result;
switch (e.Type)
{
case PhidgetException.ErrorType.PHIDGET_ERREVENT_BADPASSWORD:
phid.close();
TextInputBox dialog = new TextInputBox("Error Event",
"Authentication error: This server requires a password.", "Please enter the password, or cancel.");
result = dialog.ShowDialog();
if (result == DialogResult.OK)
openCmdLine(phid, dialog.password);
else
Environment.Exit(0);
break;
default:
if (!errorBox.Visible)
errorBox.Show();
break;
}
errorBox.addMessage(DateTime.Now.ToLongDateString() + " " + DateTime.Now.ToLongTimeString() + ": " + e.Description);
}



//Servo velocity change event handling code


private void initAllValues()
{
tmax = 1;


AdelAtrMotor1.Enabled = true;
VelocidadM1.Enabled = true;
AdelAtrMotor2.Enabled = true;
VelocidadM2.Enabled = true;
AdelAtrMotor3.Enabled = true;
VelocidadM3.Enabled = true;
AdelAtrMotor4.Enabled = true;
VelocidadM4.Enabled = true;

for (int i = 0; i < NumeroServos; i++)
{
advServo.servos[i].PositionMax = 160;
advServo.servos[i].PositionMin = 20;
advServo.servos[i].VelocityLimit = 316;
advServo.servos[i].Acceleration = 365714;
advServo.servos[i].Position = 90;
}

}

//When the application is terminating, close the Phidget.
private void Form1_FormClosed(object sender, FormClosedEventArgs e)
{
advServo.servos[0].Position = 90;
advServo.servos[1].Position = 90;
advServo.servos[2].Position = 90;
advServo.servos[3].Position = 90;

advServo.Attach -= new AttachEventHandler(advServo_Attach);
advServo.Detach -= new DetachEventHandler(advServo_Detach);
advServo.Error -= new ErrorEventHandler(advServo_Error);


//run any events in the message queue - otherwise close will hang if there are any outstanding events
Application.DoEvents();

advServo.close();
advServo = null;
}






//Parses command line arguments and calls the appropriate open
#region Command line open functions
private void openCmdLine(Phidget p)
{
openCmdLine(p, null);
}

private void openCmdLine(Phidget p, String pass)
{
int serial = -1;
int port = 5001;
String host = null;
bool remote = false, remoteIP = false;
string[] args = Environment.GetCommandLineArgs();
String appName = args[0];

try
{ //Parse the flags
for (int i = 1; i < args.Length; i++)
{
if (args[i].StartsWith("-"))
switch (args[i].Remove(0, 1).ToLower())
{
case "n":
serial = int.Parse(args[++i]);
break;
case "r":
remote = true;
break;
case "s":
remote = true;
host = args[++i];
break;
case "p":
pass = args[++i];
break;
case "i":
remoteIP = true;
host = args[++i];
if (host.Contains(":"))
{
port = int.Parse(host.Split(':')[1]);
host = host.Split(':')[0];
}
break;
default:
goto usage;
}
else
goto usage;
}

if (remoteIP)
p.open(serial, host, port, pass);
else if (remote)
p.open(serial, host, pass);
else
p.open(serial);
return; //success
}
catch { }
usage:
StringBuilder sb = new StringBuilder();
sb.AppendLine("Invalid Command line arguments." + Environment.NewLine);
sb.AppendLine("Usage: " + appName + " [Flags...]");
sb.AppendLine("Flags:\t-n serialNumber\tSerial Number, omit for any serial");
sb.AppendLine("\t-r\t\tOpen remotely");
sb.AppendLine("\t-s serverID\tServer ID, omit for any server");
sb.AppendLine("\t-i ipAddress:port\tIp Address and Port. Port is optional, defaults to 5001");
sb.AppendLine("\t-p password\tPassword, omit for no password" + Environment.NewLine);
sb.AppendLine("Examples: ");
sb.AppendLine(appName + " -n 50098");
sb.AppendLine(appName + " -r");
sb.AppendLine(appName + " -s myphidgetserver");
sb.AppendLine(appName + " -n 45670 -i 127.0.0.1:5001 -p paswrd");
MessageBox.Show(sb.ToString(), "Argument Error", MessageBoxButtons.OK, MessageBoxIcon.Error);

Application.Exit();
}
#endregion




private void AdelAtrMotor1_SelectedIndexChanged(object sender, EventArgs e)
{
AdelanteAtrasSeleccionado1 = true;
textBox1.Text = (string)AdelAtrMotor1.SelectedItem;
if ((string)AdelAtrMotor1.SelectedItem == "Nula")
{
AdelanteAtrasSeleccionado1 = false;
}

}

private void AdelAtrMotor2_SelectedIndexChanged(object sender, EventArgs e)
{
AdelanteAtrasSeleccionado2 = true;
textBox3.Text = (string)AdelAtrMotor2.SelectedItem;
if ((string)AdelAtrMotor2.SelectedItem == "Nula")
{
AdelanteAtrasSeleccionado2 = false;
}
}

private void AdelAtrMotor3_SelectedIndexChanged(object sender, EventArgs e)
{
AdelanteAtrasSeleccionado3 = true;
textBox5.Text = (string)AdelAtrMotor3.SelectedItem;
if ((string)AdelAtrMotor3.SelectedItem == "Nula")
{
AdelanteAtrasSeleccionado3 = false;
}
}

private void AdelAtrMotor4_SelectedIndexChanged(object sender, EventArgs e)
{
AdelanteAtrasSeleccionado4 = true;
textBox7.Text = (string)AdelAtrMotor4.SelectedItem;
if ((string)AdelAtrMotor4.SelectedItem == "Nula")
{
AdelanteAtrasSeleccionado4 = false;
}
}

private void VelocidadM1_SelectedIndexChanged(object sender, EventArgs e)
{
VelM1Seleccionado = true;
textBox2.Text = (string)VelocidadM1.SelectedItem;
}



private void VelocidadM2_SelectedIndexChanged_1(object sender, EventArgs e)
{
VelM2Seleccionado = true;
textBox4.Text = (string)VelocidadM2.SelectedItem;
}

private void VelocidadM3_SelectedIndexChanged(object sender, EventArgs e)
{
VelM3Seleccionado = true;
textBox6.Text = (string)VelocidadM3.SelectedItem;
}

private void VelocidadM4_SelectedIndexChanged(object sender, EventArgs e)
{
VelM4Seleccionado = true;
textBox8.Text = (string)VelocidadM4.SelectedItem;
}

public void EncontrarPosicionMotor()
{
if (TempDir == "Nula")
{
positionMotor = 90;
}

if (TempVel == "Muy Rápida" && TempDir == "Adelante")
{
positionMotor = 145;
}
else
if (TempVel == "Rápida" && TempDir == "Adelante")
{
positionMotor = 137;
}
else
if (TempVel == "Media" && TempDir == "Adelante")
{
positionMotor = 128;
}
else
if (TempVel == "Lenta" && TempDir == "Adelante")
{
positionMotor = 119;
}
else
if (TempVel == "Muy Lenta" && TempDir == "Adelante")
{
positionMotor = 110;
}


if (TempVel == "Muy Rápida" && TempDir == "Reversa")
{
positionMotor = 42;
}
else
if (TempVel == "Rápida" && TempDir == "Reversa")
{
positionMotor = 51;
}
else
if (TempVel == "Media" && TempDir == "Reversa")
{
positionMotor = 60;
}
else
if (TempVel == "Lenta" && TempDir == "Reversa")
{
positionMotor = 69;
}
else
if (TempVel == "Muy Lenta" && TempDir == "Reversa")
{
positionMotor = 78;
}
}


private void ArranqueParo_Click(object sender, EventArgs e)
{
TiempoMotores.Text = tmax.ToString();
segundos = tmax;
if (StartStop && AdelanteAtrasSeleccionado1 && VelM1Seleccionado && AdelanteAtrasSeleccionado2 && VelM2Seleccionado && AdelanteAtrasSeleccionado3 && VelM3Seleccionado && AdelanteAtrasSeleccionado4 && VelM4Seleccionado)
{
TempDir = (string)AdelAtrMotor1.SelectedItem;
TempVel = (string)VelocidadM1.SelectedItem;
EncontrarPosicionMotor();
advServo.servos[0].Position = positionMotor;

TempDir = (string)AdelAtrMotor2.SelectedItem;
TempVel = (string)VelocidadM2.SelectedItem;
EncontrarPosicionMotor();
advServo.servos[1].Position = positionMotor;

TempDir = (string)AdelAtrMotor3.SelectedItem;
TempVel = (string)VelocidadM3.SelectedItem;
EncontrarPosicionMotor();
advServo.servos[2].Position = positionMotor;

TempDir = (string)AdelAtrMotor4.SelectedItem;
TempVel = (string)VelocidadM4.SelectedItem;
EncontrarPosicionMotor();
advServo.servos[3].Position = positionMotor;

TempDir = "";
TempVel = "";

timer1.Enabled = true;
ArranqueParo.Text = "Detener";
StartStop = false;
AdelAtrMotor1.Enabled = false;
VelocidadM1.Enabled = false;
AdelAtrMotor2.Enabled = false;
VelocidadM2.Enabled = false;
AdelAtrMotor3.Enabled = false;
VelocidadM3.Enabled = false;
AdelAtrMotor4.Enabled = false;
VelocidadM4.Enabled = false;



}
else
{
advServo.servos[0].Position = 90;
advServo.servos[1].Position = 90;
advServo.servos[2].Position = 90;
advServo.servos[3].Position = 90;

ArranqueParo.Text = "Iniciar";
timer1.Enabled = false;
StartStop = true;

AdelAtrMotor1.Enabled = true;
VelocidadM1.Enabled = true;
AdelAtrMotor2.Enabled = true;
VelocidadM2.Enabled = true;
AdelAtrMotor3.Enabled = true;
VelocidadM3.Enabled = true;
AdelAtrMotor4.Enabled = true;
VelocidadM4.Enabled = true;


}

if (AdelanteAtrasSeleccionado1 && VelM1Seleccionado)
{
label9.Text = "";
}
else
{
label9.Text = Errormessage1;
}

}

private void timer1_Tick(object sender, EventArgs e)
{
segundos--;
TiempoMotores.Text = segundos.ToString();
if (segundos == 0)
{
advServo.servos[0].Position = 90;
advServo.servos[1].Position = 90;
advServo.servos[2].Position = 90;
advServo.servos[3].Position = 90;

ArranqueParo.Text = "Iniciar";
timer1.Enabled = false;
StartStop = true;

AdelAtrMotor1.Enabled = true;
VelocidadM1.Enabled = true;
AdelAtrMotor2.Enabled = true;
VelocidadM2.Enabled = true;
AdelAtrMotor3.Enabled = true;
VelocidadM3.Enabled = true;
AdelAtrMotor4.Enabled = true;
VelocidadM4.Enabled = true;

}
}

private void TiempoMotores_TextChanged(object sender, EventArgs e)
{

}

private void label7_Click(object sender, EventArgs e)
{

}

private void TiempoSeg_ValueChanged(object sender, EventArgs e)
{
tmax = Convert.ToInt32(TiempoSeg.Value);

}



}
}

Re: Controlling 4 motors with AdvancedServo

Posted: Tue Jul 24, 2012 8:05 am
by erik
Hello,

I might have missed it, but it doesn't look like you are turning on the servos. Add this to your initAllValues() function:

Code: Select all

advServo.servos[i].Engaged = true;

Re: Controlling 4 motors with AdvancedServo

Posted: Tue Jul 24, 2012 9:14 am
by viccos
Awesome Erick!!!!

That was the problem I just had!!!!

At some point, while I was modifiying the advancedservo full , I erased that line....

Thanks a lot....