/// <summary>
        /// Convert a DriveCommand to a BCL payload in a byte array.
        /// </summary>
        /// <param name="driveCommand">The DriveCommand to be executed.</param>
        /// <returns>byte[] - Byte array representing a BCL payload.</returns>
        private static byte[] DriveCommandToBclPayload(DriveCommand driveCommand)
        {
            // Convert DriveCommand to byte array BCL packet
            byte[] payload = { (byte)driveCommand.Left, (byte)driveCommand.Right };

            return(payload);
        }
示例#2
0
        public async Task SendCommand(DriveCommand command)
        {
            if (LegoBrick == null)
            {
                return;
            }

            switch (command)
            {
                case DriveCommand.Forward:
                    LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.C, HalfPower, OneSecond, false);
                    LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.B, HalfPower, OneSecond, false);

                    break;
                case DriveCommand.Back:
                    LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.C, HalfPower * -1, OneSecond, false);
                    LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.B, HalfPower * -1, OneSecond, false);

                    break;
                case DriveCommand.Left:
                    LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.B, HalfPower * -1, QuarterSecond, false);
                    LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.C, HalfPower, QuarterSecond, false);                   

                    break;
                case DriveCommand.Right:
                    LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.C, HalfPower * -1, QuarterSecond, false);
                    LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.B, HalfPower, QuarterSecond, false);

                    break;
                default:
                    break;
            }
            await LegoBrick.BatchCommand.SendCommandAsync();
        }
示例#3
0
        /// <summary>
        /// Find the next DriveCommand to be issued to ROCKS.
        /// </summary>
        /// <returns>DriveCommand - the next drive command for ROCKS to execute.</returns>
        public DriveCommand FindNextDriveCommand()
        {
            TennisBall ball = this.camera.GetBallCopy();

            // If verified to be within required distance, send success to ROCKS and halt
            if (this.isWithinRequiredDistance)
            {
                // Send success back to base station until receive ACK
                StatusHandler.SendSimpleAIPacket(Status.AIS_FOUND_GATE);
                Console.WriteLine("Within required distance - halting ");

                return(DriveCommand.Straight(Speed.HALT));
            }

            // Recently detected ball but now don't now. Stop driving to redetect since we may have dropped it due to bouncing.
            if (ball == null && DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() < this.camera.BallTimestamp + DROP_BALL_DELAY)
            {
                StatusHandler.SendSimpleAIPacket(Status.AIS_DROP_BALL);
                Console.WriteLine("Dropped ball - halting ");

                this.verificationQueue.Clear();
                return(DriveCommand.Straight(Speed.HALT));
            }

            // Ball detected
            if (ball != null)
            {
                return(DriveBallDetected(ball));
            }

            // No ball detected
            return(DriveNoBallDetected(ball));
        }
        private async void MoveLego(DriveCommand movement)
        {
            switch (movement)
            {
            case DriveCommand.Forward:
                ////mueve la rueda izquierda y mueve la rueda derecha
                await _brick.DirectCommand.TurnMotorAtSpeedForTimeAsync(OutputPort.C | OutputPort.B, 0x25, 1000, false);

                break;

            case DriveCommand.Right:
                ////mueve la rueda izquierda
                await _brick.DirectCommand.TurnMotorAtSpeedForTimeAsync(OutputPort.B, 0x25, 1000, false);

                break;

            case DriveCommand.Left:
                ////mueve la rueda derecha
                await _brick.DirectCommand.TurnMotorAtSpeedForTimeAsync(OutputPort.C, 0x25, 1000, false);

                break;

            case DriveCommand.Back:
                ////mueve la rueda derecha y la izquierda en velocidad negativa
                await _brick.DirectCommand.TurnMotorAtSpeedForTimeAsync(OutputPort.C | OutputPort.B, -0x25, 1000, false);

                break;
            }
        }
示例#5
0
 private void RunCommand(DriveCommand command)
 {
     Action action = async() => {
         txtCommand.Text = command.ToString();
         await _lego.SendCommand(command);
     };
 }
示例#6
0
        /// <summary>
        /// 这是老的办法 但是会显示很长
        /// </summary>
        public void Test01()
        {
            DriveCommand command = DriveCommand.Start;

            switch (command)
            {
            case DriveCommand.Start:
                break;

            case DriveCommand.Stop:
                break;

            case DriveCommand.Pause:
                break;

            case DriveCommand.TurnLeft:
                break;

            case DriveCommand.TurnRight:
                break;

            default:
                break;
            }
        }
 private void RunCommand(DriveCommand command)
 {
     Dispatcher.BeginInvoke(async() =>
     {
         txtCommand.Text = command.ToString();
         await _lego.SendCommand(command);
     });
 }
        private void WriteMovement(DriveCommand movement)
        {
            deleteButtons();
            string name  = movement.ToString();
            var    boton = (Button)arrows.FindName(name);

            boton.Visibility = Visibility.Visible;
        }
示例#9
0
 public void UpdateVoteCounter(DriveCommand command)
 {
     if (CurrentVotesCount[command] < Int32.MaxValue)
     {
         CurrentVotesCount[command]++;
     }
     Clients.All.updateVotesCounter(CurrentVotesCount);
 }
示例#10
0
        /// <summary>
        /// Send a given DriveCommand to ROCKS.
        /// </summary>
        /// <param name="driveCommand">The DriveCommand to be executed.</param>
        /// <returns>bool - If DriveCommand was successfully sent.</returns>
        public static void SendDriveCommand(DriveCommand driveCommand)
        {
            // Form payload for BCL drive command from specified DriveCommand
            byte[] bclPayload = DriveCommandToBclPayload(driveCommand);

            // Send opcode, payload to AscentPacketHandler to send drive packet to ROCKS
            AscentPacketHandler.SendPayloadToROCKS(OPCODE_ALL_WHEEL_SPEED, bclPayload, AscentPacketHandler.ROCKS_AI_SERVICE_ID);

            // For debugging
            Console.WriteLine("left: " + (sbyte)driveCommand.Left + " | right: " + (sbyte)driveCommand.Right);
        }
        /// <summary>
        /// Main execution function for AutonomousService.
        /// </summary>
        public void Execute(Object source, ElapsedEventArgs e)
        {
            // Don't execute if existing execution is not complete
            if (!Monitor.TryEnter(executeLock))
            {
                return;
            }

            // TODO debugging - delete
            //Console.WriteLine("The Elapsed event was raised at {0:HH:mm:ss.fff}", e.SignalTime);

            // this is for when we're running out of time and just roll back to only gps and hope for the best
            if (!this.panic && (DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - this.startTimeStamp.ToUnixTimeMilliseconds() > this.fuckItGoForItCountDown))
            {
                StatusHandler.SendDebugAIPacket(Status.AIS_LOG, "Starting hail mary");
                this.panic = true;
                this.driveContext.HandleFinalCountDown();
            }

            // If detected an obstacle within the last 5 seconds, continue straight to clear obstacle
            if (IsLastObstacleWithinInterval(OBSTACLE_WATCHDOG_MILLIS))
            {
                StatusHandler.SendSimpleAIPacket(Status.AIS_OUT_WATCHDOG);
                Console.WriteLine("Watchdog");

                // If more than 0.5 seconds have passed since last event, it's safe to start issuing drive
                // commands - otherwise race condition may occur when continually detecting an obstacle
                if (!IsLastObstacleWithinInterval(CLEAR_OBSTACLE_DELAY_MILLIS))
                {
                    this.driveContext.Drive(DriveCommand.Straight(Speed.CLEAR_OBSTACLE));
                }

                return;
            }

            // Get DriveCommand from current drive state, issue DriveCommand
            DriveCommand driveCommand = this.driveContext.FindNextDriveCommand();

            this.driveContext.Drive(driveCommand);

            // If state change is required, change state
            StateType nextState = this.driveContext.GetNextStateType();

            if (this.driveContext.IsStateChangeRequired(nextState))
            {
                Console.WriteLine("Switching from state: " + this.driveContext.StateType.ToString() + " to: " + nextState.ToString());

                this.driveContext.ChangeState(nextState);
            }

            Monitor.Exit(executeLock);
        }
示例#12
0
        private void HDG()
        {
            if (Event.current.Equals(Event.KeyboardEvent("return")) && GUI.GetNameOfFocusedControl().StartsWith("RC"))
            {
                mFlightComputer.Enqueue(DriveCommand.DistanceHeading(Dist, Heading, mSteerClamp, Speed));
            }

            GUILayout.BeginHorizontal();
            {
                GUILayout.Label(new GUIContent("Wheel: ", "How sharp to turn at max"));
                GUILayout.FlexibleSpace();
                GUILayout.Label(new GUIContent(mSteerClamp.ToString("P"), "How sharp to turn at max"));
                GUILayout.Label(new GUIContent("max", "How sharp to turn at max"), GUILayout.Width(40));
            }
            GUILayout.EndHorizontal();

            RTUtil.HorizontalSlider(ref mSteerClamp, 0, 1);

            GUILayout.BeginHorizontal();
            {
                GUILayout.Label(new GUIContent("Hdg.", "Heading to keep"), GUILayout.Width(50));
                GUI.SetNextControlName("RC1");
                RTUtil.TextField(ref Mheading, GUILayout.Width(50), GUILayout.ExpandWidth(false));
                GUILayout.Label("(°)", GUI.skin.textField, GUILayout.Width(40));
            }
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            {
                GUILayout.Label(new GUIContent("Dist.", "Distance to drive"), GUILayout.Width(50));
                GUI.SetNextControlName("RC2");
                RTUtil.TextField(ref mDist, GUILayout.Width(50), GUILayout.ExpandWidth(false));
                GUILayout.Label("(m)", GUI.skin.textField, GUILayout.Width(40));
            }
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            {
                GUILayout.Label(new GUIContent("Speed", "Speed to keep"), GUILayout.Width(50));
                GUI.SetNextControlName("RC3");
                RTUtil.TextField(ref mSpeed, GUILayout.Width(50), GUILayout.ExpandWidth(false));
                GUILayout.Label(new GUIContent("(m/s)", "Speed to keep"), GUI.skin.textField, GUILayout.Width(40));
            }
            GUILayout.EndHorizontal();

            Mheading = RTUtil.ConstrictNum(Mheading, 360);
            mDist    = RTUtil.ConstrictNum(mDist, false);
            mSpeed   = RTUtil.ConstrictNum(mSpeed, false);
        }
示例#13
0
        public void SendDriveCommand(DriveCommand command)
        {
            Trace.TraceInformation("SendDriveCommand: " + command.ToString());

            //call to BeAPI
            using (var client = new HttpClient())
            {
                client.BaseAddress = new Uri("http://legoev3api.azurewebsites.net/");
                client.DefaultRequestHeaders.Accept.Clear();
                client.DefaultRequestHeaders.Accept.Add(new MediaTypeWithQualityHeaderValue("application/json"));

                var response = client.PostAsJsonAsync("api/values", (int)command).Result;
                if (response.IsSuccessStatusCode)
                {
                    var a = response.Headers.Location;
                }
            }
        }
        private ACommand driveProcess(DriveCommand command, Battlefield.RobotAndBattlefield robotAndBattlefield)
        {
            BattlefieldRobot robot = robotAndBattlefield.ROBOT;

            if (robot.HitPoints > 0)
            {
                if (robot.Power <= robot.Motor.ROTATE_IN)
                {
                    robot.AngleDrive = command.ANGLE;
                }
                robot.WantedPower = command.POWER > 100 ? 100 : (command.POWER < 0 ? 0 : command.POWER);
                return(new DriveAnswerCommand(robot.AngleDrive.DEquals(command.ANGLE)));
            }
            else
            {
                return(new DriveAnswerCommand(robot.AngleDrive.DEquals(command.ANGLE)));
            }
        }
示例#15
0
        public void SendDriveCommand(DriveCommand command)
        {
            Trace.TraceInformation("SendDriveCommand: " + command.ToString());

            //call to BeAPI
            using (var client = new HttpClient())
            {
                client.BaseAddress = new Uri("http://legoev3api.azurewebsites.net/");
                client.DefaultRequestHeaders.Accept.Clear();
                client.DefaultRequestHeaders.Accept.Add(new MediaTypeWithQualityHeaderValue("application/json"));

                var response = client.PostAsJsonAsync("api/values", (int)command).Result;
                if (response.IsSuccessStatusCode)
                {
                    var a = response.Headers.Location;
                }
            }


        }
示例#16
0
        private DriveCommand DriveBallDetected(TennisBall ball)
        {
            // Sanity check
            if (ball == null)
            {
                this.verificationQueue.Clear();
                return(null);
            }

            // Add to verification queue
            DetectedBall detectedBall = new DetectedBall(ball, AscentPacketHandler.GPSData.GetDistanceTo(this.gate), DateTimeOffset.UtcNow.ToUnixTimeMilliseconds());

            this.verificationQueue.Enqueue(detectedBall);

            // TODO debugging - delete
            Console.Write("Ball detected | Verifying ({0})... ", this.verificationQueue.Count);


            // Detected ball so no longer scan
            this.scan = null;

            // Within required distance - use verification queue to determine if we should send back success
            if (IsWithinRequiredDistance(ball))
            {
                if (this.verificationQueue.VerifyBallDetection(
                        VERIFICATION_DISTANCE_PERCENTAGE,
                        DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - VERIFICATION_TIMESTAMP_THRESHOLD,
                        DriveContext.REQUIRED_DISTANCE_FROM_BALL,
                        DriveContext.REQUIRED_DISTANCE_FROM_BALL + DriveContext.GPS_PRECISION))
                {
                    this.isWithinRequiredDistance = true;
                }

                Console.Write("WITHIN REQUIRED DISTANCE | ");

                // Halt to wait for success to be sent back to base station
                return(DriveCommand.Straight(Speed.HALT));
            }

            return(DriveTowardBall(ball));
        }
示例#17
0
        // Could go off angle, but left as X coordinate for now
        private DriveCommand DriveTowardBall(TennisBall ball)
        {
            // Not within required distance
            float ballX = ball.CenterPoint.X;

            if (ballX < leftThreshold)  // TODO look into this for dynamic video sizes. ie. be able to account for 1080, 720, etc.
            {
                // Ball to left
                return(DriveCommand.LeftTurn(Speed.VISION));
            }
            else if (ballX > rightThreshold)
            {
                // Ball to right
                return(DriveCommand.RightTurn(Speed.VISION));
            }
            else
            {
                // Ball straight ahead
                return(DriveCommand.Straight(Speed.VISION));
            }
        }
示例#18
0
        private void TimerElapsed(object state)
        {
            //Trace.TraceInformation(DateTime.Now.ToLongTimeString() + ": SignalR Timer Elapsed ");
            stateCounter++;

            //Send vote total every 1/2 second
            Clients.All.sendState(storage.GetState());

            //Send command every 3 seconds
            if (stateCounter >= 6)
            {
                //Trace.TraceInformation("Sending to LEGO!");
                stateCounter = 0;
                if (storage.HasCommand())
                {
                    DriveCommand cmd = storage.GetMostPopularCommand();
                    Debug.WriteLine("Popular: " + cmd.ToString());
                    Clients.All.commandToRun(cmd);
                    storage.Reset();
                }
            }
        }
示例#19
0
        public async Task SendCommand(DriveCommand command)
        {
            if (LegoBrick == null)
            {
                return;
            }

            switch (command)
            {
            case DriveCommand.Forward:
                LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.C, HalfPower, OneSecond, false);
                LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.B, HalfPower, OneSecond, false);

                break;

            case DriveCommand.Back:
                LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.C, HalfPower * -1, OneSecond, false);
                LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.B, HalfPower * -1, OneSecond, false);

                break;

            case DriveCommand.Left:
                LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.C, HalfPower * -1, QuarterSecond, false);
                LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.B, HalfPower, QuarterSecond, false);

                break;

            case DriveCommand.Right:
                LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.B, HalfPower * -1, QuarterSecond, false);
                LegoBrick.BatchCommand.TurnMotorAtPowerForTime(OutputPort.C, HalfPower, QuarterSecond, false);

                break;

            default:
                break;
            }
            await LegoBrick.BatchCommand.SendCommandAsync();
        }
示例#20
0
 private void OnExecClick(int modeIndex)
 {
     //FINE
     if (modeIndex == (int)RoverModes.FineMode && Speed != 0)
     {
         if (mSteering != 0 && Turn != 0)
         {
             EnqueueTurn();
         }
         else if (Dist != 0)
         {
             EnqueueDist();
         }
         else
         {
             if (!distDefault && mSteering != 0 && Turn != 0)
             {
                 EnqueueTurn();
             }
             else if (distDefault && Dist != 0)
             {
                 EnqueueDist();
             }
         }
     }
     //HDG
     else if (modeIndex == (int)RoverModes.HeadingMode)
     {
         mFlightComputer.Enqueue(DriveCommand.DistanceHeading(Dist, Heading, mSteerClamp, Speed));
     }
     //TGT
     else if (modeIndex == (int)RoverModes.TargetMode)
     {
         mFlightComputer.Enqueue(DriveCommand.Coord(mSteerClamp, Latitude, Longitude, Speed));
     }
 }
示例#21
0
 public static extern bool DeviceIoControl(IntPtr handle,
   DriveCommand command, ref DriveCommandParameter parameter,
   int parameterSize, out DriveIdentifyResult result, int resultSize,
   out uint bytesReturned, IntPtr overlapped);
示例#22
0
 public void SendDriveCommand(DriveCommand command)
 {
     instance.UpdateVoteCounter(command);
     Trace.TraceInformation("SendDriveCommand: " + command.ToString());
 }
示例#23
0
 public static extern bool DeviceIoControl(SafeHandle handle,
                                           DriveCommand command, ref DriveCommandParameter parameter,
                                           int parameterSize, out DriveSmartReadThresholdsResult result,
                                           int resultSize, out uint bytesReturned, IntPtr overlapped);
示例#24
0
        private void RunCommand(DriveCommand command)
        {     
        
            Action action = async () =>            {

                txtCommand.Text = command.ToString();
                await _lego.SendCommand(command);
            };



        }
示例#25
0
 public void AddOrUpdateVote(string id, DriveCommand command)
 {
     PlayerVote.AddOrUpdate(id, command, (key, oldValue) => command);
 }
        public void InitMode(DriveCommand dc)
        {
            UpdateAxis();

            mRoverAlt = (float)mVessel.altitude;
            mRoverLat = (float)mVessel.latitude;
            mRoverLon = (float)mVessel.longitude;

            switch (dc.mode) {
                case DriveCommand.DriveMode.Turn:
                    mRoverRot = mVessel.ReferenceTransform.rotation;
                    break;
                case DriveCommand.DriveMode.Distance:
                    mWheelPID.setClamp(-1, 1);
                    mKeepHDG = RoverHDG;
                    break;
                case DriveCommand.DriveMode.DistanceHeading:
                    mWheelPID.setClamp(-dc.steering, dc.steering);
                    break;
                case DriveCommand.DriveMode.Coord:
                    mWheelPID.setClamp(-dc.steering, dc.steering);
                    mTargetLat = dc.target;
                    mTargetLon = dc.target2;
                    break;
            }
            mThrottlePID.Reset();
            mWheelPID.Reset();
            mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false);
        }
示例#27
0
        private DriveCommand DriveNoBallDetected(TennisBall ball)
        {
            // Sanity check
            if (ball != null)
            {
                return(null);
            }


            Console.Write("Ball not detected | ");

            // Clear verification queue if it has values
            this.verificationQueue.Clear();

            GPS    ascent         = AscentPacketHandler.GPSData;
            double distanceToGate = ascent.GetDistanceTo(this.gate);

            // Kick back to GPS
            if (distanceToGate > DISTANCE_SWITCH_TO_GPS)    // 6 meters
            {
                Console.WriteLine("Distance: " + distanceToGate + ". Switch to GPS");

                switchToGPS = true;
                return(DriveCommand.Straight(Speed.HALT));
            }

            // Turn to face heading, drive toward it
            if (distanceToGate > DISTANCE_USE_HEADING)      // 3 meters
            {
                Console.WriteLine("Distance: " + distanceToGate + ". Turning toward heading to drive towrad it");

                short  ascentHeading = AscentPacketHandler.Compass;
                double headingToGate = ascent.GetHeadingTo(this.gate);

                Console.Write("currCompass: "******" | headingToGoal: " + headingToGate + " | distance: " + distanceToGate + " | ");

                // Aligned with heading. Start going straight
                if (IMU.IsHeadingWithinThreshold(ascentHeading, headingToGate, Scan.HEADING_THRESHOLD))
                {
                    return(DriveCommand.Straight(Speed.VISION));
                }

                // Turn toward gate heading angle
                if (IMU.IsHeadingWithinThreshold(ascentHeading, (headingToGate + 90) % 360, 90))
                {
                    return(DriveCommand.LeftTurn(Speed.VISION_SCAN));
                }
                else
                {
                    return(DriveCommand.RightTurn(Speed.VISION_SCAN));
                }

                // Probably would work, kept as reference

                /*
                 * double lowBound = headingToGate;
                 * double highBound = (headingToGate + 180) % 360;
                 *
                 * if (lowBound < highBound)
                 * {
                 *  if (lowBound < ascentHeading && ascentHeading < highBound)
                 *  {
                 *      return DriveCommand.LeftTurn(DriveCommand.SPEED_VISION_SCAN);
                 *  }
                 *  else
                 *  {
                 *      return DriveCommand.RightTurn(DriveCommand.SPEED_VISION_SCAN);
                 *  }
                 * }
                 * else
                 * {
                 *  if (!(highBound < ascentHeading && ascentHeading < lowBound))
                 *  {
                 *      return DriveCommand.LeftTurn(DriveCommand.SPEED_VISION_SCAN);
                 *  }
                 *  else
                 *  {
                 *      return DriveCommand.RightTurn(DriveCommand.SPEED_VISION_SCAN);
                 *  }
                 * }
                 */
            }

            // If scanning, complete scan
            if (this.scan != null)
            {
                Console.WriteLine("Scanning... Distance: " + distanceToGate);

                if (!this.scan.IsComplete())
                {
                    return(scan.FindNextDriveCommand());
                }
                else
                {
                    this.completedScans++;

                    // Clear scan, will rescan below
                    this.scan = null;
                }
            }

            switch (this.completedScans)
            {
            case 0:
            {
                // Initialize the first scan
                if (distanceToGate > DISTANCE_CLOSE_RANGE)          // 2 meters
                {
                    // Turn toward heading. Scan, use heading as reference
                    StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
                    Console.WriteLine("Scan: Using heading as reference. Distance: " + Math.Round(distanceToGate, 2));

                    this.scan = new Scan(this.gate, true);
                }
                else
                {
                    // Scan
                    StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Not using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
                    Console.WriteLine("Scan: Not using heading as reference. Distance: " + Math.Round(distanceToGate, 2));

                    this.scan = new Scan(this.gate, false);
                }

                break;
            }

            case 1:
            {
                // Align toward heading, drive for 10ish seconds,
                StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: 1st 10s scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
                Console.WriteLine("Scan: Distance: " + Math.Round(distanceToGate, 2) + ". Scanning (using heading). Driving 5m away...");

                this.scan = new Scan(this.gate, SCAN_DRIVE_STRAIGHT_DURATION_MILLIS);

                break;
            }

            /*
             * case 2:
             * {
             *  // Broaden HSV values, scan
             *  StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Broadening HSV values. Distance: " + Math.Round(distanceToGate, 2));
             *  Console.WriteLine("Scan: Broadening HSV values. Distance: " + Math.Round(distanceToGate, 2));
             *
             *  this.camera.BroadenHsvValues();
             *
             *  this.scan = new Scan(this.gate, false);
             *
             *  break;
             * }
             */
            case 2:
            {
                // Align toward heading (again), drive for 5ish seconds,
                StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: 2nd 5m scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
                Console.WriteLine("Scan: 2nd 5m scan toward heading. Distance: " + Math.Round(distanceToGate, 2));

                this.scan = new Scan(this.gate, SCAN_DRIVE_STRAIGHT_DURATION_MILLIS);

                break;
            }

            case 3:
            {
                // We've already run 1 scan, 1 5m scan, 1 broaden HSV, 1 more 5m scan, so drive straight to kick back to GPS and do it over again
                return(DriveCommand.Straight(Speed.VISION));
            }

            default:
            {
                break;
            }
            }

            return(scan.FindNextDriveCommand());
        }
        public void InitMode(DriveCommand dc)
        {
            axis = ModuleWheel.AlignmentAxis.None;
            foreach (Part p in mVessel.parts) {
                if (p.Modules.Contains("ModuleWheel")) {
                    axis = (p.Modules["ModuleWheel"] as ModuleWheel).alignmentAxis;
                    if (axis != ModuleWheel.AlignmentAxis.None)
                        break;
                }
            }
            mRoverAlt = (float)mVessel.altitude;
            mRoverLat = (float)mVessel.latitude;
            mRoverLon = (float)mVessel.longitude;

            switch (dc.mode) {
                case DriveCommand.DriveMode.Turn:
                    mRoverRot = mVessel.ReferenceTransform.rotation;
                    break;
                case DriveCommand.DriveMode.Distance:
                    mWheelPID.setClamp(-1, 1);
                    mKeepHDG = RoverHDG;
                    break;
                case DriveCommand.DriveMode.DistanceHeading:
                    mWheelPID.setClamp(-dc.steering, dc.steering);
                    break;
                case DriveCommand.DriveMode.Coord:
                    mWheelPID.setClamp(-dc.steering, dc.steering);
                    mTargetLat = dc.target;
                    mTargetLon = dc.target2;
                    break;
            }
            mThrottlePID.Reset();
            mWheelPID.Reset();
            mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false);
        }
示例#29
0
 public DriveCommand FindNextDriveCommand()
 {
     return(DriveCommand.Straight(Speed.NORMAL_OPERATION));
 }
        private void Coord(DriveCommand dc, FlightCtrlState fs)
        {
            float
                dist = Vector3.Distance(mVessel.CoM, TargetPos),
                deg = RTUtil.ClampDegrees180(RoverHDG - TargetHDG);

            if (dist > Math.Abs(deg) / 36) {
                fs.wheelThrottle = mThrottlePID.Control(dc.speed - RoverSpeed);
                fs.wheelSteer = mWheelPID.Control(deg);
            } else {
                fs.wheelThrottle = 0;
                fs.wheelSteer = 0;
                mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
                dc.mode = DriveCommand.DriveMode.Off;
                dc = null;
            }
        }
 private void DistanceHeading(DriveCommand dc, FlightCtrlState fs)
 {
     if (Vector3.Distance(RoverOrigPos, mVessel.CoM) < Math.Abs(dc.target)) {
         fs.wheelThrottle = mThrottlePID.Control(dc.speed - RoverSpeed);
         fs.wheelSteer = mWheelPID.Control(RTUtil.ClampDegrees180(RoverHDG - dc.target2));
     } else {
         fs.wheelThrottle = 0;
         fs.wheelSteer = 0;
         mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
         dc.mode = DriveCommand.DriveMode.Off;
         dc = null;
     }
 }
示例#32
0
 private void EnqueueTurn()
 {
     mFlightComputer.Enqueue(DriveCommand.Turn(mSteering, Turn, Speed));
 }
示例#33
0
 private void EnqueueDist()
 {
     mFlightComputer.Enqueue(DriveCommand.Distance(Dist, 0, Speed));
 }
 public void Drive(DriveCommand dc, FlightCtrlState fs)
 {
     if (dc != null) {
         switch (dc.mode) {
             case DriveCommand.DriveMode.Turn:
                 Turn(dc, fs);
                 break;
             case DriveCommand.DriveMode.Distance:
                 Distance(dc, fs);
                 break;
             case DriveCommand.DriveMode.DistanceHeading:
                 DistanceHeading(dc, fs);
                 break;
             case DriveCommand.DriveMode.Coord:
                 Coord(dc, fs);
                 break;
         }
     }
 }
示例#35
0
        public DriveCommand FindNextDriveCommand()
        {
            short ascentHeading = AscentPacketHandler.Compass;

            if (this.isAligningToHeading)
            {
                // Turn toward heading
                // Scan, use heading as reference

                GPS    ascent        = AscentPacketHandler.GPSData;
                double headingToGate = ascent.GetHeadingTo(this.gate);

                Console.Write("Scan aligning toward heading | compass: "******" | Heading to gate: " + headingToGate + " | ");



                // Have reached heading. Start turning right
                if (IMU.IsHeadingWithinThreshold(ascentHeading, headingToGate, HEADING_THRESHOLD))
                {
                    this.isAligningToHeading = false;

                    // If driving for a certain duration
                    if (driveTowardHeadingForMillis > 0)
                    {
                        // Drive straight for a duration
                        this.isDrivingStraightForDuration = true;
                        this.driveStraightUntilMillis     = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() + this.driveTowardHeadingForMillis;

                        return(DriveCommand.Straight(Speed.VISION_SCAN));
                    }
                    else
                    {
                        // Start scan
                        this.isScanning          = true;
                        scanStartHeading         = ascentHeading;
                        scanStartTimestampMillis = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();

                        return(DriveCommand.RightTurn(Speed.VISION_SCAN));
                    }
                }

                // Turn toward heading angle
                if (ascentHeading < ((headingToGate + 180) % 360) && ascentHeading > headingToGate)
                {
                    return(DriveCommand.LeftTurn(Speed.VISION_SCAN));
                }
                else
                {
                    return(DriveCommand.RightTurn(Speed.VISION_SCAN));
                }
            }
            else if (this.isDrivingStraightForDuration)
            {
                if (this.driveStraightUntilMillis > DateTimeOffset.UtcNow.ToUnixTimeMilliseconds())
                {
                    // Drive straight for duration
                    return(DriveCommand.Straight(Speed.VISION_SCAN));
                }
                else
                {
                    // Start scan
                    this.isDrivingStraightForDuration = false;

                    this.isScanning          = true;
                    scanStartHeading         = ascentHeading;
                    scanStartTimestampMillis = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();

                    return(DriveCommand.RightTurn(Speed.VISION_SCAN));
                }
            }
            else
            {
                // Scan
                // ... more to do for this case
                Console.Write("Scan scanning | ");

                // Increment deltaTheta accordingly
                deltaTheta = (ascentHeading - scanStartHeading + 360) % 360;

                Console.Write("Scan scanning | compass: "******" | deltaTheta: " + deltaTheta + " | ");

                if (deltaTheta > 330 && (DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() > scanStartTimestampMillis + 2000))
                {
                    // Use boolean incase of wrap around to 0 due to mod
                    isScanNearlyComplete = true;
                }

                return(DriveCommand.RightTurn(Speed.VISION_SCAN));
            }
        }
示例#36
0
        private void Target()
        {
            if (Event.current.Equals(Event.KeyboardEvent("return")) && GUI.GetNameOfFocusedControl().StartsWith("RC"))
            {
                mFlightComputer.Enqueue(DriveCommand.Coord(mSteerClamp, Latitude, Longitude, Speed));
            }
            else if (GameSettings.MODIFIER_KEY.GetKey() && ((Input.GetMouseButton(0) || Input.GetMouseButton(1)) != MouseClick))
            {
                MouseClick = Input.GetMouseButton(0) || Input.GetMouseButton(1);
                Vector2 latlon;
                if (MouseClick && RTUtil.CBhit(mFlightComputer.Vessel.mainBody, out latlon))
                {
                    Latitude  = latlon.x;
                    Longitude = latlon.y;

                    if (Input.GetMouseButton(1))
                    {
                        mFlightComputer.Enqueue(DriveCommand.Coord(mSteerClamp, Latitude, Longitude, Speed));
                    }
                }
            }

            GUILayout.BeginHorizontal();
            {
                GUILayout.Label(new GUIContent("Wheel: ", "How sharp to turn at max"));
                GUILayout.FlexibleSpace();
                GUILayout.Label(new GUIContent(mSteerClamp.ToString("P"), "How sharp to turn at max"));
                GUILayout.Label(new GUIContent("max", "How sharp to turn at max"), GUILayout.Width(40));
            }
            GUILayout.EndHorizontal();

            RTUtil.HorizontalSlider(ref mSteerClamp, 0, 1);

            GUILayout.BeginHorizontal();
            {
                GUILayout.Label(new GUIContent("LAT.", "Latitude to drive to"), GUILayout.Width(50));
                GUI.SetNextControlName("RC1");
                RTUtil.TextField(ref mLatitude, GUILayout.Width(50), GUILayout.ExpandWidth(false));
                GUILayout.Label(new GUIContent("(°)", "Hold " + GameSettings.MODIFIER_KEY.name + " and click on ground to input coordinates"), GUI.skin.textField, GUILayout.Width(40));
            }
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            {
                GUILayout.Label(new GUIContent("LON.", "Longitude to drive to"), GUILayout.Width(50));
                GUI.SetNextControlName("RC2");
                RTUtil.TextField(ref mLongditude, GUILayout.Width(50), GUILayout.ExpandWidth(false));
                GUILayout.Label(new GUIContent("(°)", "Hold " + GameSettings.MODIFIER_KEY.name + " and click on ground to input coordinates"), GUI.skin.textField, GUILayout.Width(40));
            }
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            {
                GUILayout.Label(new GUIContent("Speed", "Speed to keep"), GUILayout.Width(50));
                GUI.SetNextControlName("RC3");
                RTUtil.TextField(ref mSpeed, GUILayout.Width(50), GUILayout.ExpandWidth(false));
                GUILayout.Label(new GUIContent("(m/s)", "Speed to keep"), GUI.skin.textField, GUILayout.Width(40));
            }
            GUILayout.EndHorizontal();


            mLatitude   = RTUtil.ConstrictNum(mLatitude);
            mLongditude = RTUtil.ConstrictNum(mLongditude);
            mSpeed      = RTUtil.ConstrictNum(mSpeed, false);
        }
示例#37
0
 public void SendDriveCommand(DriveCommand command)
 {
     instance.UpdateVoteCounter(command);
     Trace.TraceInformation("SendDriveCommand: " + command.ToString());
 }
示例#38
0
        private static async void MoveLego(DriveCommand movement)
        {
            switch (movement)
            {
                case DriveCommand.Forward:

                    ////mueve la rueda izquierda
                    await _brick.DirectCommand.TurnMotorAtSpeedForTimeAsync(OutputPort.B, 0x25, 1000, false);
                    ////mueve la rueda derecha
                    await _brick.DirectCommand.TurnMotorAtSpeedForTimeAsync(OutputPort.C, 0x25, 1000, false);

                    break;
                case DriveCommand.Right:
                    ////mueve la rueda izquierda
                    await _brick.DirectCommand.TurnMotorAtSpeedForTimeAsync(OutputPort.B, 0x25, 1000, false);

                    break;
                case DriveCommand.Left:
                    ////mueve la rueda derecha
                    await _brick.DirectCommand.TurnMotorAtSpeedForTimeAsync(OutputPort.C, 0x25, 1000, false);

                    break;
                case DriveCommand.Back:
                    await _brick.DirectCommand.TurnMotorAtSpeedForTimeAsync(OutputPort.B, -0x25, 1000, false);
                    ////mueve la rueda derecha
                    await _brick.DirectCommand.TurnMotorAtSpeedForTimeAsync(OutputPort.C, -0x25, 1000, false);
                    break;
            }
        }
示例#39
0
 private async void Button_Click(object sender, RoutedEventArgs e)
 {
     var          name = ((Button)sender).Name;
     DriveCommand cmd  = (DriveCommand)Enum.Parse(typeof(DriveCommand), name);
     await _lego.SendCommand(cmd);
 }
示例#40
0
 public static extern bool DeviceIoControl(SafeHandle handle,
                                           DriveCommand command, IntPtr inBuffer, int inputSize, ref DISK_PERFORMANCE performance,
                                           int resultSize,
                                           out int bytesReturned, IntPtr overlapped);
示例#41
0
 public static extern bool DeviceIoControl(IntPtr handle,
                                           DriveCommand command, ref DriveCommandParameter parameter,
                                           int parameterSize, out DriveIdentifyResult result, int resultSize,
                                           out uint bytesReturned, IntPtr overlapped);
 private void Turn(DriveCommand dc, FlightCtrlState fs)
 {
     ModuleWheel m = new ModuleWheel();
     if (Math.Abs(Quaternion.Angle(mRoverRot, mVessel.ReferenceTransform.rotation)) < dc.target) {
         fs.wheelThrottle = mThrottlePID.Control(dc.speed - RoverSpeed);
         fs.wheelSteer = dc.steering;
     } else {
         fs.wheelThrottle = 0;
         fs.wheelSteer = 0;
         mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
         dc.mode = DriveCommand.DriveMode.Off;
         dc = null;
     }
 }