// controller link dispatcher
public void Link(object sender, EventArgs dontcare)
{
// get state of player one
currentState = GamePad.GetState(PlayerIndex.One);
// if controller is connected...
if (currentState.IsConnected)
{
// ...say its connected in the textbloxk, color it green cuz its good to go
LinkTextBlock.Text = "Controller: Connected";
LinkTextBlock.Foreground = Brushes.Green;
foreach (Buttons b in Enum.GetValues(typeof(Buttons)))
{
Button(b);
}
double left_y = currentState.ThumbSticks.Left.Y;
double left_x = -currentState.ThumbSticks.Left.X;
if (_adr)
{
//left_x *= -1;
left_y *= -1;
}
gm.Twist vel = new gm.Twist { linear = new gm.Vector3 { x = left_y * _trans.Value }, angular = new gm.Vector3 { z = left_x * _rot.Value } };
if(velPub != null)
velPub.publish(vel);
//arm controls via joystick are done here.
double right_y = currentState.ThumbSticks.Right.Y;
// this is inverted to reflect mikes arm driver. right requires a negative number, not the default positive value
double right_x = -1 * currentState.ThumbSticks.Right.X;
double right_trigger = currentState.Triggers.Right;
if (!engaged && (Math.Abs(right_y) > .1 || Math.Abs(right_x) > .1 ))
{
engaged = true;
ArmON.publish(new m.Bool() { data = true });
Arm_Engaged.Content = "Arm Engaged";
Arm_Engaged.Background = Brushes.White;
Arm_Engaged.Foreground = Brushes.Green;
}
//if trigger is not pressed, send close signal ( -1 ). Th goal is to have the gripper
// going to a close state when the right trigger is not being pressed.
if (right_trigger == 0)
right_trigger = -1;
/*Console.WriteLine( "joy_right_x: " + right_x.ToString());
Console.WriteLine( "joy_right_y: " + right_y.ToString());
Console.WriteLine( "right trigger: " + right_trigger.ToString());*/
am.ArmMovement armmove = new am.ArmMovement();
armmove.pan_motor_velocity = right_x;
armmove.tilt_motor_velocity = right_y;
armmove.gripper_open = (right_trigger >= 0.5);
if (armPub != null)
armPub.publish(armmove);
}
// unless if controller is not connected...
else if (!currentState.IsConnected)
{
// ...have program complain controller is disconnected
LinkTextBlock.Text = "Controller: Disconnected";
LinkTextBlock.Foreground = Brushes.Red;
}
}
// controller link dispatcher
public void Link(object sender, EventArgs dontcare)
{
// get state of player one
currentState = GamePad.GetState(PlayerIndex.One);
// if controller is connected...
if (currentState.IsConnected)
{
// ...say its connected in the textbloxk, color it green cuz its good to go
LinkTextBlock.Text = "Controller: Connected";
LinkTextBlock.Foreground = Brushes.Green;
foreach (Buttons b in Enum.GetValues(typeof(Buttons)))
{
Button(b);
}
double left_y = currentState.ThumbSticks.Left.Y;
double left_x = -currentState.ThumbSticks.Left.X;
if (_adr)
{
//left_x *= -1;
left_y *= -1;
}
gm.Twist vel = new gm.Twist {
linear = new gm.Vector3 {
x = left_y * _trans.Value
}, angular = new gm.Vector3 {
z = left_x * _rot.Value
}
};
if (velPub != null)
{
velPub.publish(vel);
}
//arm controls via joystick are done here.
double right_y = currentState.ThumbSticks.Right.Y;
// this is inverted to reflect mikes arm driver. right requires a negative number, not the default positive value
double right_x = -1 * currentState.ThumbSticks.Right.X;
double right_trigger = currentState.Triggers.Right;
if (!engaged && (Math.Abs(right_y) > .1 || Math.Abs(right_x) > .1))
{
engaged = true;
ArmON.publish(new m.Bool()
{
data = true
});
Arm_Engaged.Content = "Arm Engaged";
Arm_Engaged.Background = Brushes.White;
Arm_Engaged.Foreground = Brushes.Green;
}
//if trigger is not pressed, send close signal ( -1 ). Th goal is to have the gripper
// going to a close state when the right trigger is not being pressed.
if (right_trigger == 0)
{
right_trigger = -1;
}
/*Console.WriteLine( "joy_right_x: " + right_x.ToString());
* Console.WriteLine( "joy_right_y: " + right_y.ToString());
* Console.WriteLine( "right trigger: " + right_trigger.ToString());*/
am.ArmMovement armmove = new am.ArmMovement();
armmove.pan_motor_velocity = right_x;
armmove.tilt_motor_velocity = right_y;
armmove.gripper_open = (right_trigger >= 0.5);
if (armPub != null)
{
armPub.publish(armmove);
}
}
// unless if controller is not connected...
else if (!currentState.IsConnected)
{
// ...have program complain controller is disconnected
LinkTextBlock.Text = "Controller: Disconnected";
LinkTextBlock.Foreground = Brushes.Red;
}
}
