Ejemplo n.º 1
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="wheelOutputs"></param>
        public void Play(OutputToWheels wheelOutputs)
        {
            // deduce values of power
            var leftMotPower  = ExtractMotorPower(AbsMotorPower, wheelOutputs.LeftMotorPolarity);
            var rightMotPower = ExtractMotorPower(AbsMotorPower, wheelOutputs.RightMotorPolarity);

            // sleep if delay
            if (wheelOutputs.DelayInSeconds > 0)
            {
                System.Threading.Thread.Sleep(wheelOutputs.DelayInSeconds * 1000);
            }
            // MOVE!
            controller.MoveLeftWheel(leftMotPower, wheelOutputs.LeftMotorTachocount);
            controller.MoveRightWheel(rightMotPower, wheelOutputs.RightMotorTachocount);
        }
Ejemplo n.º 2
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="inhibitPhysicalMovement"></param>
        /// <returns></returns>
        public OutputToWheels Drive(int x, int y, bool inhibitPhysicalMovement)
        {
            // outputs
            OutputToWheels output = new OutputToWheels();

            // polar radius from cartesian coordinate
            var radius = Math.Sqrt(x * x + y * y);
            // angle (radians) from cartesian coordinate
            var angle_radians = Math.Acos(Math.Abs(x) / radius);

            if (double.IsNaN(angle_radians))
            {
                angle_radians = 0;
            }
            // angle (degrees) from cartesian coordinate
            var angle_degrees = angle_radians * 180 / Math.PI;
            // turn coeffecient i.e. scale of deviation from 45-degrees (NE, NW, SE, SW)
            var tcoeff = (angle_degrees / 45) - 1; //-1 + (angle_degrees/ 90) * 2;
            // raw output of the turn motor
            var rawTurn = tcoeff * Math.Abs((Math.Abs(y) - Math.Abs(x)));

            rawTurn = Math.Round(rawTurn * 100) / 100;
            var turnMotPolarity = ExtractMotorPolarity(Convert.ToSingle(rawTurn));
            var turnMotTacho    = Convert.ToUInt32((Math.Abs(rawTurn) * _distancePerDegreeInCM));
            // raw output of the distance motor
            var rawDistance         = Math.Max(Math.Abs(y), Math.Abs(x));
            var distanceMotPolarity = ExtractMotorPolarity(rawDistance);
            var distanceMotTacho    = Convert.ToUInt32((Math.Abs(rawDistance) * _distancePerDegreeInCM));

            //// re-map values to correct output range (from -G to +G)
            //var valLeft = ReMap(rawLeft, _minXYVal, _maxXYVal, minOutputVal, maxOutputVal);
            //var valRight = ReMap(rawRight, _minXYVal, _maxXYVal, minOutputVal, maxOutputVal);

            // if input coordinate in 1st or 3rd quadrant; left motor is Distance, right motor is Turn
            if ((x >= 0 && y >= 0) || (x < 0 && y < 0))
            {
                output.LeftMotorPolarity    = distanceMotPolarity;
                output.LeftMotorTachocount  = distanceMotTacho;
                output.RightMotorPolarity   = turnMotPolarity;
                output.RightMotorTachocount = turnMotTacho;
            }
            else
            {
                output.RightMotorPolarity   = distanceMotPolarity;
                output.RightMotorTachocount = distanceMotTacho;
                output.LeftMotorPolarity    = turnMotPolarity;
                output.LeftMotorTachocount  = turnMotTacho;
            }
            // Reverse polarity
            //if (y < 0)
            //{
            //    rawLeft = (float)(0.0 - rawLeft);
            //    rawRight = (float)(0.0 - rawRight);
            //}

            // play generated output (MOVE!)
            if (!inhibitPhysicalMovement)
            {
                Play(output);
            }

            // return generated outputs.
            return(output);
        }