public RobotPos(int x, int y, int angle, RobotStatusFlag statusFlag)
{
_x = x;
_y = y;
_angle = angle % 360;
_statusFlag.HoopStatus = statusFlag.HoopStatus;
_statusFlag.LeftArmStatus = statusFlag.LeftArmStatus;
_statusFlag.RightArmStatus = statusFlag.RightArmStatus;
}
public RobotStatusFlag(RobotStatusFlag newValue)
{
if(newValue != null)
{
HoopStatus = newValue.HoopStatus;
LeftArmStatus = newValue.LeftArmStatus;
RightArmStatus = newValue.RightArmStatus;
}
else
{
_hoopStatus = EnumRobotStatusFlag.Closed;
_leftArmStatus = EnumRobotStatusFlag.Closed;
_rightArmStatus = EnumRobotStatusFlag.Closed;
}
}
public RobotStatusFlag(RobotStatusFlag newValue)
{
if (newValue != null)
{
HoopStatus = newValue.HoopStatus;
LeftArmStatus = newValue.LeftArmStatus;
RightArmStatus = newValue.RightArmStatus;
}
else
{
_hoopStatus = EnumRobotStatusFlag.Closed;
_leftArmStatus = EnumRobotStatusFlag.Closed;
_rightArmStatus = EnumRobotStatusFlag.Closed;
}
}
public List<RobotPos> GetOutputPos()
{
List<RobotPos> outputPos = new List<RobotPos>();
List<RobotPos> Ret = new List<RobotPos>();
int transX = 0;
int transY = 0;
int transA = 0;
if(_robotAction != null)
{
switch (_robotAction.cmd)
{
// _______________________________________________________
case EnumCmd.Mvt_UseMixedMode:
case EnumCmd.Mvt_UseSpline:
case EnumCmd.App_SetNewPos:
if ((int.TryParse(_robotAction.param2, out transX) == true) && (int.TryParse(_robotAction.param3, out transY) == true) && (int.TryParse(_robotAction.param4, out transA) == true))
{
if (_initialPos.Count() > 0)
{
foreach (RobotPos currentRobotPos in _initialPos)
{
outputPos.Add(new RobotPos(transX, transY, transA, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(transX, transY, transA, new RobotStatusFlag()));
}
}
else
{
outputPos.Add(new RobotPos(1500, 1000, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
case EnumCmd.Mvt_UseAngleOnly:
case EnumCmd.MvtSimple_RotateToAngleInDeg:
if (int.TryParse(_robotAction.param4, out transA) == true)
{
foreach (RobotPos currentRobotPos in _initialPos)
{
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, transA, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(0, 0, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
case EnumCmd.MvtSimple_RotateInDeg:
if (int.TryParse(_robotAction.param4, out transA) == true)
{
foreach (RobotPos currentRobotPos in _initialPos)
{
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, currentRobotPos.angle + transA, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(0, 0, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
case EnumCmd.Mvt_UseDistOnly:
if ((int.TryParse(_robotAction.param2, out transX) == true) && (int.TryParse(_robotAction.param3, out transY) == true))
{
foreach (RobotPos currentRobotPos in _initialPos)
{
outputPos.Add(new RobotPos(transX, transY, currentRobotPos.angle, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(0, 0, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
case EnumCmd.Mvt_UsePivotMode:
if (int.TryParse(_robotAction.param4, out transA) == true)
{
foreach (RobotPos currentRobotPos in _initialPos)
{
int XWheel = 0; // X Pos for the locked wheel
int YWheel = 0; // Y Pos for the locked wheel
int Dist = 135; // Distance between center and locked Wheel
int ExpectedAngle = transA;
int _ComputedPosX, _ComputedPosY, _ComputedPosAngle;
if (_robotAction.param2 == "LEFT_WHEEL")
{
// Compute the position of the locked wheel
XWheel = Convert.ToInt32(currentRobotPos.x - Dist * Math.Sin(currentRobotPos.angle * Math.PI / 180.0));
YWheel = Convert.ToInt32(currentRobotPos.y + Dist * Math.Cos(currentRobotPos.angle * Math.PI / 180.0));
// Compute the robot center after this movement
_ComputedPosX = Convert.ToInt32(XWheel + Dist * Math.Sin(ExpectedAngle * Math.PI / 180.0));
_ComputedPosY = Convert.ToInt32(YWheel - Dist * Math.Cos(ExpectedAngle * Math.PI / 180.0));
_ComputedPosAngle = ExpectedAngle;
}
else
{
// Compute the position of the locked wheel
XWheel = Convert.ToInt32(currentRobotPos.x + Dist * Math.Sin(currentRobotPos.angle * Math.PI / 180.0));
YWheel = Convert.ToInt32(currentRobotPos.y - Dist * Math.Cos(currentRobotPos.angle * Math.PI / 180.0));
// Compute the robot center after this movement
_ComputedPosX = Convert.ToInt32(XWheel - Dist * Math.Sin(ExpectedAngle * Math.PI / 180.0));
_ComputedPosY = Convert.ToInt32(YWheel + Dist * Math.Cos(ExpectedAngle * Math.PI / 180.0));
_ComputedPosAngle = ExpectedAngle;
}
outputPos.Add(new RobotPos(_ComputedPosX, _ComputedPosY, _ComputedPosAngle, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(0, 0, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
case EnumCmd.MvtSimple_MoveInMM:
if (int.TryParse(_robotAction.param2, out transX) == true)
{
foreach (RobotPos currentRobotPos in _initialPos)
{
int _ComputedPosX = currentRobotPos.x + Convert.ToInt32(Convert.ToDouble(transX) * Math.Cos(currentRobotPos.angle * Math.PI / 180.0));
int _ComputedPosY = currentRobotPos.y + Convert.ToInt32(Convert.ToDouble(transX) * Math.Sin(currentRobotPos.angle * Math.PI / 180.0)); ;
int _ComputedPosAngle = currentRobotPos.angle;
outputPos.Add(new RobotPos(_ComputedPosX, _ComputedPosY, _ComputedPosAngle, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(0, 0, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
// Pas de modification sur la position du robot
case EnumCmd.Sensors_SetHoopLevel:
foreach (RobotPos currentRobotPos in _initialPos)
{
RobotStatusFlag newStatus = new RobotStatusFlag(currentRobotPos.statusFlag);
if (_robotAction.param1 == EnumSensorsHoopLevel.HOOP_LEVEL_UP.ToString())
{
newStatus.HoopStatus = EnumRobotStatusFlag.Closed;
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, currentRobotPos.angle, newStatus));
}
else
{
newStatus.HoopStatus = EnumRobotStatusFlag.Open;
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, currentRobotPos.angle, newStatus));
}
}
break;
// _______________________________________________________
// Pas de modification sur la position du robot
case EnumCmd.Sensors_SetArmsStatus:
foreach (RobotPos currentRobotPos in _initialPos)
{
RobotStatusFlag newStatus = new RobotStatusFlag(currentRobotPos.statusFlag);
if (_robotAction.param1 == "ARM_OPEN")
newStatus.RightArmStatus = EnumRobotStatusFlag.Open;
else if (_robotAction.param1 == "ARM_FRONT")
newStatus.RightArmStatus = EnumRobotStatusFlag.Front;
else
newStatus.RightArmStatus = EnumRobotStatusFlag.Closed;
if (_robotAction.param2 == "ARM_OPEN")
newStatus.LeftArmStatus = EnumRobotStatusFlag.Open;
else if (_robotAction.param2 == "ARM_FRONT")
newStatus.LeftArmStatus = EnumRobotStatusFlag.Front;
else
newStatus.LeftArmStatus = EnumRobotStatusFlag.Closed;
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, currentRobotPos.angle, newStatus));
}
break;
// _______________________________________________________
// Pas de modification
case EnumCmd.App_IfGoto_Strategy:
case EnumCmd.App_IfGoto_System:
case EnumCmd.App_SetStrategyFlags:
case EnumCmd.App_Wait:
case EnumCmd.Mvt_Stop:
case EnumCmd.NotSet:
default:
foreach (RobotPos currentRobotPos in _initialPos)
{
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, currentRobotPos.angle, currentRobotPos.statusFlag));
}
break;
}
}
// Verification des données de sortie
Boolean isAlreadyAdded = false;
// Pour chaque élément en sortie
foreach (RobotPos robotPosToChecked in outputPos)
{
isAlreadyAdded = false;
// On verifie s'il n'est pas déjà présent
foreach (RobotPos currentRobotPos in Ret)
{
if ((currentRobotPos.x == robotPosToChecked.x) && (currentRobotPos.y == robotPosToChecked.y) && (currentRobotPos.angle == robotPosToChecked.angle) && (currentRobotPos.statusFlag.GetValueString() == robotPosToChecked.statusFlag.GetValueString()))
isAlreadyAdded = true;
}
if (isAlreadyAdded == false)
Ret.Add(robotPosToChecked);
}
return Ret;
}
public List <RobotPos> GetOutputPos()
{
List <RobotPos> outputPos = new List <RobotPos>();
List <RobotPos> Ret = new List <RobotPos>();
int transX = 0;
int transY = 0;
int transA = 0;
if (_robotAction != null)
{
switch (_robotAction.cmd)
{
// _______________________________________________________
case EnumCmd.Mvt_UseMixedMode:
case EnumCmd.Mvt_UseSpline:
case EnumCmd.App_SetNewPos:
if ((int.TryParse(_robotAction.param2, out transX) == true) && (int.TryParse(_robotAction.param3, out transY) == true) && (int.TryParse(_robotAction.param4, out transA) == true))
{
if (_initialPos.Count() > 0)
{
foreach (RobotPos currentRobotPos in _initialPos)
{
outputPos.Add(new RobotPos(transX, transY, transA, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(transX, transY, transA, new RobotStatusFlag()));
}
}
else
{
outputPos.Add(new RobotPos(1500, 1000, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
case EnumCmd.Mvt_UseAngleOnly:
case EnumCmd.MvtSimple_RotateToAngleInDeg:
if (int.TryParse(_robotAction.param4, out transA) == true)
{
foreach (RobotPos currentRobotPos in _initialPos)
{
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, transA, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(0, 0, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
case EnumCmd.MvtSimple_RotateInDeg:
if (int.TryParse(_robotAction.param4, out transA) == true)
{
foreach (RobotPos currentRobotPos in _initialPos)
{
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, currentRobotPos.angle + transA, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(0, 0, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
case EnumCmd.Mvt_UseDistOnly:
if ((int.TryParse(_robotAction.param2, out transX) == true) && (int.TryParse(_robotAction.param3, out transY) == true))
{
foreach (RobotPos currentRobotPos in _initialPos)
{
outputPos.Add(new RobotPos(transX, transY, currentRobotPos.angle, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(0, 0, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
case EnumCmd.Mvt_UsePivotMode:
if (int.TryParse(_robotAction.param4, out transA) == true)
{
foreach (RobotPos currentRobotPos in _initialPos)
{
int XWheel = 0; // X Pos for the locked wheel
int YWheel = 0; // Y Pos for the locked wheel
int Dist = 135; // Distance between center and locked Wheel
int ExpectedAngle = transA;
int _ComputedPosX, _ComputedPosY, _ComputedPosAngle;
if (_robotAction.param2 == "LEFT_WHEEL")
{
// Compute the position of the locked wheel
XWheel = Convert.ToInt32(currentRobotPos.x - Dist * Math.Sin(currentRobotPos.angle * Math.PI / 180.0));
YWheel = Convert.ToInt32(currentRobotPos.y + Dist * Math.Cos(currentRobotPos.angle * Math.PI / 180.0));
// Compute the robot center after this movement
_ComputedPosX = Convert.ToInt32(XWheel + Dist * Math.Sin(ExpectedAngle * Math.PI / 180.0));
_ComputedPosY = Convert.ToInt32(YWheel - Dist * Math.Cos(ExpectedAngle * Math.PI / 180.0));
_ComputedPosAngle = ExpectedAngle;
}
else
{
// Compute the position of the locked wheel
XWheel = Convert.ToInt32(currentRobotPos.x + Dist * Math.Sin(currentRobotPos.angle * Math.PI / 180.0));
YWheel = Convert.ToInt32(currentRobotPos.y - Dist * Math.Cos(currentRobotPos.angle * Math.PI / 180.0));
// Compute the robot center after this movement
_ComputedPosX = Convert.ToInt32(XWheel - Dist * Math.Sin(ExpectedAngle * Math.PI / 180.0));
_ComputedPosY = Convert.ToInt32(YWheel + Dist * Math.Cos(ExpectedAngle * Math.PI / 180.0));
_ComputedPosAngle = ExpectedAngle;
}
outputPos.Add(new RobotPos(_ComputedPosX, _ComputedPosY, _ComputedPosAngle, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(0, 0, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
case EnumCmd.MvtSimple_MoveInMM:
if (int.TryParse(_robotAction.param2, out transX) == true)
{
foreach (RobotPos currentRobotPos in _initialPos)
{
int _ComputedPosX = currentRobotPos.x + Convert.ToInt32(Convert.ToDouble(transX) * Math.Cos(currentRobotPos.angle * Math.PI / 180.0));
int _ComputedPosY = currentRobotPos.y + Convert.ToInt32(Convert.ToDouble(transX) * Math.Sin(currentRobotPos.angle * Math.PI / 180.0));;
int _ComputedPosAngle = currentRobotPos.angle;
outputPos.Add(new RobotPos(_ComputedPosX, _ComputedPosY, _ComputedPosAngle, currentRobotPos.statusFlag));
}
}
else
{
outputPos.Add(new RobotPos(0, 0, 0, new RobotStatusFlag()));
}
break;
// _______________________________________________________
// Pas de modification sur la position du robot
case EnumCmd.Sensors_SetHoopLevel:
foreach (RobotPos currentRobotPos in _initialPos)
{
RobotStatusFlag newStatus = new RobotStatusFlag(currentRobotPos.statusFlag);
if (_robotAction.param1 == EnumSensorsHoopLevel.HOOP_LEVEL_UP.ToString())
{
newStatus.HoopStatus = EnumRobotStatusFlag.Closed;
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, currentRobotPos.angle, newStatus));
}
else
{
newStatus.HoopStatus = EnumRobotStatusFlag.Open;
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, currentRobotPos.angle, newStatus));
}
}
break;
// _______________________________________________________
// Pas de modification sur la position du robot
case EnumCmd.Sensors_SetArmsStatus:
foreach (RobotPos currentRobotPos in _initialPos)
{
RobotStatusFlag newStatus = new RobotStatusFlag(currentRobotPos.statusFlag);
if (_robotAction.param1 == "ARM_OPEN")
{
newStatus.RightArmStatus = EnumRobotStatusFlag.Open;
}
else if (_robotAction.param1 == "ARM_FRONT")
{
newStatus.RightArmStatus = EnumRobotStatusFlag.Front;
}
else
{
newStatus.RightArmStatus = EnumRobotStatusFlag.Closed;
}
if (_robotAction.param2 == "ARM_OPEN")
{
newStatus.LeftArmStatus = EnumRobotStatusFlag.Open;
}
else if (_robotAction.param2 == "ARM_FRONT")
{
newStatus.LeftArmStatus = EnumRobotStatusFlag.Front;
}
else
{
newStatus.LeftArmStatus = EnumRobotStatusFlag.Closed;
}
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, currentRobotPos.angle, newStatus));
}
break;
// _______________________________________________________
// Pas de modification
case EnumCmd.App_IfGoto_Strategy:
case EnumCmd.App_IfGoto_System:
case EnumCmd.App_SetStrategyFlags:
case EnumCmd.App_Wait:
case EnumCmd.Mvt_Stop:
case EnumCmd.NotSet:
default:
foreach (RobotPos currentRobotPos in _initialPos)
{
outputPos.Add(new RobotPos(currentRobotPos.x, currentRobotPos.y, currentRobotPos.angle, currentRobotPos.statusFlag));
}
break;
}
}
// Verification des données de sortie
Boolean isAlreadyAdded = false;
// Pour chaque élément en sortie
foreach (RobotPos robotPosToChecked in outputPos)
{
isAlreadyAdded = false;
// On verifie s'il n'est pas déjà présent
foreach (RobotPos currentRobotPos in Ret)
{
if ((currentRobotPos.x == robotPosToChecked.x) && (currentRobotPos.y == robotPosToChecked.y) && (currentRobotPos.angle == robotPosToChecked.angle) && (currentRobotPos.statusFlag.GetValueString() == robotPosToChecked.statusFlag.GetValueString()))
{
isAlreadyAdded = true;
}
}
if (isAlreadyAdded == false)
{
Ret.Add(robotPosToChecked);
}
}
return(Ret);
}
