// Build a MotorOutputs protobuf from our simulated motors
public MotorOutputs GetMotorOutputs()
{
MotorOutputs motorOutputs = new MotorOutputs();
foreach (SimMotor simMotor in simMotors)
{
var output = new FRCSim.MotorOutputs.Types.MotorOutput
{
Id = simMotor.Id,
Handle = simMotor.Handle,
Output = simMotor.Output,
Position = simMotor.Position,
SensorPosition = simMotor.SensorPosition,
Velocity = simMotor.Velocity,
IntegralState = simMotor.IntegralState,
LastError = simMotor.LastError
};
motorOutputs.MotorOutput.Add(output);
}
return(motorOutputs);
}
public void Update()
{
if (RobotState.Initialized)
{
MotorOutputs outputs = new MotorOutputs();
foreach (SimMotor motor in simMotors)
{
outputs.MotorOutput.Add(new FRCSim.MotorOutputs.Types.MotorOutput
{
Id = motor.Id,
Handle = motor.Handle,
Output = motor.Output,
Velocity = motor.Velocity,
Position = motor.Position,
LastError = motor.LastError,
IntegralState = motor.IntegralState
});
}
// Notify any listeners that we have new motor outputs
EventManager.PublishMotorOutputsUpdatedEvent(outputs);
}
}
private void MotorOutputsUpdated(MotorOutputs motorOutputs)
{
client.SendUpdatedMotorOutputs(motorOutputs);
}
private async void SendData()
{
var channel = new Channel(javaServer, ChannelCredentials.Insecure);
var conn = new global::FRCSim.RobotService.RobotServiceClient((global::Grpc.Core.ChannelBase)channel);
while (isConnectionOpen) // Run while robot code is running or until the object stops existing
{
try
{
GD.Print("Connecting to robot.");
await conn.ConnectToRobotAsync(new Empty(), new CallOptions().WithWaitForReady());
GD.Print("Connected to robot, waiting for streams to open");
using (var inputCall = conn.Input(null, null, tokenSource.Token))
{
using (var outputsCall = conn.UpdateMotorOutputs(null, null, tokenSource.Token))
{
while (isConnectionOpen)
{
if (NextInputRequest != null)
{
if (!NextInputRequest.Equals(lastInputRequest))
{
lastInputRequest = NextInputRequest;
NextInputRequest = null;
await inputCall.RequestStream.WriteAsync(lastInputRequest);
// GD.Print("Sent input to robot");
}
NextInputRequest = null;
}
if (NextMotorOutputs != null)
{
// GD.Print("SendData has new motor outputs");
if (!NextMotorOutputs.Equals(lastMotorOuputs))
{
lastMotorOuputs = NextMotorOutputs;
NextMotorOutputs = null;
// GD.Print("Sending motor outputs to robot");
await outputsCall.RequestStream.WriteAsync(lastMotorOuputs);
// GD.Print("Sent motor outputs to robot");
}
else
{
// GD.Print("SendData didn't send duplicate motor outputs");
}
NextMotorOutputs = null;
}
Connected = true;
await Task.Delay(LOOP_DELAY); // ms
}
}
}
}
catch (Exception e)
{
GD.Print("Lost connection: " + e);
Connected = false;
await Task.Delay(ERROR_DELAY); // ms
}
}
using (var call = conn.Input())
{
await call.RequestStream.CompleteAsync();
}
using (var call = conn.UpdateMotorOutputs())
{
await call.RequestStream.CompleteAsync();
}
GD.Print("Closed connection");
Connected = false;
isConnectionOpen = false;
}
internal void SendUpdatedMotorOutputs(MotorOutputs outputs)
{
NextMotorOutputs = outputs;
}
public static void PublishMotorOutputsUpdatedEvent(MotorOutputs motorOutputs)
{
// Debug.Log("Publshing new motor outputs");
MotorOutputsUpdated?.Invoke(motorOutputs);
}