public RobotState(int robotID, RobotMode mode, RobotSubMode subMode, RobotPose pose, RobotPose rpose,
bool poseWatchdogFail, bool plannerFail, bool inObstacle, bool wpInObstacle, bool isLockedOn,
RobotNeutralizeStatus neutralizeStatus, List <Vector2> waypoints, double angleWhenDone,
IPath path, IPath sparsePath, RobotTwoWheelCommand cmd, DateTime cmdTime, int waypointsCompleted)
{
RobotID = robotID;
Mode = mode;
SubMode = subMode;
Pose = pose.DeepCopy();
rPose = rpose.DeepCopy();
PoseWatchdogFail = poseWatchdogFail;
PlannerFail = plannerFail;
InObstacle = inObstacle;
WpInObstacle = wpInObstacle;
IsLockedOn = isLockedOn;
NeutralizeStatus = neutralizeStatus;
Waypoints = new List <Vector2>(waypoints);
AngleWhenDone = angleWhenDone;
TrackedPath = path.Clone();
SparsePath = sparsePath.Clone();
Cmd = new RobotTwoWheelCommand(cmd.velocity, cmd.turn);
CmdTime = new DateTime(cmdTime.Ticks);
WaypointsCompleted = waypointsCompleted;
}
public RobotState(int robotID, RobotMode mode, RobotSubMode subMode, RobotPose pose, RobotPose rpose,
bool poseWatchdogFail, bool plannerFail, bool inObstacle, bool wpInObstacle, bool isLockedOn,
RobotNeutralizeStatus neutralizeStatus, List<Vector2> waypoints, double angleWhenDone,
IPath path, IPath sparsePath, RobotTwoWheelCommand cmd, DateTime cmdTime, int waypointsCompleted)
{
RobotID = robotID;
Mode = mode;
SubMode = subMode;
Pose = pose.DeepCopy();
rPose = rpose.DeepCopy();
PoseWatchdogFail = poseWatchdogFail;
PlannerFail = plannerFail;
InObstacle = inObstacle;
WpInObstacle = wpInObstacle;
IsLockedOn = isLockedOn;
NeutralizeStatus = neutralizeStatus;
Waypoints = new List<Vector2>(waypoints);
AngleWhenDone = angleWhenDone;
TrackedPath = path.Clone();
SparsePath = sparsePath.Clone();
Cmd = new RobotTwoWheelCommand(cmd.velocity, cmd.turn);
CmdTime = new DateTime(cmdTime.Ticks);
WaypointsCompleted = waypointsCompleted;
}