/// <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);
}
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();
}
/// <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;
}
}
private void RunCommand(DriveCommand command)
{
Action action = async() => {
txtCommand.Text = command.ToString();
await _lego.SendCommand(command);
};
}
/// <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;
}
public void UpdateVoteCounter(DriveCommand command)
{
if (CurrentVotesCount[command] < Int32.MaxValue)
{
CurrentVotesCount[command]++;
}
Clients.All.updateVotesCounter(CurrentVotesCount);
}
/// <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);
}
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);
}
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)));
}
}
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 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));
}
// 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));
}
}
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();
}
}
}
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();
}
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));
}
}
public static extern bool DeviceIoControl(IntPtr handle, DriveCommand command, ref DriveCommandParameter parameter, int parameterSize, out DriveIdentifyResult result, int resultSize, out uint bytesReturned, IntPtr overlapped);
public void SendDriveCommand(DriveCommand command)
{
instance.UpdateVoteCounter(command);
Trace.TraceInformation("SendDriveCommand: " + command.ToString());
}
public static extern bool DeviceIoControl(SafeHandle handle,
DriveCommand command, ref DriveCommandParameter parameter,
int parameterSize, out DriveSmartReadThresholdsResult result,
int resultSize, out uint bytesReturned, IntPtr overlapped);
private void RunCommand(DriveCommand command)
{
Action action = async () => {
txtCommand.Text = command.ToString();
await _lego.SendCommand(command);
};
}
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);
}
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);
}
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;
}
}
private void EnqueueTurn()
{
mFlightComputer.Enqueue(DriveCommand.Turn(mSteering, Turn, Speed));
}
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;
}
}
}
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));
}
}
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);
}
public void SendDriveCommand(DriveCommand command)
{
instance.UpdateVoteCounter(command);
Trace.TraceInformation("SendDriveCommand: " + command.ToString());
}
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;
}
}
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);
}
public static extern bool DeviceIoControl(SafeHandle handle,
DriveCommand command, IntPtr inBuffer, int inputSize, ref DISK_PERFORMANCE performance,
int resultSize,
out int bytesReturned, IntPtr overlapped);
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;
}
}
