public override MotorSubFrame GenerateSubFrame(GamePadState state, RobotFrame previousFrame)
{
var leftThruster = new Motor();
var rightThruster = new Motor();
var vertThruster = new Motor();
//Gross forward motion.
leftThruster.Value = state.ThumbSticks.Left.Y;
rightThruster.Value = state.ThumbSticks.Left.Y;
//Take care of turning. Will reduce one motor and increase the other.
leftThruster.Value += state.ThumbSticks.Left.X;
rightThruster.Value -= state.ThumbSticks.Left.X;
vertThruster.Value = state.ThumbSticks.Right.Y;
Constrain(leftThruster);
Constrain(rightThruster);
Constrain(vertThruster);
//Map the thrusters
return(new MotorSubFrame()
{
ThrusterA = leftThruster,
ThrusterB = rightThruster,
ThrusterC = vertThruster
});
//vertThruster.Direction = Math
}
private void CentralClockOnFrameGenerated(RobotFrame robotFrame)
{
BeginInvoke((Action)(() =>
{
LogicMapper <TResponseData> .DashboardModifier.OnRobotFrameGenerated(this, _centralClock, robotFrame);
propA.Value = (int)(robotFrame.Motors.ThrusterA.Value * 100);
propB.Value = (int)(robotFrame.Motors.ThrusterB.Value * 100);
propC.Value = (int)(robotFrame.Motors.ThrusterC.Value * 100);
var lightColor = robotFrame.LightIsOn ? Color.Chartreuse : Color.Red;
if (lblLight.BackColor != lightColor)
{
lblLight.BackColor = lightColor;
}
//Servo Tab
if (outputTabs.SelectedIndex == 2)
{
servo1.Angle = robotFrame.Servos.Servo1.Angle;
servo2.Angle = robotFrame.Servos.Servo2.Angle;
servo3.Angle = robotFrame.Servos.Servo3.Angle;
servo4.Angle = robotFrame.Servos.Servo4.Angle;
}
}));
}
public virtual RobotFrame ProcessData(GamePadState state, RobotFrame previousFrame)
{
return(new RobotFrame()
{
Motors = new MotorSubFrame(),
Servos = new ServoSubFrame(),
});
}
public override ServoSubFrame GenerateSubFrame(GamePadState state, RobotFrame previousFrame)
{
//This is the only time we should be making a new servo sub frame. When previousFrame == null this is the first loop
if (previousFrame == null)
{
return(new ServoSubFrame());
}
//Remember, in this method we do not create a new ServoSubFrame. That gets recycled each time.
var servos = previousFrame.Servos;
var vel = state.Triggers.Right - state.Triggers.Left;
servos.Servo1.SetVelocity(vel);
servos.Servo2.SetVelocity(vel);
servos.Servo3.SetVelocity(-1.0f * vel);
servos.Servo4.SetVelocity(-1.0f * vel);
return(servos);
}
public override void SendFrame(RobotFrame frame)
{
//Standard old variables
_client.SetVariable(CommandField.PropellerA_mode, frame.Motors.ThrusterA.Mode);
_client.SetVariable(CommandField.PropellerA_speed, (int)Math.Abs(frame.Motors.ThrusterA.Value * 100.0));
_client.SetVariable(CommandField.PropellerB_mode, frame.Motors.ThrusterB.Mode);
_client.SetVariable(CommandField.PropellerB_speed, (int)Math.Abs(frame.Motors.ThrusterB.Value * 100.0));
_client.SetVariable(CommandField.PropellerC_mode, frame.Motors.ThrusterC.Mode);
_client.SetVariable(CommandField.PropellerC_speed, (int)Math.Abs(frame.Motors.ThrusterC.Value * 100.0));
_client.SetVariable(CommandField.Light, frame.LightIsOn ? 1:0);
//Servo
//I've used command fields 21 - 24
//They dont exist in the definition of CommandField, so they will have no name
_client.SetVariable((CommandField)10, frame.Servos.Servo1.Angle);
_client.SetVariable((CommandField)11, frame.Servos.Servo2.Angle);
_client.SetVariable((CommandField)12, frame.Servos.Servo3.Angle);
_client.SetVariable((CommandField)13, frame.Servos.Servo4.Angle);
}
public override void sendData(string data)
{
try
{
if (Status != StatusE.CONNECTED)
{
return;
}
var frameToSend = new RobotFrame(data).getFrame();
networkStream.BeginWrite(frameToSend, 0, frameToSend.Length, sendCallback, tcpClient);
}
catch (IOException)
{
disconnect();
Logger.Instance.log(LogLevel.INFO,
string.Format("Device {0} disconnected. Connection terminated by host.", this));
}
catch
{
Logger.Instance.log(LogLevel.INFO, "Could not send data to device: " + deviceName);
}
}
public override ServoSubFrame GenerateSubFrame(GamePadState state, RobotFrame previousFrame)
{
if (previousFrame == null)
{
return(new ServoSubFrame());
}
var servos = previousFrame.Servos;
if (state.Buttons.A == ButtonState.Pressed)
{
servos.Servo1.SetVelocity(1.0f);
}
else if (state.Buttons.B == ButtonState.Pressed)
{
servos.Servo1.SetVelocity(-1.0f);
}
else
{
servos.Servo1.SetVelocity(0.0f);
}
return(servos);
}
public override RobotFrame ProcessData(GamePadState state, RobotFrame previousFrame)
{
//LT - Open Gripper
//RT - Close Gripper
var f = new RobotFrame();
//Add all button states to the frame
f.Buttons.Add(GamePadButton.Start, state.Buttons.Start);
f.Buttons.Add(GamePadButton.A, state.Buttons.A);
f.Buttons.Add(GamePadButton.B, state.Buttons.B);
f.Buttons.Add(GamePadButton.Select, state.Buttons.Back);
f.Buttons.Add(GamePadButton.Xbox, state.Buttons.Guide);
f.Buttons.Add(GamePadButton.LBumper, state.Buttons.LeftShoulder);
f.Buttons.Add(GamePadButton.LStick, state.Buttons.LeftStick);
f.Buttons.Add(GamePadButton.RBumper, state.Buttons.RightShoulder);
f.Buttons.Add(GamePadButton.RStick, state.Buttons.RightStick);
f.Buttons.Add(GamePadButton.X, state.Buttons.X);
f.Buttons.Add(GamePadButton.Y, state.Buttons.Y);
//Add all previous variable states.
if (previousFrame != null)
{
f.LightIsOn = previousFrame.LightIsOn;
//f.camServoUP = previousFrame.camServoUP;
//f.camServoDOWN = previousFrame.camServoDOWN;
}
//Compare all the buttons. If their state has changed since the last frame, then we can call the event
//So, heres what you do to add events: Follow the example bellow. I use LeftBumper for the light, so that
//is a simple example
// ============== Example ==============
if (
Compare( //The compare function will only return true if the button is being pressed (rising),
//ie. has changed from "NotPressed" to "Pressed"
_previousGamePadState, //The state of the game pad LAST time the loop ran
state, //The current state of the game pad
s => s.Buttons.LeftShoulder)) //A lambda expression to choose the property to compare.
//In this case we want to see if LeftShoulder changed
{
//When we get here, we know the button has been pressed down.
//This will notify all other classes that are listening for button presses. Make sure this gets called.
//It can be called before your work, or after.
InvokeButtonPress(GamePadButton.LBumper);
//Now we can do whatever work we want to when the button gets pressed.
//In this case, we'll just invert the boolean variable for LightIsOn in the frame
//Note the elvis operator (?.) and the null coalesce here.
//you cannot guarantee on the first run that _previousFrame will not be null. Account for it.
f.LightIsOn = (!previousFrame?.LightIsOn) ?? false;
}
//You can compact this down (without all the comments) into
//if (Compare(_previousGamePadState, state, s => s.Buttons.LeftShoulder))
//{
// InvokeButtonPress(GamePadButton.LBumper);
// f.LightIsOn = !_previousFrame?.LightIsOn ?? false;
//}
// ============== End Example ==============
if (Compare(_previousGamePadState, state, s => s.Buttons.Start))
{
InvokeButtonPress(GamePadButton.Start);
}
if (Compare(_previousGamePadState, state, s => s.Buttons.A))
{
InvokeButtonPress(GamePadButton.A);
//f.camServoDOWN = !previousFrame?.camServoDOWN ?? false;
}
if (Compare(_previousGamePadState, state, s => s.Buttons.B))
{
InvokeButtonPress(GamePadButton.B);
}
if (Compare(_previousGamePadState, state, s => s.Buttons.Back))
{
InvokeButtonPress(GamePadButton.Select);
}
if (Compare(_previousGamePadState, state, s => s.Buttons.Guide))
{
InvokeButtonPress(GamePadButton.Xbox);
}
if (Compare(_previousGamePadState, state, s => s.Buttons.RightShoulder))
{
InvokeButtonPress(GamePadButton.RBumper);
}
if (Compare(_previousGamePadState, state, s => s.Buttons.LeftStick))
{
InvokeButtonPress(GamePadButton.LStick);
}
if (Compare(_previousGamePadState, state, s => s.Buttons.RightStick))
{
InvokeButtonPress(GamePadButton.RStick);
}
if (Compare(_previousGamePadState, state, s => s.Buttons.X))
{
InvokeButtonPress(GamePadButton.X);
}
if (Compare(_previousGamePadState, state, s => s.Buttons.Y))
{
InvokeButtonPress(GamePadButton.Y);
}
//Dpad
if (Compare(_previousGamePadState, state, s => s.DPad.Up))
{
InvokeButtonPress(GamePadButton.DPadUp);
}
if (Compare(_previousGamePadState, state, s => s.DPad.Down))
{
InvokeButtonPress(GamePadButton.DPadDown);
}
if (Compare(_previousGamePadState, state, s => s.DPad.Left))
{
InvokeButtonPress(GamePadButton.DPadLeft);
}
if (Compare(_previousGamePadState, state, s => s.DPad.Right))
{
InvokeButtonPress(GamePadButton.DPadRight);
}
//Process the joysticks into a motor sub-frame
f.Motors = LogicMapper <ResponseData> .MotorSubFrameProcessor.GenerateSubFrame(state, previousFrame);
//Process the joysticks into a servo sub-frame
f.Servos = LogicMapper <ResponseData> .ServoSubFrameProcessor.GenerateSubFrame(state, previousFrame);
_previousGamePadState = state;
return(f);
}
public void Add(float currentTime)
{
var newFrame = new RobotFrame (RawCentroid, RawAngle, currentTime);
if (frames.Count >= 5)
frames.RemoveAt (0);
frames.Add (newFrame);
}
public void OnRobotFrameGenerated(IDashboardContract dash, CentralClock <T> clock, RobotFrame frame)
{
}
public virtual T GenerateSubFrame(GamePadState state, RobotFrame previousFrame)
{
return(new T());
}
public virtual void SendFrame(RobotFrame frame)
{
//Do nothing
}
