/** Singleton instance and entry into while loop that runs the desired program*/
public static void Main()
{
/* Tracking gamepad buttons states for single press captures*/
bool LastBtn2 = false;
bool LastBtn3 = false;
/* Forword/Backward Scalor */
const float kScalarX = 0.50f;
/* Left/Right Scalor */
const float kScalarY = 0.50f;
/* Turning Scalor */
const float kScalarTwist = 0.30f;
/* Ramp Rate */
const float kVoltageRampSec = 0.2f;
/* Configure Talons to operate in percentage VBus mode, and Ramp Up Voltage*/
foreach (CTRE.TalonSrx temp in Talons)
{
temp.SetControlMode(TalonSrx.ControlMode.kPercentVbus);
temp.SetVoltageRampRate(12.0f / kVoltageRampSec);
}
/* Clear DisplayModule */
LCDDisplay.ClearSprites();
/* Fill the screen with the target object (Top half magenta, bottom half green) */
LCDDisplay.AddRectSprite(CTRE.HERO.Module.DisplayModule.Color.Magenta, 0, 0, 160 / 2, 128);
LCDDisplay.AddRectSprite(CTRE.HERO.Module.DisplayModule.Color.Green, 160 / 2, 0, 160 / 2, 128);
while (true)
{
/* Keep robot enabled if gamepad is connected and in 'D' mode */
if (GamePad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
/* Allow user to enable/disable Display Module backlight
* Button 2 (A) Disables the backlight
* Button 3 (B) Enables the backlight */
bool Btn2 = GamePad.GetButton(2);
bool Btn3 = GamePad.GetButton(3);
if (Btn2 & !LastBtn2)
{
LCDDisplay.BacklightEnable = false;
}
else if (Btn3 & !LastBtn3)
{
LCDDisplay.BacklightEnable = true;
}
LastBtn2 = Btn2;
LastBtn3 = Btn3;
/* Regular mecanum drive that is scaled and Gamepad joysticks have been adjusted */
float X = GamePad.GetAxis(0);
/* Invert gamepad so forward is truly forward */
float Y = -1 * GamePad.GetAxis(1);
float Twist = GamePad.GetAxis(2);
MecanumDrive(Y * kScalarY, X * kScalarX, Twist * kScalarTwist);
}
}
private void HandleCursorPositionUpdate()
{
cursorPosition += mouseCursorSpeed * new Vector3(Input.GetAxisRaw(InputNames.MOUSE_X), Input.GetAxisRaw(InputNames.MOUSE_Y));
float joystickDeltaX = Gamepad.GetAxis(InputNames.JOYSTICK_CURSOR_X);
float joystickDeltaY = Gamepad.GetAxis(InputNames.JOYSTICK_CURSOR_Y);
cursorPosition = cursorPosition + joystickCursorSpeed * new Vector3(joystickDeltaX, joystickDeltaY);
if (cursorPosition.x < 0.0f)
{
cursorPosition.x = 0.0f;
}
if (cursorPosition.x > Screen.width)
{
cursorPosition.x = Screen.width;
}
if (cursorPosition.y < 0)
{
cursorPosition.y = 0;
}
if (cursorPosition.y > Screen.height)
{
cursorPosition.y = Screen.height;
}
}
public static void Main()
{
TalonSrx test = new TalonSrx(0);
Gamepad stick = new Gamepad(UsbHostDevice.GetInstance());
/* loop forever */
while (true)
{
if (stick.GetConnectionStatus() == UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
//This call is redundant but you can un-comment to guarantee limit switches will work or change the mode.
//test.ConfigLimitMode(TalonSrx.LimitMode.kLimitMode_SwitchInputsOnly);
Debug.Print("Rev: " + test.IsRevLimitSwitchClosed() + " | Fwd: " + test.IsFwdLimitSwitchClosed());
Debug.Print("" + test.GetFaults());
if (stick.GetButton(1))
{
test.ClearStickyFaults();
}
test.Set(stick.GetAxis(1));
/* wait a bit */
System.Threading.Thread.Sleep(10);
}
}
public float GetAxis(AxisControl control)
{
float axisValue = 0f;
foreach (AxisCode code in keyBinding.GetAxisCodes(control))
{
axisValue = gamepad.GetAxis(code);
if (!axisValue.Equals(0f))
{
return(axisValue);
}
}
return(0f);
}
private void Update()
{
if (Gamepad != null) // && Ready) {
//Acceleration = Distance / (Time * Time)
//Distance = 0.5f * Acceleration * (Time * Time)
//Distance = Speed * Time
//Speed = Distance / Time
//Time = Distance / Speed
{
CurrentDistance = 0.5f * Gamepad.GetAxis("axis 0") * (Time.deltaTime * Time.deltaTime);
TotalDistance += CurrentDistance;
if (Direction == 0)
{
if (TotalDistance > 0)
{
Direction = 1.0f;
}
else if (TotalDistance < 0)
{
Direction = -1.0f;
}
}
PaddleDistance += TotalDistance * PaddleSpeed;
if (Mathf.Abs(PaddleDistance) > PaddleMove.x)
{
PaddleDistance = Mathf.Sign(PaddleDistance) * PaddleMove.x;
}
rt.localPosition = new Vector3(PaddleDistance, rt.localPosition.y, rt.localPosition.z);
if (AttachedBall != null)
{
if (Gamepad.GetButton("button 0"))
{
AttachedBall.body.bodyType = RigidbodyType2D.Dynamic;
AttachedBall.body.velocity = transform.up * BallLaunchVelocity;
AttachedBall.transform.parent = transform.parent;
AttachedBall = null;
}
}
}
}
private void FixedUpdate()
{
if (Gamepad != null)
{
Vector3 MovementAxis = new Vector3(Gamepad.GetAxis("axis 0"), 0.0f, -Gamepad.GetAxis("axis 1"));
if (MovementAxis.magnitude > 0)
{
character.Movement.Move = new Vector3(
MovementAxis.x,
0.0f,
MovementAxis.z
).normalized;
character.transform.LookAt(character.transform.position + character.Movement.Move);
}
else
{
character.Movement.Move = Vector3.zero;
}
if (Gamepad.GetButton("button 0"))
{
//Kick precedes weapon use
if (character.Kick)
{
character.Kick.Use(gameObject);
}
//character.animator.SetTrigger("Kick");
}
else if (Gamepad.GetButton("button 1"))
{
//Use Weapon
if (character.Weapon)
{
character.Weapon.Use(gameObject);
}
//character.animator.SetTrigger("Attack");
}
}
else
{
character.Movement.Move = Vector3.zero;
}
}
public float Get()
{
float val = 0;
if (controllerID <= 0)
{
// KeyCodes
foreach (KeyCodeAxis key in keys)
{
val += key.Get();
}
}
if (controllerID >= 0)
{
// Gamepad Axis
foreach (Gamepad.Axis a in axis)
{
val += Gamepad.GetAxis(a, controllerID);
}
// Gamepad Buttons
foreach (GamepadButtonAxis button in buttons)
{
val += button.Get();
}
}
val = Mathf.Clamp(val, -1, 1);
if (controllerID <= 0)
{
// Raw Axis
foreach (string r in raw)
{
val += Input.GetAxisRaw(r);
}
}
return(val);
}
private void LateUpdate()
{
if (Gamepad != null)
{
for (b = 0; b < 10; b++)
{
Indicator = Indicators[b];
button = Gamepad.GetButton("button " + b.ToString());
Indicator.SetActive(button);
InputDebug[b] = button ? 1.0f : 0.0f;
}
for (a = 0; a < 8; a++)
{
Indicator = Indicators[10 + a];
axis = Gamepad.GetAxis("axis " + a.ToString());
Indicator.transform.localPosition = Indicator.transform.right * axis;
InputDebug[10 + a] = axis;
}
}
}
private void MoveCameraWithGamepad()
{
Vector3 movement = new Vector3(0, 0, 0);
float xAxis = 0.0f;
float yAxis = 0.0f;
float zAxis = 0.0f;
if (Gamepad.GetButton(InputNames.SELECTION_MODIFIER))
{
yAxis = -1f * Gamepad.GetAxis(InputNames.CAMERA_MOVE_Z);
}
else
{
xAxis = Gamepad.GetAxis(InputNames.CAMERA_MOVE_X);
zAxis = Gamepad.GetAxis(InputNames.CAMERA_MOVE_Z);
}
movement.x = xAxis;
movement.y = yAxis;
movement.z = zAxis;
MoveCamera(movement);
}
/** Singleton instance and entry into while loop that runs the desired program*/
public static void Main()
{
/* Tracking gamepad buttons states for single press captures*/
bool LastBtn1 = false;
bool LastBtn2 = false;
bool LastBtn3 = false;
bool LastBtn5 = false;
bool LastBtn6 = false;
/* Forword/Backward Scalor */
const float kScalarX = 0.50f;
/* Left/Right Scalor */
const float kScalarY = 0.50f;
/* Turning Scalor */
const float kScalarTwist = 0.30f;
/* Ramp Rate */
const float kVoltageRampSec = 0.2f;
/* Gains for PixyDrive (May have to play with this for a better PixyDrive experience)*/
float KpGain = 0.002f; /*P-Gain for Turning*/
float KdGain = 0.001f; /*D-Gain for Turning*/
float kTurnCorrectionRatio = 0.3f; /*Ratio for turning speed*/
float KpGain1 = 0.0003f; /*P-Gain for driving*/
float KdGain1 = 0.0f; /*D-Gain for driving*/
float kForwardCorrectionRatio = 0.6f; /*Ratio for driving speed*/
/* Configure Talons to operate in percentage VBus mode, and Ramp Up Voltage*/
foreach (CTRE.TalonSrx temp in Talons)
{
temp.SetControlMode(TalonSrx.ControlMode.kPercentVbus);
temp.SetVoltageRampRate(12.0f / kVoltageRampSec);
}
/* Initiate the program by starting off in Manaul Drive mode */
CurrentState = States.ManualDrive;
/* Variables to hold target position and distance of object, set when PixyDrive is initialized */
int TargetX = 160;
int TargetArea = 0;
/* Number of values to use when averaging data from Block, Set to 1 to remove average */
int AvgCount = 3;
/* Tracks the Distance between the PixyCamera and the Object using the pervious Area used in PD Loop */
int LastErrorArea = 0;
/* Value used to cycle between 3 colors of the LED on the PixyCamera */
byte RGB = 0;
while (true)
{
/* Keep robot enabled if gamepad is connected and in 'D' mode */
if (GamePad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
/* Clear the average of X, Y, and Area */
int AverageX = 0;
int AverageY = 0;
int AverageArea = 0;
/* Adds up the pervious values of X, Y, and Area */
for (int i = 0; i < AvgCount; i++)
{
/* Run Pixy process to check for Data, pull Block if there is data */
Thread.Sleep(5);
Pixy.Process();
if (Pixy.GetBlock(blockToFill))
{
/* Store block from Pixy into Local Block */
CurrentBlock = blockToFill;
/* Uncomment for block data */
Debug.Print("===================================");
Debug.Print(blockToFill.ToString());
}
///* Pull Block data into variables */
AverageX += CurrentBlock.X; /* Pull X from CurrentBlock */
AverageY += CurrentBlock.Y; /* Pull Y from CurrentBlock */
AverageArea += CurrentBlock.Area; /* Pull Area from CurrentBlock */
}
/* Finishes the average process by dividing the sum by the number of values */
AverageX /= AvgCount;
AverageY /= AvgCount;
AverageArea /= AvgCount;
/* Sync Status of Pixy */
bool Synced = Pixy.Status.Synced;
/* Duration since last time Pixy had a good data in ms */
long TimeSinceLastBlock = Pixy.Status.TimeSinceLastBlock;
/* Uncomment for Pixy status information */
//Debug.Print("===================================");
//Debug.Print(Pixy.Status.ToString());
/* Get angular rate of the robot from Pigeon IMU */
float[] XYZ_Dps = new float[3];
Pidgeot.GetRawGyro(XYZ_Dps);
float CurrentAngularRate = XYZ_Dps[2];
/* Allows for increasing or decreasing the distance between PixyCamera and the target object
* Button 5 (Left-bumper) decreases the range
* Button 6 (Right-bumper) increases the range */
bool Btn5 = GamePad.GetButton(5);
bool Btn6 = GamePad.GetButton(6);
if (Btn5 && !LastBtn5)
{
if (TargetArea < (TargetArea + 350)) /* Minimum Distance found through play */
{
TargetArea += 50; /* Step closer to object */
}
/* Else, the object is too close to PixyCam */
}
if (Btn6 & !LastBtn6)
{
if (TargetArea > (TargetArea - 350)) /* Maximum Distance found through play */
{
TargetArea -= 50; /* Step further from object */
}
/* Else, the object is too far from PixyCam */
}
/* Control the LED on the Pixy Camera
* Button 2 (A) Cycles the color of the LED between Red, Green, and Blue */
bool Btn2 = GamePad.GetButton(2);
if (Btn2 && !LastBtn2)
{
/* Cycle between 3 values by incrementing and wrapping around */
if (RGB == 2)
{
RGB = 0;
}
else
{
RGB++;
}
/* Set the states of the color based on RGB value
* Colors can be modified by doing different combinations of 0 and 255 */
if (RGB == 0)
{
Pixy.SetLED(true, false, false);
}
else if (RGB == 1)
{
Pixy.SetLED(false, true, false);
}
else if (RGB == 2)
{
Pixy.SetLED(false, false, true);
}
}
LastBtn2 = Btn2;
/* Always give user the ability to enter Manaul Drive mode
* Button 1 (X) changes the PixyDrive State to Manual Drive State */
bool Btn1 = GamePad.GetButton(1);
if (Btn1 && !LastBtn1)
{
CurrentState = States.ManualDrive;
}
LastBtn1 = Btn1;
/* State machine that determines if Pixy Target needs to be initiated, Pixy object is in target of Pixy Camera parameters, or
* robot is in Manual Drive mode */
switch (CurrentState)
{
case States.Init:
/* Grab Inital area and X-position, and set as target values */
TargetX = AverageX;
TargetArea = AverageArea;
CurrentState = States.PixyDrive;
break;
case States.PixyDrive:
/* Bulk of the PixyDrive, where it uses the current Pixy data to allign with the TargetArea and TargetX */
if (Synced == true && TimeSinceLastBlock < 50) /* Time since last block should be around 50 ms */
{
/* Object is in View of camera and we getting good data */
/* Find the Error between the desired values between the the current values of Area and X */
int ErrorX = TargetX - AverageX;
int ErrorArea = TargetArea - AverageArea;
/* Variable used later on to smooth out PD of Drive */
int ErrorArea_Kd1 = ErrorArea - LastErrorArea;
/* Track the last error value and compared to next error value */
LastErrorArea = ErrorArea;
/* Checks to see if the error is within an allowable margin,
* ErrorX is good in the sense that there is minimal noise on X and Y
* Error Area can be tweeked according to application as larger objects will have more noise and smallear objects have
* less noise and will require more preciseness for better response */
if ((ErrorX < 3 && ErrorX > -3) && (ErrorArea < 20 && ErrorArea > -20))
{
/* Consisered centerd and a good distance away so stop */
TankDrive(0, 0);
}
else
{
/* There is error with either X or Area, so fix that */
bool xOutOfBounds = OutOfBoundsX(AverageX, CurrentBlock.Width);
/* If object is out bounds, kill the forward and backward drive, turn right or left to still follow */
if (xOutOfBounds)
{
/* Set ErrorArea to 0 to prevent forward/backward drive in PixyDrive */
ErrorArea = 0;
ErrorArea_Kd1 = 0;
}
/* PD Loop for PixyDrive until centered
* Turning uses ErrorX for P and CurrentAngularRate for D
* Forward/Backward uses ErrorArea for P and ErrorArea_Kd1 (ErrorArea - LastErrorArea) for D */
float Turn = (ErrorX) * KpGain - (CurrentAngularRate) * KdGain;
float Forward = (ErrorArea) * KpGain1 + (ErrorArea_Kd1) * KdGain1;
/* Cap the return values to turn down the output of the motors */
Turn = Cap(Turn, kTurnCorrectionRatio);
Forward = Cap(Forward, kForwardCorrectionRatio);
/* With the turn and forward values found, feed that to the TankDrive accordingly */
float LeftAuto = ((-Turn + Forward));
float RightAuto = ((Turn + Forward));
TankDrive(LeftAuto, RightAuto);
}
}
else
{
/* We are in PixyDrive Mode, but there is no object found, so stop */
TankDrive(0, 0);
}
break;
case States.ManualDrive:
/* Allow the user to enter PixyDrive mode
* Button 3 (B) Changes Manual Drive Stat to PixyDrive State */
bool Btn3 = GamePad.GetButton(3);
if (Btn3 && !LastBtn3)
{
CurrentState = States.Init;
}
LastBtn3 = Btn3;
/* Regular mecanum drive that is scaled and Gamepad joysticks have been adjusted */
float X = GamePad.GetAxis(0);
/* Invert gamepad so forward is truly forward */
float Y = -1 * GamePad.GetAxis(1);
float Twist = GamePad.GetAxis(2);
MecanumDrive(Y * kScalarY, X * kScalarX, Twist * kScalarTwist);
break;
}
}
}
// Update is called once per frame
void Update()
{
Vector3 leftStick = new Vector3(playerOne.GetAxis(InputAxis.LEFTX), playerOne.GetAxis(InputAxis.LEFTY), playerOne.GetAxis(InputAxis.LT) - playerOne.GetAxis(InputAxis.RT));
Vector3 rightStick = new Vector3(-playerOne.GetAxis(InputAxis.RIGHTY), 0, -playerOne.GetAxis(InputAxis.RIGHTX));
Vector2 dPad = new Vector2(playerOne.GetAxis(InputAxis.DPADX), playerOne.GetAxis(InputAxis.DPADY));
modifiedObject.Translate(leftStick * 10 * Time.deltaTime, Space.World);
modifiedObject.Rotate(rightStick, 100 * Time.deltaTime, Space.World);
if (dPad.y > 0)
{
mf.mesh = cube;
}
if (dPad.y < 0)
{
mf.mesh = sphere;
}
if (dPad.x > 0)
{
mf.mesh = cylinder;
}
if (dPad.x < 0)
{
mf.mesh = capsule;
}
if (playerOne.GetButtonDown(PressedButton.START))
{
Application.Quit();
}
if (playerOne.GetButtonDown(PressedButton.BACK))
{
cam.gameObject.SetActive(!cam.gameObject.activeInHierarchy);
}
if (playerOne.GetButtonDown(PressedButton.A))
{
mat.color = Color.green;
}
if (playerOne.GetButtonDown(PressedButton.B))
{
mat.color = Color.red;
}
if (playerOne.GetButtonDown(PressedButton.X))
{
mat.color = Color.blue;
}
if (playerOne.GetButtonDown(PressedButton.Y))
{
mat.color = Color.yellow;
}
if (playerOne.GetButtonDown(PressedButton.RB))
{
modifiedObject.localScale *= 2;
}
if (playerOne.GetButtonDown(PressedButton.LB))
{
modifiedObject.localScale /= 2;
}
}
// Update is called once per frame
void Update()
{
Vector3 leftStick = new Vector3(playerOne.GetAxis(InputAxis.LEFTX), -playerOne.GetAxis(InputAxis.LEFTY), (playerOne.LeftTrigger() ? 1 : 0) - (playerOne.RightTrigger() ? 1 : 0));
Vector3 rightStick = new Vector3(-playerOne.GetAxis(InputAxis.RIGHTY), 0, -playerOne.GetAxis(InputAxis.RIGHTX));
Vector2 dPad = new Vector2(playerOne.GetAxis(InputAxis.DPADX), playerOne.GetAxis(InputAxis.DPADY));
modifiedObject.Translate(leftStick * 10 * Time.deltaTime, Space.World);
modifiedObject.Rotate(rightStick, 100 * Time.deltaTime, Space.World);
if (dPad.y > 0)
{
mf.mesh = cube;
}
if (dPad.y < 0)
{
mf.mesh = sphere;
}
if (dPad.x > 0)
{
mf.mesh = cylinder;
}
if (dPad.x < 0)
{
mf.mesh = capsule;
}
if (playerOne.GetButtonDown(PressedButton.START))
{
Application.Quit();
}
if (playerOne.GetButtonDown(PressedButton.BACK))
{
cam.gameObject.SetActive(!cam.gameObject.activeInHierarchy);
}
if (playerOne.GetButtonDown(PressedButton.A))
{
mat.color = Color.green;
}
if (playerOne.GetButtonDown(PressedButton.B))
{
mat.color = Color.red;
}
if (playerOne.GetButtonDown(PressedButton.X))
{
mat.color = Color.blue;
}
if (playerOne.GetButtonDown(PressedButton.Y))
{
mat.color = Color.yellow;
}
if (playerOne.GetButton(InputAxis.LT))
{
modifiedObject.localScale += 2 * Time.deltaTime * Vector3.one;
}
if (playerOne.GetButton(InputAxis.RT))
{
modifiedObject.localScale -= 2 * Time.deltaTime * Vector3.one;
}
if (playerOne.GetButtonDown(PressedButton.LS))
{
playerOne.SwitchInputs(InputAxis.RT, PressedButton.B);
playerOne.SaveCustomScheme(Application.dataPath + "/Controller Input Manager/Demo/PlayerOneControl");
}
if (playerOne.GetButtonDown(PressedButton.RS))
{
playerOne.SwitchInputs(InputAxis.LEFTX, InputAxis.RIGHTX);
playerOne.SwitchInputs(InputAxis.LEFTY, InputAxis.RIGHTY);
playerOne.SaveCustomScheme(Application.dataPath + "/Controller Input Manager/Demo/PlayerOneControl");
}
if (playerOne.GetButtonDown(PressedButton.START))
{
playerOne.ResetToDefault();
}
}
void UpdateSteering()
{
//radius = Mathf.Sqrt(Mathf.Pow(transform.position.x, 2) + Mathf.Pow(transform.position.z, 2));
radiusVelocity = gamepad.GetAxis(AxisCode.GamepadAxisLeftX) * stearingAmount * StearingAmountOverRadius.Evaluate(radius);
}