コード例 #1
0
 public SimulationWheelShaftEncoderState(string name, SimulationMotorState motor, float wheelRadius, int ticksPerRevolution)
 {
     Name               = name;
     Motor              = motor;
     WheelRadius        = wheelRadius;
     TicksPerRevolution = ticksPerRevolution;
 }
コード例 #2
0
 public SimulationRobotState(float width, float height, float density, SimulationMotorState[] motorStates, SimulationWheelShaftEncoderState[] wheelShaftEncoderStates, SimulationGyroscopeState yawGyroscopeState)
 {
     this.width = width;
      this.height = height;
      this.density = density;
      this.motorStates = motorStates;
      this.wheelShaftEncoderStates = wheelShaftEncoderStates;
      this.yawGyroscopeState = yawGyroscopeState;
 }
コード例 #3
0
      private void ApplyMotorForces(SimulationMotorState motorState) {
         var forceVectorWorld = Vector2.Transform(motorState.CurrentForceVector, Matrix.CreateRotationZ(robotBody.Rotation));
//         Console.WriteLine(forceVectorWorld);
         robotBody.ApplyForce(
            forceVectorWorld,
            robotBody.GetWorldPoint(motorState.RelativePosition)
            );
         //         renderer.DrawForceVectorWorld(position, forceVectorWorld, Color.Cyan);
      }
コード例 #4
0
 private void DrawMotorBody(IRenderer renderer, SimulationMotorState motorState) {
    var position = robotBody.GetWorldPoint(motorState.RelativePosition);
    var extents = new Vector2(robotState.Width / 10, robotState.Height / 10);
    renderer.DrawCenteredRectangleWorld(
       position,
       extents,
       robotBody.Rotation,
       Color.Lime);
    var forceVectorWorld = Vector2.Transform(motorState.CurrentForceVector, Matrix.CreateRotationZ(robotBody.Rotation));
    renderer.DrawForceVectorWorld(position, forceVectorWorld, Color.Cyan);
 }
 /// <summary>
 /// Given robot :[^]:, tl/br vect is \, tr/bl vect is /
 /// </summary>
 /// <param name="robotWidth">Width of robot</param>
 /// <param name="robotHeight">Height of robot (well, y-axis 'height')</param>
 /// <param name="wheelForceAngle">magnitude of mecanum wheels' force vectors' angles (relative to ^ direction) in radians</param>
 /// <param name="wheelForceAmplitude">Maximum effective force applied by wheel</param>
 /// <returns></returns>
 public static SimulationMotorState[] MecanumDrive(float robotWidth, float robotHeight, float wheelForceAngle, float wheelForceAmplitude)
 {
     var forceVector = new Vector2(0, wheelForceAmplitude);
      var forceTopLeft = Vector2.Transform(forceVector, Matrix.CreateRotationZ(wheelForceAngle));
      var forceTopRight = Vector2.Transform(forceVector, Matrix.CreateRotationZ(-wheelForceAngle));
      var motorStates = new SimulationMotorState[4];
      float halfWidth = robotWidth / 2, halfHeight = robotHeight / 2;
      float backFrontSpacing = halfHeight / 2;
      motorStates[0] = new SimulationMotorState("Drive.Motors.RearRight", new Vector2(halfWidth, -backFrontSpacing), forceTopLeft);
      motorStates[1] = new SimulationMotorState("Drive.Motors.RearLeft", new Vector2(-halfWidth, -backFrontSpacing), forceTopRight);
      motorStates[2] = new SimulationMotorState("Drive.Motors.FrontLeft", new Vector2(-halfWidth, backFrontSpacing), forceTopLeft);
      motorStates[3] = new SimulationMotorState("Drive.Motors.FrontRight", new Vector2(halfWidth, backFrontSpacing), forceTopRight);
      return motorStates;
 }
コード例 #6
0
        /// <summary>
        /// Given robot :[^]:, fl/fr is |, rl is \, rr is /
        /// </summary>
        /// <param name="robotWidth">Width of robot</param>
        /// <param name="robotHeight">Height of robot (well, y-axis 'height')</param>
        /// <param name="wheelForceAngle">magnitude of mecanum wheels' force vectors' angles (relative to ^ direction) in radians</param>
        /// <param name="wheelForceAmplitude">Maximum effective force applied by wheel</param>
        /// <returns></returns>
        public static SimulationMotorState[] HybridDrive(float robotWidth, float robotHeight, float wheelForceAngle, float wheelForceAmplitude)
        {
            var   forwardForceVector = new Vector2(0, wheelForceAmplitude);
            var   forceRearLeft = Vector2.Transform(forwardForceVector, Matrix.CreateRotationZ(-wheelForceAngle));
            var   forceRearRight = Vector2.Transform(forwardForceVector, Matrix.CreateRotationZ(wheelForceAngle));
            var   motorStates = new SimulationMotorState[4];
            float halfWidth = robotWidth / 2, halfHeight = robotHeight / 2;
            float backFrontSpacing = halfHeight / 2;

            motorStates[0] = new SimulationMotorState("Drive.Motors.RearRight", new Vector2(halfWidth, -backFrontSpacing), forceRearLeft);
            motorStates[1] = new SimulationMotorState("Drive.Motors.RearLeft", new Vector2(-halfWidth, -backFrontSpacing), forceRearRight);
            motorStates[2] = new SimulationMotorState("Drive.Motors.FrontLeft", new Vector2(-halfWidth, backFrontSpacing), forwardForceVector);
            motorStates[3] = new SimulationMotorState("Drive.Motors.FrontRight", new Vector2(halfWidth, backFrontSpacing), forwardForceVector);
            return(motorStates);
        }
コード例 #7
0
        public static SimulationMotorState[] RovDrive(float robotWidth, float robotHeight, float wheelForceAngle, float wheelForceAmplitude)
        {
            var   forceVector = new Vector2(0, wheelForceAmplitude);
            var   forceTopLeft = Vector2.Transform(forceVector, Matrix.CreateRotationZ(-wheelForceAngle));
            var   forceTopRight = Vector2.Transform(forceVector, Matrix.CreateRotationZ(wheelForceAngle));
            var   motorStates = new SimulationMotorState[7];
            float halfWidth = robotWidth / 2, halfHeight = robotHeight / 2;
            float backFrontSpacing = halfHeight / 2;

            motorStates[0] = new SimulationMotorState("Drive.Motors.RearRight", new Vector2(halfWidth, -backFrontSpacing), forceTopLeft);
            motorStates[1] = new SimulationMotorState("Drive.Motors.RearLeft", new Vector2(-halfWidth, -backFrontSpacing), forceTopRight);
            motorStates[2] = new SimulationMotorState("Drive.Motors.FrontLeft", new Vector2(-halfWidth, backFrontSpacing), forceTopLeft);
            motorStates[3] = new SimulationMotorState("Drive.Motors.FrontRight", new Vector2(halfWidth, backFrontSpacing), forceTopRight);

            // dummy vertical motors
            motorStates[4] = new SimulationMotorState("Drive.Motors.VertFrontLeft", Vector2.Zero, forceVector);
            motorStates[5] = new SimulationMotorState("Drive.Motors.VertFrontRight", Vector2.Zero, forceVector);
            motorStates[6] = new SimulationMotorState("Drive.Motors.VertRear", Vector2.Zero, forceVector);

            return(motorStates);
        }
 public static SimulationWheelShaftEncoderState FromMotor(SimulationMotorState motor, float wheelRadius, int ticksPerRevolution)
 {
     return(new SimulationWheelShaftEncoderState(motor.Name + ".Encoder", motor, wheelRadius, ticksPerRevolution));
 }
 public static SimulationWheelShaftEncoderState FromMotor(SimulationMotorState motor, float wheelRadius, int ticksPerRevolution)
 {
     return new SimulationWheelShaftEncoderState(motor.Name + ".Encoder", motor, wheelRadius, ticksPerRevolution);
 }
 public static SimulationWheelShaftEncoderState[] FromMotors(SimulationMotorState[] motors, float wheelRadius, int ticksPerRevolution)
 {
     return Util.Generate(motors.Length, i => FromMotor(motors[i], wheelRadius, ticksPerRevolution));
 }
コード例 #11
0
 private void ApplyMotorForces(SimulationMotorState motorState)
 {
     var forceVectorWorld = Vector2.Transform(motorState.CurrentForceVector, Matrix.CreateRotationZ(robotBody.Rotation));
     //         Console.WriteLine(forceVectorWorld);
      robotBody.ApplyForce(
     forceVectorWorld,
     robotBody.GetWorldPoint(motorState.RelativePosition)
     );
      //         renderer.DrawForceVectorWorld(position, forceVectorWorld, Color.Cyan);
 }
コード例 #12
0
 private void DrawMotorBody(IRenderer renderer, SimulationMotorState motorState)
 {
     var position = robotBody.GetWorldPoint(motorState.RelativePosition);
      var extents = new Vector2(robotState.Width / 10, robotState.Height / 10);
      renderer.DrawCenteredRectangleWorld(
     position,
     extents,
     robotBody.Rotation,
     Color.Lime);
      var forceVectorWorld = Vector2.Transform(motorState.CurrentForceVector, Matrix.CreateRotationZ(robotBody.Rotation));
      renderer.DrawForceVectorWorld(position, forceVectorWorld, Color.Cyan);
 }