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);
 }