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 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 MonteCarloFollower(double lookAhead, double maxVelocity, double maxTurn)
        {
            DISTANCE_WEIGHT = 25;
            VELOCITY_WEIGHT = 1;
            TURN_WEIGHT = 1;

            this.lookAhead = lookAhead;
            this.maxVelocity = maxVelocity;
            this.maxTurn = maxTurn;
            state = new RobotTwoWheelState(new RobotPose(), new RobotTwoWheelCommand(0, 0), new RobotWheelModel(0, 0, 0), new RobotWheelModel(0, 0, 0));
        }
        public static RobotTwoWheelState Simulate(RobotTwoWheelCommand cmd, RobotTwoWheelState inititalState, double timestep)
        {
            //copy the initial state first!
            RobotTwoWheelState final = new RobotTwoWheelState(inititalState,cmd);

            double prevIntegratedLeft = inititalState.leftWheel.pos;
            double prevIntegratedRight = inititalState.rightWheel.pos;

            if (cmd.velocity > 2) cmd.velocity = 2;
            else if (cmd.velocity < -2) cmd.velocity = -2;
            if (cmd.turn > 360) cmd.turn = 360;
            else if (cmd.turn < -360) cmd.turn = -360;
            cmd.turn = cmd.turn * 0.5;

            double cmdLeftWheelVel = cmd.velocity - 0.5 * trackWidth * (cmd.turn * Math.PI / 180);
            double cmdRightWheelVel = cmd.velocity + 0.5 * trackWidth * (cmd.turn * Math.PI / 180);

            final.leftWheel.ApplyCommand(cmdLeftWheelVel, timestep);
            final.rightWheel.ApplyCommand(cmdRightWheelVel, timestep);

            //now figure out the updated pose given the new state of our robot..
            double diffL = final.leftWheel.pos - prevIntegratedLeft;
            double diffR = final.rightWheel.pos - prevIntegratedRight;
            prevIntegratedLeft = final.leftWheel.pos;
            prevIntegratedRight = final.rightWheel.pos;

            //from lavalle's book ; http://planning.cs.uiuc.edu/node659.html

            //calculate the derivatives in measurements we have
            double xdot = ((diffL + diffR) / 2.0) * Math.Cos(inititalState.Pose.yaw);
            double ydot = ((diffL + diffR) / 2.0) * Math.Sin(inititalState.Pose.yaw);
            double headingDot = (diffR - diffL) / trackWidth;

            //update the state vector
            final.Pose.x = final.Pose.x + xdot;
            final.Pose.y = final.Pose.y + ydot;
            final.Pose.yaw = final.Pose.yaw + headingDot;
            while (final.Pose.yaw > Math.PI)
                final.Pose.yaw = final.Pose.yaw - 2 * Math.PI;
            while (final.Pose.yaw < -Math.PI)
                final.Pose.yaw = final.Pose.yaw + 2 * Math.PI;
            final.Pose.timestamp = final.Pose.timestamp + timestep;
            return final;
        }
 public RRTNode(RobotTwoWheelState state)
 {
     this.state = state;
     this.parent = this;
 }
 public RRTNode(RobotTwoWheelState state, RRTNode parent)
 {
     this.state = state;
     this.parent = parent;
 }
        public static RobotTwoWheelState Simulate(RobotTwoWheelCommand cmd, RobotTwoWheelState inititalState, double timestep)
        {
            //copy the initial state first!
            RobotTwoWheelState final = new RobotTwoWheelState(inititalState, cmd);

            double prevIntegratedLeft  = inititalState.leftWheel.pos;
            double prevIntegratedRight = inititalState.rightWheel.pos;

            if (cmd.velocity > 2)
            {
                cmd.velocity = 2;
            }
            else if (cmd.velocity < -2)
            {
                cmd.velocity = -2;
            }
            if (cmd.turn > 360)
            {
                cmd.turn = 360;
            }
            else if (cmd.turn < -360)
            {
                cmd.turn = -360;
            }
            cmd.turn = cmd.turn * 0.5;

            double cmdLeftWheelVel  = cmd.velocity - 0.5 * trackWidth * (cmd.turn * Math.PI / 180);
            double cmdRightWheelVel = cmd.velocity + 0.5 * trackWidth * (cmd.turn * Math.PI / 180);

            final.leftWheel.ApplyCommand(cmdLeftWheelVel, timestep);
            final.rightWheel.ApplyCommand(cmdRightWheelVel, timestep);

            //now figure out the updated pose given the new state of our robot..
            double diffL = final.leftWheel.pos - prevIntegratedLeft;
            double diffR = final.rightWheel.pos - prevIntegratedRight;

            prevIntegratedLeft  = final.leftWheel.pos;
            prevIntegratedRight = final.rightWheel.pos;

            //from lavalle's book ; http://planning.cs.uiuc.edu/node659.html

            //calculate the derivatives in measurements we have
            double xdot       = ((diffL + diffR) / 2.0) * Math.Cos(inititalState.Pose.yaw);
            double ydot       = ((diffL + diffR) / 2.0) * Math.Sin(inititalState.Pose.yaw);
            double headingDot = (diffR - diffL) / trackWidth;

            //update the state vector
            final.Pose.x   = final.Pose.x + xdot;
            final.Pose.y   = final.Pose.y + ydot;
            final.Pose.yaw = final.Pose.yaw + headingDot;
            while (final.Pose.yaw > Math.PI)
            {
                final.Pose.yaw = final.Pose.yaw - 2 * Math.PI;
            }
            while (final.Pose.yaw < -Math.PI)
            {
                final.Pose.yaw = final.Pose.yaw + 2 * Math.PI;
            }
            final.Pose.timestamp = final.Pose.timestamp + timestep;
            return(final);
        }