public RobotTwoWheelState GetCurrentState(RobotTwoWheelCommand currentCommand)
{
RobotWheelModel rightW = new RobotWheelModel(rWheelPos, rWheelVel, rWheelAccel);
RobotWheelModel leftW = new RobotWheelModel(lWheelPos, lWheelVel, lWheelAccel);
RobotTwoWheelState state = new RobotTwoWheelState(curPose, currentCommand, rightW, leftW);
return new RobotTwoWheelState(state, currentCommand);
}
public RobotTwoWheelState(RobotTwoWheelState toCopy, RobotTwoWheelCommand command)
{
//deep copy the class
this.pose = new RobotPose(toCopy.pose);
this.rightWheel = new RobotWheelModel(toCopy.rightWheel);
this.leftWheel = new RobotWheelModel(toCopy.leftWheel);
this.command = command;
}
public RobotWheelModel(RobotWheelModel toCopy)
{
this.accel = toCopy.accel; this.vel = toCopy.vel; this.pos = toCopy.pos;
}
public RobotTwoWheelState(RobotPose initialPose, RobotTwoWheelCommand initialCommand, RobotWheelModel right, RobotWheelModel left)
{
this.pose = initialPose; this.command = initialCommand;
rightWheel = new RobotWheelModel(right);
leftWheel = new RobotWheelModel(left);
}
public RobotWheelModel(RobotWheelModel toCopy)
{
this.accel = toCopy.accel; this.vel = toCopy.vel; this.pos = toCopy.pos;
}
public RobotTwoWheelState(RobotPose initialPose, RobotTwoWheelCommand initialCommand, RobotWheelModel right, RobotWheelModel left)
{
this.pose = initialPose; this.command = initialCommand;
rightWheel = new RobotWheelModel(right);
leftWheel = new RobotWheelModel(left);
}