/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode)
{
var current = sliders[sliderIndex];
if (!current.Drive(velocity, mode))
{
return(false);
}
if (velocity >= 0)
{
if (current.State == TelescopicState.Maximum)
{
sliderIndex++;
}
}
else
{
if (current.State == TelescopicState.Minimum)
{
sliderIndex--;
}
}
sliderIndex = ClampSliderIndex(sliderIndex);
return(true);
}
// Event handler for when the mode button is clicked.
private void ModeButtonClick(object Sender, RoutedEventArgs Args)
{
// For each button, unset the border.
foreach (Button ModeButton in ButtonList)
{
ModeButton.BorderBrush = Brushes.DimGray;
}
((Button)Sender).BorderBrush = Brushes.Black;
// Set the drive mode.
switch (((Button)Sender).Name)
{
case "TeleOpButton":
RobotDriveMode = DriveMode.TeleOP;
break;
case "AutoButton":
RobotDriveMode = DriveMode.Auto;
break;
case "PracticeButton":
RobotDriveMode = DriveMode.Practice;
break;
case "TestButton":
RobotDriveMode = DriveMode.Test;
break;
}
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode)
{
Displacement += velocity * Time.deltaTime;
Displacement = Mathf.Clamp(Displacement, stroke.min, stroke.max);
MoveSlider(Displacement);
return(DriveRockers(velocity, mode));
}
public static void ProcessModePackets()
{
for (int i = 0; !ModePackets.IsEmpty() && i < NUM_PACKETS_TO_PROCESS; i++)
{
Packet p = ModePackets.Dequeue();
CurDriveMode = (DriveMode)p.Data.Payload[1];
}
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode = DriveMode.Ignore)
{
currentAngle += velocity * Time.deltaTime;
var direction = Quaternion.AngleAxis(currentAngle, transform.forward) * transform.right;
transform.localPosition = StartPosition + GetLocalDirection(direction) * amplitudeRadius;
return(true);
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode)
{
if (!base.OnDrive(velocity, mode))
{
return(false);
}
return(DriveGears(velocity));
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode)
{
if (!crank.Drive(velocity, mode))
{
return(false);
}
return(DriveLinkJoints());
}
static void incrementDrivingMode(float timestep)
{
gVehicleModeTimer += timestep;
if (gVehicleModeTimer > gVehicleModeLifetime)
{
//Increment to next driving mode.
gVehicleModeTimer = 0.0f;
gVehicleOrderProgress++;
releaseAllControls();
//If we are at the end of the list of driving modes then start again.
if (eDRIVE_MODE_NONE == gDriveModeOrder[gVehicleOrderProgress])
{
gVehicleOrderProgress = 0;
gVehicleOrderComplete = true;
}
//Start driving in the selected mode.
DriveMode eDriveMode = gDriveModeOrder[gVehicleOrderProgress];
switch (eDriveMode)
{
case eDRIVE_MODE_ACCEL_FORWARDS:
startAccelerateForwardsMode();
break;
case eDRIVE_MODE_ACCEL_REVERSE:
startAccelerateReverseMode();
break;
case eDRIVE_MODE_HARD_TURN_LEFT:
startTurnHardLeftMode();
break;
case eDRIVE_MODE_HANDBRAKE_TURN_LEFT:
startHandbrakeTurnLeftMode();
break;
case eDRIVE_MODE_HARD_TURN_RIGHT:
startTurnHardRightMode();
break;
case eDRIVE_MODE_HANDBRAKE_TURN_RIGHT:
startHandbrakeTurnRightMode();
break;
case eDRIVE_MODE_BRAKE:
startBrakeMode();
break;
case eDRIVE_MODE_NONE:
break;
}
;
}
}
/// <summary>
/// Drive the rockers that join at this mechanism.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected bool DriveRockers(float velocity, DriveMode mode)
{
foreach (var rocker in rockers)
{
if (!rocker.Drive(velocity, mode))
{
return(false);
}
}
return(true);
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity = 0, DriveMode mode = DriveMode.Ignore)
{
joint.position = transform.position;
var angle = -VectorUtility.ProjectAngle(joint.forward, ZeroAxis, Axis);
var euler = new Vector3(transform.localEulerAngles.x, transform.localEulerAngles.y, angle);
transform.localRotation = Quaternion.Euler(euler);
return(true);
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode)
{
foreach (var unit in mechanismUnits)
{
if (!unit.Drive(velocity, mode))
{
return(false);
}
}
return(true);
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode = DriveMode.Ignore)
{
currentOffset += velocity * Mathf.Deg2Rad * direction * Time.deltaTime;
if (currentOffset < -amplitudeRadius || currentOffset > amplitudeRadius)
{
direction *= -1;
currentOffset = Mathf.Clamp(currentOffset, -amplitudeRadius, amplitudeRadius);
}
transform.localPosition = StartPosition + LocalAxis * currentOffset;
return(true);
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode)
{
if (mode == DriveMode.Linear)
{
velocity *= Mathf.Rad2Deg / radius;
}
Angle += velocity * Time.deltaTime;
RotateCrank(Angle);
return(DriveRockers(velocity, mode));
}
public void DriveAuto(int heading)
{
forward_joystick.SetActive(false);
forward_linear.gameObject.SetActive(true);
forward_manual.SetActive(false);
socket.SetForwardMotorsSpeed(0, 0);
socket.SetAutoHeading(true, heading);
drivemode = DriveMode.Auto;
}
void PacketReceiveCallback(IAsyncResult ar)
{
IPEndPoint endpoint = new IPEndPoint(IPAddress.Any, 0);
byte[] buffer = null;
try
{
buffer = packetSocket.EndReceive(ar, ref endpoint);
}
catch (SocketException)
{
logger.Write(LoggerLevel.Error, "Connection closed - Robot code is not running or has crashed!");
connectionThread.Abort();
return;
}
packetSocket.BeginReceive(PacketReceiveCallback, null);
Opcode opcode = handler.GetPacketOpcode(buffer, buffer.Length);
switch (opcode)
{
case Opcode.REPORT_HEARTBEAT:
lastHeartbeatReceived = DateTime.Now;
logger.Write(LoggerLevel.Info, "ReportHeartbeat received at " + DateTime.Now.ToString("h:mm:ss tt"));
break;
case Opcode.REMOTE_SWITCH:
currentDriveMode = DriveMode.Remote;
Dispatcher.BeginInvoke(new Action(() =>
{
for (int i = 0; i < robotModeListBox.Items.Count; i++)
{
var boxItem = robotModeListBox.Items[i] as ComboBoxItem;
var item = boxItem.Content as string;
if (item == "Remote Drive")
{
robotModeListBox.SelectedIndex = i;
break;
}
}
}));
logger.Write(LoggerLevel.Warning, "LAME detected AI timeout and has reverted to manual control.");
break;
case Opcode.SWITCH_MODE_ACK:
receivedDriveModeAck = true;
break;
default:
return;
}
}
public void ToggleCruiseMode()
{
if (driveMode == DriveMode.Cruise)
{
driveMode = DriveMode.Controlled;
}
else
{
driveMode = DriveMode.Cruise;
cruiseTargetSpeed = CurrentSpeed;
accellInput = 0.0f;
}
}
public static void ProcessModePackets()
{
for (int i = 0; !ModePackets.IsEmpty() && i < NUM_PACKETS_TO_PROCESS; i++)
{
Packet p = ModePackets.Dequeue();
CurDriveMode = (DriveMode)p.Data.Payload[0];
//temporary fix to test, actually fix it later to get corect values form payload
if (p.Data.Payload[0] > 0)
{
CurDriveMode = DriveMode.toGPS;
}
}
}
public void Backtrack()
{
if (replay.ReplayAny())
{
print(name + " - ignoring backtrack command (replay)");
return;
}
StopBacktrack();
PrepareBacktrackDump(GetBacktrackFilename());
InitReplayFrom (GetBacktrackFilename());
mode = DriveMode.Backtrack;
StartExclusiveReplay();
}
public void GearboxShiftDown()
{
if (Mathf.RoundToInt(currentGear) == 1)
{
InReverse = true;
driveMode = DriveMode.Controlled;
headlights.Reverselights = InReverse;
return;
}
lastGear = Mathf.RoundToInt(currentGear);
targetGear = lastGear - 1;
lastShift = Time.time;
shifting = true;
}
public void GearboxShiftDown()
{
//Debug.Log("GearboxShiftDown");
if (Mathf.RoundToInt(currentGear) == 1)
{
InReverse = true;
driveMode = DriveMode.Controlled;
Reverselights = InReverse;
return;
}
lastGear = Mathf.RoundToInt(currentGear);
Gear = lastGear - 1;
lastShift = Time.time;
IsShifting = true;
}
public void DriveAhead(float distance_cm, float speed_cm_per_sec)
{
if (replay.ReplayAny())
{
print(name + " - ignoring drive command (replay)");
return;
}
StopBacktrack();
mode = DriveMode.Auto;
packet.timestamp_us = Timestamp.TimestampUs();
packet.command = DrivePacket.Commands.TO_POSITION_WITH_SPEED;
DistanceAndSpeedToEngineCountsAndSpeed (distance_cm, speed_cm_per_sec, out packet.param1, out packet.param2, out packet.param3, out packet.param4);
Send(packet);
timeSinceLastPacketMs = 0.0f;
}
public void ToggleShift()
{
if (InReverse)
{
InReverse = false;
headlights.Reverselights = InReverse;
}
else
{
currentGear = 1;
InReverse = true;
driveMode = DriveMode.Controlled;
headlights.Reverselights = InReverse;
}
// dash ui
ChangeDashState(DashStateTypes.Shift, InReverse ? 0 : 1);
}
public MainWindow()
{
InitializeComponent();
logger = Logger.GetInstance();
handler = new PacketHandler();
lastHeartbeatReceived = DateTime.MinValue;
receivedDriveModeAck = false;
xboxService = new Xbox360Service();
bool xbox_init_ok = xboxService.Initialize();
if (!xbox_init_ok)
{
logger.Write(LoggerLevel.Warning, "Failed to initialize the Xbox360Service. Joysticks are inoperable for this session.");
}
packetSocket = new UdpClient(PKT_PORT);
packetSocket.BeginReceive(PacketReceiveCallback, null);
imgListener = new TcpListener(IPAddress.Any, IMG_PORT);
imgListener.Start();
imgListenerThread = new Thread(ImageListenerThread);
imgListenerThread.SetApartmentState(ApartmentState.STA);
imgListenerThread.Start();
queryHeartbeatTimer = new System.Timers.Timer(QUERY_HEARTBEAT_INTERVAL);
queryHeartbeatTimer.Elapsed += new ElapsedEventHandler(QueryHeartbeatEvent);
queryHeartbeatTimer.Enabled = true;
isConnected = false;
windowClosing = false;
currentDriveMode = DriveMode.Direct;
connectionThread = new Thread(ConnectingWorker);
joystickReadThread = new Thread(ReadJoystickValues);
joystickReadThread.IsBackground = true;
joystickReadThread.Start();
qualitySlider.Value = 15; // default
dispSlider.Value = -1;
vibSlider.Value = -1;
}
public void SetDriveMode(DriveMode mode)
{
m_drive_mode = mode;
switch (mode)
{
case DriveMode.FORWARD:
m_current_gear_ratio = GetForwardGearRatio();
break;
case DriveMode.REVERSE:
m_current_gear_ratio = GetReverseGearRatio();
break;
case DriveMode.NEUTRAL:
m_current_gear_ratio = 1e20;
break;
}
}
public void GearboxShiftDown()
{
if (Mathf.RoundToInt(currentGear) == 1)
{
InReverse = true;
driveMode = DriveMode.Controlled;
headlights.Reverselights = InReverse;
// dash ui
ChangeDashState(DashStateTypes.Shift, InReverse ? 0 : 1);
return;
}
lastGear = Mathf.RoundToInt(currentGear);
targetGear = lastGear - 1;
lastShift = Time.time;
shifting = true;
}
void Update ()
{
if (replay.ReplayAny())
return;
timeSinceLastPacketMs += Time.deltaTime*1000.0f;
if (timeSinceLastPacketMs < packetDelayMs)
return;
if (IsManualInput ())
{
if (mode == DriveMode.Auto)
{
mode = DriveMode.Manual;
return;
}
if (mode == DriveMode.Backtrack)
StopReplay ();
}
if (mode == DriveMode.Backtrack)
{
if (!ReplayRunning)
mode = DriveMode.Manual;
else
return;
}
packet.timestamp_us = Timestamp.TimestampUs();
if(mode == DriveMode.Auto)
packet.command = DrivePacket.Commands.KEEPALIVE;
else if (mode == DriveMode.Manual)
{
packet.command = DrivePacket.Commands.SET_SPEED;
InputToEngineSpeeds (Input.GetAxis(input.horizontal), Input.GetAxis(input.vertical), (1.0f-input.accelerationPower) + input.accelerationPower *Input.GetAxis(input.acceleration), out packet.param1,out packet.param2);
}
Send(packet);
timeSinceLastPacketMs = 0.0f;
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
public virtual bool Drive(float velocity, DriveMode mode)
{
if (Application.isPlaying)
{
if (IsStuck)
{
if (velocity * lastVelocity > 0)
{
return(false);
}
}
else
{
lastVelocity = velocity;
}
}
isDriveUnrestricted = OnDrive(velocity, mode);
return(isDriveUnrestricted);
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode)
{
var angular = velocity;
var linear = velocity;
if (mode == DriveMode.Linear)
{
angular = velocity / radius * Mathf.Rad2Deg;
}
else
{
linear = velocity * Mathf.Deg2Rad * radius;
}
transform.Rotate(Vector3.forward, angular * Time.deltaTime, Space.Self);
if (!DriveCoaxes(angular))
{
return(false);
}
return(DriveEngages(-linear));
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode = DriveMode.Ignore)
{
mode = DriveMode.Angular;
if (!leftGear.Drive(velocity * coefficient, mode))
{
return(false);
}
if (!rightGear.Drive(velocity * coefficient, mode))
{
return(false);
}
if (!leftAxle.Drive(-velocity * leftCoefficient, mode))
{
return(false);
}
return(rightAxle.Drive(velocity * rightCoefficient, mode));
}
/// <summary>
/// Drive mechanism by velocity.
/// </summary>
/// <param name="velocity">Velocity of drive.</param>
/// <param name="mode">Mode of drive.</param>
/// <returns>Drive is unrestricted?</returns>
protected override bool OnDrive(float velocity, DriveMode mode)
{
var angular = velocity;
var linear = velocity;
if (mode == DriveMode.Linear)
{
angular = velocity / radius * Mathf.Rad2Deg;
}
else
{
linear = velocity * Mathf.Deg2Rad * radius;
}
if (!DriveCoaxes(angular))
{
return(false);
}
return(DriveEngages(-linear));
}
private void cmdAnimation_Click(object sender, RoutedEventArgs e)
{
driveMode = DriveMode.Animation;
Sync();
}
private void cmdStatic_Click(object sender, RoutedEventArgs e)
{
driveMode = DriveMode.Static;
Sync();
}
private void Drive(DriveMode dm) // Drives the LEDs
{
switch (dm)
{
case DriveMode.Animation: // Animation Mode
{
if (!running)
{
running = true;
Stopwatch sw = new Stopwatch();
while (!finish)
{
if (animation == null)
{
running = false;
return;
}
Animation operation = new Animation(animation);
Animation_UpdateRequested = false;
DefaultConfiguartion();
List<List<LED>> leds = Animation.GetNeededLEDs(operation);
while(!Animation_UpdateRequested)
{
for (int i = 0; i < operation.AnimationModel.Count; i++)
{
sw.Restart();
while (sw.ElapsedMilliseconds < operation.Durations[i])
{
for (int j = 0; j < leds[i].Count; j++)
{
UpdateLED(leds[i][j]);
UpdateLED(leds[i][j], !leds[i][j].State);
}
if (cancelAnimation.IsCancellationRequested)
{
running = false;
return;
}
}
sw.Stop();
}
}
}
running = false;
}
}
break;
case DriveMode.Static: // Static Mode
{
if (!running)
{
running = true;
while (!finish)
{
if (image == null)
{
running = false;
return;
}
Grid operation = new Grid(image.Map);
Image_UpdateRequested = false;
DefaultConfiguartion();
List<LED> leds = Grid.GetNeededLEDs(operation);
while(!Image_UpdateRequested)
{
for (int i = 0; i < leds.Count; i++)
{
UpdateLED(leds[i]);
UpdateLED(leds[i], !leds[i].State);
if (cancelAnimation.IsCancellationRequested)
{
running = false;
return;
}
}
}
}
running = false;
}
}
break;
}
}
public async void StartDriverAsync(DriveMode dm) // Starts the driver
{
try
{
cancelAnimation = new CancellationTokenSource(); // Sets a new token
finish = false; // Sets finish to false
Task anim = Task.Factory.StartNew(() => Drive(dm), cancelAnimation.Token);
await anim; // Starts a new task and..
} // ..awaits it
catch(OperationCanceledException)
{
// Operation gets cancelled
}
}
/// <summary>
/// Drive mode end
/// </summary>
/// <param name="driveMode"></param>
/// <param name="e"></param>
public static void DriveModeEnd(DriveMode driveMode, CancelEventArgs e)
{
if (driveMode != null && driveMode.IsOpen)
{
driveMode.StopDriveMode();
e.Cancel = true;
}
}
