/// <summary>
/// Constructor for rod action to be performed
/// </summary>
/// <param name="type">Current Rod Type</param>
/// <param name="rotation">Rotational move to be performed (default is undefined)</param>
/// <param name="linear">Linear move to be performed (default is undefined)</param>
public RodAction(eRod type, eRotationalMove rotation = eRotationalMove.NA, eLinearMove linear = eLinearMove.NA)
{
_dcPosition = 0;
RodType = type;
Rotation = rotation;
Linear = linear;
}
/// <summary>
/// Move arduino DC and Servo
/// </summary>
/// <param name="dc">DC movement (-1) for no movement</param>
/// <param name="servo">Servo Position</param>
/// <exception cref="InitializationException">Thrown in case arduino was not initialized</exception>
/// <exception cref="InvalidOperationException">Thrown in of wrong DC parameter</exception>
public void Move(int dc = -1, eRotationalMove servo = eRotationalMove.NA)
{
if (!_isInitialized)
{
throw new InitializationException(String.Format(
"[{0}] Unable to move arduino because arduino is not initialized.",
MethodBase.GetCurrentMethod().Name));
}
if (dc < -1 || dc > MaxTicks)
{
throw new InvalidOperationException(String.Format(
"[{0}] Unable to move arduino because received DC movement: [{1}] is not in range of rod: [{2} to {3}]",
MethodBase.GetCurrentMethod().Name, dc, 0, MaxTicks));
}
if (_lastServo != servo || _lastDc != dc)
{
if (watch == null || watch.ElapsedMilliseconds > Communication.SLEEP)
{
Log.Print(String.Format("{0} DC: {1} SERVO: {2}", _comPortName, dc, servo.ToString()), eCategory.Info, LogTag.COMMUNICATION);
watch = Stopwatch.StartNew();
byte command = _encoder.Encode(dc, servo);
_comPort.Write(command);
_lastServo = servo;
_lastDc = dc;
}
}
}
/// <summary>
/// Constructor for rod action to be performed
/// </summary>
/// <param name="type">Current Rod Type</param>
/// <param name="rotation">Rotational move to be performed (default is undefined)</param>
/// <param name="linear">Linear move to be performed (default is undefined)</param>
public RodAction(eRod type, eRotationalMove rotation = eRotationalMove.NA, eLinearMove linear = eLinearMove.NA)
{
_dcPosition = 0;
RodType = type;
Rotation = rotation;
Linear = linear;
}
/// <summary>
/// Constructor for rod state
/// </summary>
/// <param name="dcLowLimit">Minimal Position of DC in mm</param>
/// <param name="dcHighLimit">Maximal Position of DC in mm</param>
/// <param name="initialDcPos">Initial DC position [default is 0]</param>
/// <param name="initialServoPos">Initial Servo position [default is DEFENCE]</param>
public RodState(int dcLowLimit, int dcHighLimit, int initialDcPos = 0, eRotationalMove initialServoPos = eRotationalMove.DEFENCE)
{
DC_LOW_LIMIT = dcLowLimit;
DC_HIGH_LIMIT = dcHighLimit;
_dcPosition = (initialDcPos < dcLowLimit) ? dcLowLimit : initialDcPos;
_servoPosition = initialServoPos;
}
/// <summary>
/// Constructor for rod state
/// </summary>
/// <param name="dcLowLimit">Minimal Position of DC in mm</param>
/// <param name="dcHighLimit">Maximal Position of DC in mm</param>
/// <param name="initialDcPos">Initial DC position [default is 0]</param>
/// <param name="initialServoPos">Initial Servo position [default is DEFENCE]</param>
public RodState(int dcLowLimit, int dcHighLimit, int initialDcPos = 0, eRotationalMove initialServoPos = eRotationalMove.DEFENCE)
{
DC_LOW_LIMIT = dcLowLimit;
DC_HIGH_LIMIT = dcHighLimit;
_dcPosition = (initialDcPos < dcLowLimit) ? dcLowLimit : initialDcPos;
_servoPosition = initialServoPos;
}
//public static void DrawRodPlayers(eRod rod, int linearMoveDestination, eRotationalMove rotationalMove, bool isLocation = true)
//{
// try
// {
// eMarks playersBase = 0;
// eMarks mark;
// Enum.TryParse<eMarks>(rod.ToString(), out mark);
// int eMarkType = ((int)mark);
// int rodPlayersCount = _rodPlayerCount[mark];
// int yDistance = _rodsPlayersDistance[mark];
// int firstPlayerOffsetY = _rodsOffsetY[mark];
// int rodStartX = XTableToDeviceCoordinates(_rods[mark]);
// switch (eMarkType)
// {
// case (int)eMarks.GoalKeeper: playersBase = eMarks.GoalKeeperPlayer; break;
// case (int)eMarks.Defence: playersBase = eMarks.DefencePlayer1; break;
// case (int)eMarks.Midfield: playersBase = eMarks.MidfieldPlayer1; break;
// case (int)eMarks.Attack: playersBase = eMarks.AttackPlayer1; break;
// default: break;
// }
// for (int rodPlayer = 0; rodPlayer < rodPlayersCount; rodPlayer++)
// {
// int movmentOffset = 0;
// int x = rodStartX;
// int y = 0;
// int y1 = 0;
// if (isLocation)
// {
// ConvertToLocation(ref x, ref y1);
// }
// y = y1 + linearMoveDestination + firstPlayerOffsetY + yDistance * rodPlayer;
// DrawPlayer(playersBase + rodPlayer,
// new Point(x * _actualWidthRate, y * _actualHeightRate), 12, rotationalMove);
// }
// }
// catch (Exception e)
// {
// Log.Common.Error(String.Format("[{0}] Failed to draw rods players mark. Reason: {1}",
// MethodBase.GetCurrentMethod().Name, e.Message));
// }
//}
/// <summary>
/// Drawing a single player at a time , used by the DrawRodPlayers method to draw all rods players
/// </summary>
/// <param name="eNumKey">eMark of the wanted rod : GOALKEEPER , Defence , Midfield, Attack</param>
/// <param name="center">Center of the players radius</param>
/// <param name="radius">Radius size</param>
/// <param name="circleColor">Color for the roational current pos</param>
private static void DrawPlayer(eMarks eNumKey, Point center, int radius,
eRotationalMove rotationalMove, SolidColorBrush playerColor = null)
{
try
{
object token = null;
if (_positionToken.ContainsKey(eNumKey))
{
token = _positionToken[eNumKey];
}
if (token != null)
{
lock (token)
{
_playerPositions[eNumKey] = center;
}
}
int key = (int)eNumKey;
int rotationalMoveFactor = 0;
SolidColorBrush rotationalColor = Brushes.DarkBlue;
if (rotationalMove == eRotationalMove.DEFENCE)
{
rotationalMoveFactor = radius;
rotationalColor = Brushes.Blue;
}
else if (rotationalMove == eRotationalMove.RISE)
{
rotationalMoveFactor = (int)(radius * 2.5);
rotationalColor = Brushes.Cyan;
}
_dispatcher.Invoke(new ThreadStart(delegate
{
(_markups[key] as Shape).Fill = (rotationalColor == null) ? Brushes.White : rotationalColor;
_markups[key].Width = radius * 2;
_markups[key].Height = radius * 2;
Canvas.SetLeft(_markups[key], center.X - radius);
Canvas.SetTop(_markups[key], center.Y - radius);
(_markups[key + 5] as Shape).Height = 24;
(_markups[key + 5] as Shape).Width = 30;
(_markups[key + 5] as Shape).StrokeThickness = 2;
(_markups[key + 5] as Shape).Fill = rotationalColor;
Canvas.SetLeft(_markups[key + 5], center.X - rotationalMoveFactor);
Canvas.SetTop(_markups[key + 5], center.Y - radius);
}));
}
catch (Exception e)
{
Log.Print(String.Format("Failed to draw player mark. Reason: {0}", e.Message), eCategory.Error, LogTag.COMMON);
}
}
public void ServoPosition_Positive()
{
eRotationalMove expected = eRotationalMove.DEFENCE;
_rodState.ServoPosition = expected;
eRotationalMove actual = _rodState.ServoPosition;
Assert.AreEqual(expected, actual);
}
/// <summary>
/// Draw a rod by a given eMark sign , moving the rod on the linear and rotational axes
/// </summary>
/// <param name="rod">eMark of the wanted rod : GoalKeeper, Defence , Midfield, Attack</param>
/// <param name="deltaYMovment">The change on the linear movement</param>
/// <param name="rotationalMove">eRotationalMove of the rod : DEFENCE, RISE, ATTACK</param>
public static void DrawRodPlayers(eRod rod, int linearMoveDestination, eRotationalMove rotationalMove, bool isLocation = true)
{
try
{
eMarks playersBase = 0;
eMarks mark;
Enum.TryParse <eMarks>(rod.ToString(), out mark);
int eMarkType = ((int)mark);
int rodPlayersCount = _rodPlayerCount[mark];
int yDistance = _rodsPlayersDistance[mark];
int firstPlayerOffsetY = _rodsOffsetY[mark];
int x = XTableToDeviceCoordinates(_rods[mark]);
switch (eMarkType)
{
case (int)eMarks.GoalKeeper: playersBase = eMarks.GoalKeeperPlayer; break;
case (int)eMarks.Defence: playersBase = eMarks.DefencePlayer1; break;
case (int)eMarks.Midfield: playersBase = eMarks.MidfieldPlayer1; break;
case (int)eMarks.Attack: playersBase = eMarks.AttackPlayer1; break;
default: break;
}
for (int rodPlayer = 0; rodPlayer < rodPlayersCount; rodPlayer++)
{
int y = linearMoveDestination + firstPlayerOffsetY + yDistance * rodPlayer;
int y1 = y;
int x1 = x;
if (isLocation)
{
ConvertToLocation(ref x1, ref y);
}
DrawPlayer(playersBase + rodPlayer, new Point(x1 * _actualWidthRate, y1 * _actualHeightRate), 12, rotationalMove);
}
}
catch (Exception e)
{
Log.Print(String.Format("Failed to draw rods players mark. Reason: {0}", e.Message), eCategory.Error, LogTag.COMMON);
}
}
/// <summary>
/// Get actions coded in one byte to sent to Arduino
/// </summary>
/// <param name="dcInTicks">DC coordinate in ticks</param>
/// <param name="servo">Servo position</param>
/// <returns>Action as command in one byte</returns>
public byte Encode(int dcInTicks, eRotationalMove servo)
{
//Convert ticks to bits (0x00000000 (0) to 0x00111110 (62))
int dcBites = _converter.TicksToBits(dcInTicks);
//create empty command
byte command = 0x00000000;
/*
* Add Servo action as binary (YY) to empty command
* Possible Servo Actions:
* - 0x00 - NA
* - 0x01 - KICK
* - 0x10 - DEFENCE
* - 0x11 - RISE
* Command will look like: 0x000000YY
*/
command = (byte)(command | Convert.ToByte((int)servo));
/*
* Shift dc action in bites 2 bites left.
* For DC action 0xZZZZZZ shifted will be 0xZZZZZZ00
* Possible DC Actions:
* - 0x000000 - NA
* - 0x000001 - (1) minimal coordinate
* - 0x111110 - (62) maximal coordinate
*/
byte dcShifted = (byte)(Convert.ToByte(dcBites) << 2);
/*
* Combine Servo 0x000000YY and DC 0xZZZZZZ00
* and get command as 0xZZZZZZYY
*/
command = (byte)(command | dcShifted);
return(command);
}
/// <summary>
/// Get actions coded in one byte to sent to Arduino
/// </summary>
/// <param name="dcInTicks">DC coordinate in ticks</param>
/// <param name="servo">Servo position</param>
/// <returns>Action as command in one byte</returns>
public byte Encode(int dcInTicks, eRotationalMove servo)
{
//Convert ticks to bits (0x00000000 (0) to 0x00111110 (62))
int dcBites = Converter.TicksToBits(dcInTicks);
//create empty command
byte command = 0x00000000;
/*
* Add Servo action as binary (YY) to empty command
* Possible Servo Actions:
* - 0x00 - NA
* - 0x01 - KICK
* - 0x10 - DEFENCE
* - 0x11 - RISE
* Command will look like: 0x000000YY
*/
command = (byte)(command | Convert.ToByte((int)servo));
/*
* Shift dc action in bites 2 bites left.
* For DC action 0xZZZZZZ shifted will be 0xZZZZZZ00
* Possible DC Actions:
* - 0x000000 - NA
* - 0x000001 - (1) minimal coordinate
* - 0x111110 - (62) maximal coordinate
*/
byte dcShifted = (byte)(Convert.ToByte(dcBites) << 2);
/*
* Combine Servo 0x000000YY and DC 0xZZZZZZ00
* and get command as 0xZZZZZZYY
*/
command = (byte)(command | dcShifted);
return command;
}
/// <summary>
/// Draw a rod by a given eMark sign , moving the rod on the linear and rotational axes
/// </summary>
/// <param name="rod">eMark of the wanted rod : GoalKeeper, Defense , Midfield, Attack</param>
/// <param name="linearMoveDestination">Destination of a stopper in MM (Real World)</param>
/// <param name="rotationalMove">eRotationalMove of the rod : DEFENCE, RISE, ATTACK</param>
public void DrawRodPlayers(eRod rod, int linearMoveDestination, eRotationalMove rotationalMove)
{
//Get all player centers in a real world in current rod
int x = Data.GetRod(rod).XCoordinate;
int playersCount = Data.GetRod(rod).PlayersCount;
int firstPlayerOffset = Data.GetRod(rod).FirstPlayerOffsetY;
int playersDistance = Data.GetRod(rod).PlayersYDistance;
for (int i = 0; i<playersCount; i++)
{
eMarks playerMark = _state.RodTypeToMarkOfFirstPlayerType(rod) + i;
int y = firstPlayerOffset + i * playersDistance + linearMoveDestination;
Point center = new Point(x, y);
center = Data.TableToDeviceCoordinates(center);
DrawPlayer(playerMark, center, rotationalMove);
}
}
/// <summary>
/// Move arduino DC and Servo
/// </summary>
/// <param name="dc">DC movement (-1) for no movement</param>
/// <param name="servo">Servo Position</param>
/// <exception cref="InitializationException">Thrown in case arduino was not initialized</exception>
/// <exception cref="InvalidOperationException">Thrown in of wrong DC parameter</exception>
public void Move(int dc = -1, eRotationalMove servo = eRotationalMove.NA)
{
if (!_isInitialized)
throw new InitializationException(String.Format(
"[{0}] Unable to move arduino because arduino is not initialized.",
MethodBase.GetCurrentMethod().Name));
if (dc < -1 || dc > MaxTicks)
throw new InvalidOperationException(String.Format(
"[{0}] Unable to move arduino because received DC movement: [{1}] is not in range of rod: [{2} to {3}]",
MethodBase.GetCurrentMethod().Name, dc, 0, MaxTicks));
if (_lastServo != servo || _lastDc != dc)
{
byte command = _encoder.Encode(dc, servo);
_comPort.Write(command);
Log.Print(String.Format("[Local: {0}] DC: {1} SERVO: {2}", ComPortName, dc, servo.ToString()), eCategory.Info, LogTag.COMMUNICATION);
_lastDc = dc;
}
}
/// <summary>
/// Update last arduino servo state
/// </summary>
/// <param name="newState">Servo state as arduino response code</param>
public void SetLastServoState(eResponseCode newState)
{
eRotationalMove state = ResponseCodeToServoState(newState);
if (!state.Equals(eRotationalMove.NA))
{
_lastServo = state;
if (OnServoChangeState != null)
{
OnServoChangeState(RodType, _lastServo);
}
}
}
/// <summary>
/// Updates real time servo position
/// This method is used as delegate in Communication Layer
/// </summary>
/// <param name="rodType">Current Rod Type</param>
/// <param name="servoState">Current Servo Position</param>
public void UpdateRealTimeState(eRod rodType, eRotationalMove servoState)
{
IInitializableRod rod = _controlledRods.First(x => x.RodType.Equals(rodType));
rod.State.ServoPosition = servoState;
}
public RodAction(eRod type, eRotationalMove rotation, eLinearMove linear)
{
Type = type;
Rotation = rotation;
Linear = linear;
}
/// <summary>
/// Draw a rod by a given eMark sign , moving the rod on the linear and rotational axes
/// </summary>
/// <param name="rod">eMark of the wanted rod : GoalKeeper, Defense , Midfield, Attack</param>
/// <param name="linearMoveDestination">The change on the linear movement in Real World (MM)</param>
/// <param name="rotationalMove">eRotationalMove of the rod : DEFENCE, RISE, ATTACK</param>
public static void DrawRodPlayers(eRod rod, int linearMoveDestination, eRotationalMove rotationalMove)
{
try
{
Instance.DrawRodPlayers(rod, linearMoveDestination, rotationalMove);
}
catch (Exception ex)
{
Log.Print("Unable to draw rods players mark.", ex, LogTag.COMMON);
}
}
/// <summary>
/// Drawing a single player at a time , used by the DrawRodPlayers method to draw all rods players
/// </summary>
/// <param name="playerMark">eMark of the wanted rod : GoalKeeper, Defense , Midfield, Attack</param>
/// <param name="center">Center of the players radius in Foosbot WORLD</param>
/// <param name="radius">Radius size</param>
/// <param name="circleColor">Color for the rotational current position</param>
private void DrawPlayer(eMarks playerMark, Point center, eRotationalMove rotationalMove)
{
try
{
if (rotationalMove != eRotationalMove.NA)
{
//update position of player (only if first on rod)
_state.Set(playerMark, center);
//get coordinates in Frame dimesions
center = TransformAgent.Data.InvertTransform(center);
//get coordinates in Screen dimesions
center = _screen.FrameToScreenCoordinates(center);
//Select color based on Rotational Move
SolidColorBrush color = Brushes.Blue; //default for defense
int bodyOffset = 0;
switch(rotationalMove)
{
case eRotationalMove.KICK:
color = Brushes.DarkBlue;
bodyOffset = -PLAYER_RADIUS;
break;
case eRotationalMove.RISE:
color = Brushes.Cyan;
bodyOffset = Convert.ToInt32(PLAYER_RADIUS * 1.5);
break;
}
_dispatcher.Invoke(() =>
{
//Draw Player Body
_screen.Element<Shape>(playerMark+5).Fill = color;
_screen.Element<Shape>(playerMark + 5).Width = PLAYER_RADIUS * 2.5;
_screen.Element<Shape>(playerMark + 5).Height = PLAYER_RADIUS * 2;
_screen.Draw(playerMark + 5, center.X - PLAYER_RADIUS - bodyOffset, center.Y - PLAYER_RADIUS, eCoordinatesType.Screen, 10);
//Draw Player Head
_screen.Element<Shape>(playerMark).Fill = Brushes.DarkBlue;
_screen.Element<Shape>(playerMark).Stroke = Brushes.White;
_screen.Element<Shape>(playerMark).Width = PLAYER_RADIUS * 2;
_screen.Element<Shape>(playerMark).Height = PLAYER_RADIUS * 2;
_screen.Draw(playerMark, center.X - PLAYER_RADIUS, center.Y - PLAYER_RADIUS, eCoordinatesType.Screen, 20);
});
}
}
catch (Exception e)
{
Log.Print(String.Format("Unable to draw player mark. Reason: {0}", e.Message), eCategory.Warn, LogTag.COMMON);
}
}
