/// <summary>
/// Turns a servo off
/// </summary>
/// <param name="Channel">Servo number 0 - 5</param>
public void Off
(
ServoChannels Channel
)
{
SetPosition(Channel, 0);
}
/// <summary>
/// Gets the position of a servo
/// WARNING: not valid while servo is still moving
/// </summary>
/// <param name="Channel">Servo number 0 - 5</param>
/// <returns>Current position in microseconds</returns>
public int GetPosition
(
ServoChannels Channel
)
{
if (Port == null)
{
throw new Exception("Not connected to servo controller");
}
byte[] Command = new byte[4];
Command[0] = 0xAA;
Command[1] = MaestroId;
Command[2] = (byte)Commands.GetPosition;
Command[3] = (byte)((byte)Channel & 0x7F);
Port.Write(Command, 0, 4);
byte[] Response = new byte[2];
Port.Read(Response, 0, 2);
UInt16 Position = (UInt16)(((UInt16)Response[1] << 8) | Response[0]);
return(Position / 4);
}
/// <summary>
/// Checks if a switch is pressed
/// </summary>
/// <param name="Channel">Channel that switch is on</param>
/// <returns>true if switch is pressed</returns>
public bool SwitchPressed
(
ServoChannels Channel
)
{
if (GetPosition(Channel) < 10)
{
return(true);
}
return(false);
}
/// <summary>
/// Sets the servo to a specific position and waits for the servo to
/// reach the position
/// </summary>
/// <param name="Channel">Servo number 0 - 5</param>
/// <param name="Position">Position in microseconds</param>
/// <param name="Timeout">Maximum time to wait in milliseconds</param>
public void SetPositionandWait
(
ServoChannels Channel,
int Position,
int Timeout
)
{
Stopwatch Watch = new Stopwatch();
SetPosition(Channel, Position);
// start timing
Watch.Start();
//Console.WriteLine(String.Format("{0} {1} - waiting for motion to start", Watch.ElapsedMilliseconds, Channel));
/*// wait for motion to start
* while (!ServosMoving())
* {
* if (Watch.ElapsedMilliseconds > Timeout)
* {
* return;
* //throw new Exception(String.Format("Servo {0} failed to start moving in {2} ms", Channel, Position, Timeout));
* }
* }*/
//Console.WriteLine(String.Format("{0} {1} - waiting for motion to stop", Watch.ElapsedMilliseconds, Channel));
//while (ServosMoving())
while (GetPosition(Channel) != Position)
{
if (Watch.ElapsedMilliseconds > Timeout)
{
if (Math.Abs(GetPosition(Channel) - Position) < 10)
{
return;
}
//Console.WriteLine(String.Format("Servo {0} failed to reach position {1} in {2} ms - now at {3}", Channel, Position, Timeout, GetPosition(Channel)));
throw new Exception(String.Format("Servo {0} failed to reach position {1} in {2} ms - now at {3}", Channel, Position, Timeout, GetPosition(Channel)));
}
}
//Console.WriteLine(String.Format("{0} {1} - motion stopped", Watch.ElapsedMilliseconds, Channel));
}
/// <summary>
/// Sets the acceleration of a servo
/// </summary>
/// <param name="Channel">Servo number 0 - 5</param>
/// <param name="Speed">New acceleration, 0 = unlimited</param>
public void SetAcceleration
(
ServoChannels Channel,
int Acceleration
)
{
if (Port == null)
{
throw new Exception("Not connected to servo controller");
}
byte[] Command = new byte[6];
Command[0] = 0xAA;
Command[1] = MaestroId;
Command[2] = (byte)Commands.SetAcceleration;
Command[3] = (byte)((byte)Channel & 0x7F);
Command[4] = (byte)(Acceleration & 0x7F);
Command[5] = (byte)((Acceleration >> 7) & 0x7F);
Port.Write(Command, 0, 6);
}
/// <summary>
/// Sets the position of a servo
/// Does not wait for the servo to reach the position - returns immediately
/// </summary>
/// <param name="Channel">Servo number 0 - 5</param>
/// <param name="Position">Position in microseconds</param>
public void SetPosition
(
ServoChannels Channel,
int Position
)
{
if (Port == null)
{
throw new Exception("Not connected to servo controller");
}
// convert to quarter-microseconds
UInt16 Pos = (UInt16)(Position * 4);
byte[] Command = new byte[6];
Command[0] = 0xAA;
Command[1] = MaestroId;
Command[2] = (byte)Commands.SetPosition;
Command[3] = (byte)((byte)Channel & 0x7F);
Command[4] = (byte)(Pos & 0x7F);
Command[5] = (byte)((Pos >> 7) & 0x7F);
Port.Write(Command, 0, 6);
}
/// <summary>
/// Gets the position of a servo
/// WARNING: not valid while servo is still moving
/// </summary>
/// <param name="Channel">Servo number 0 - 5</param>
/// <returns>Current position in microseconds</returns>
public int GetPosition(
ServoChannels Channel
)
{
if (Port == null) throw new Exception("Not connected to servo controller");
byte[] Command = new byte[4];
Command[0] = 0xAA;
Command[1] = MaestroId;
Command[2] = (byte)Commands.GetPosition;
Command[3] = (byte)((byte)Channel & 0x7F);
Port.Write(Command, 0, 4);
byte[] Response = new byte[2];
Port.Read(Response, 0, 2);
UInt16 Position = (UInt16)(((UInt16)Response[1] << 8) | Response[0]);
return Position / 4;
}
/// <summary>
/// Checks if a switch is pressed
/// </summary>
/// <param name="Channel">Channel that switch is on</param>
/// <returns>true if switch is pressed</returns>
public bool SwitchPressed(
ServoChannels Channel
)
{
if (GetPosition(Channel) < 10) return true;
return false;
}
/// <summary>
/// Sets the speed of a servo
/// </summary>
/// <param name="Channel">Servo number 0 - 5</param>
/// <param name="Speed">New speed, 0 = unlimited</param>
public void SetSpeed(
ServoChannels Channel,
int Speed
)
{
if (Port == null) throw new Exception("Not connected to servo controller");
byte[] Command = new byte[6];
Command[0] = 0xAA;
Command[1] = MaestroId;
Command[2] = (byte)Commands.SetSpeed;
Command[3] = (byte)((byte)Channel & 0x7F);
Command[4] = (byte)(Speed & 0x7F);
Command[5] = (byte)((Speed >> 7) & 0x7F);
Port.Write(Command, 0, 6);
}
/// <summary>
/// Sets the servo to a specific position and waits for the servo to
/// reach the position
/// </summary>
/// <param name="Channel">Servo number 0 - 5</param>
/// <param name="Position">Position in microseconds</param>
/// <param name="Timeout">Maximum time to wait in milliseconds</param>
public void SetPositionandWait(
ServoChannels Channel,
int Position,
int Timeout
)
{
Stopwatch Watch = new Stopwatch();
SetPosition(Channel, Position);
// start timing
Watch.Start();
//Console.WriteLine(String.Format("{0} {1} - waiting for motion to start", Watch.ElapsedMilliseconds, Channel));
/*// wait for motion to start
while (!ServosMoving())
{
if (Watch.ElapsedMilliseconds > Timeout)
{
return;
//throw new Exception(String.Format("Servo {0} failed to start moving in {2} ms", Channel, Position, Timeout));
}
}*/
//Console.WriteLine(String.Format("{0} {1} - waiting for motion to stop", Watch.ElapsedMilliseconds, Channel));
//while (ServosMoving())
while (GetPosition(Channel) != Position)
{
if (Watch.ElapsedMilliseconds > Timeout)
{
if (Math.Abs(GetPosition(Channel) - Position) < 10) return;
//Console.WriteLine(String.Format("Servo {0} failed to reach position {1} in {2} ms - now at {3}", Channel, Position, Timeout, GetPosition(Channel)));
throw new Exception(String.Format("Servo {0} failed to reach position {1} in {2} ms - now at {3}", Channel, Position, Timeout, GetPosition(Channel)));
}
}
//Console.WriteLine(String.Format("{0} {1} - motion stopped", Watch.ElapsedMilliseconds, Channel));
}
/// <summary>
/// Sets the position of a servo
/// Does not wait for the servo to reach the position - returns immediately
/// </summary>
/// <param name="Channel">Servo number 0 - 5</param>
/// <param name="Position">Position in microseconds</param>
public void SetPosition(
ServoChannels Channel,
int Position
)
{
if (Port == null) throw new Exception("Not connected to servo controller");
// convert to quarter-microseconds
UInt16 Pos = (UInt16)(Position * 4);
byte[] Command = new byte[6];
Command[0] = 0xAA;
Command[1] = MaestroId;
Command[2] = (byte)Commands.SetPosition;
Command[3] = (byte)((byte)Channel & 0x7F);
Command[4] = (byte)(Pos & 0x7F);
Command[5] = (byte)((Pos >> 7) & 0x7F);
Port.Write(Command, 0, 6);
}
/// <summary>
/// Turns a servo off
/// </summary>
/// <param name="Channel">Servo number 0 - 5</param>
public void Off(
ServoChannels Channel
)
{
SetPosition(Channel, 0);
}
