void TeleopPeriodic() {
SmartDashboard::PutNumber("button RT", stick->GetRightThrottle());
//TODO change if using game component stick
if (stick->GetButtonY()) // turn lights on with button y
{
light->LightsOn();
}
else if (stick->GetRightThrottle() > .2) // make lights blink faster as right throttle pressed
{
light->Blink(1-stick->GetRightThrottle());
}
else // lights off otherwise
{
light->LightsOff();
}
// make lights blink if right axis in top right and bottom left quarters
if ((stick->GetRightYAxis() > 0 && stick->GetRightXAxis() > 0)
|| (stick->GetRightYAxis() < 0 && stick->GetRightXAxis() < 0))
{
light->LightsOn();
}
else // turn lights off otherwise
{
light->LightsOff();
}
double y = -stick->GetLeftYAxis();
double x = stick->GetLeftXAxis();
SmartDashboard::PutNumber("left y", y);
SmartDashboard::PutNumber("left x", x);
// a work in progress to make it on and off in each eighth of the left axis
/* if (y > 0 && ( (y > x && x > 0) || (y < x && x < 0) ) )
{
light->LightsOn();
}
else if (y < 0 && ((x < 0 && abs(y) > x) || (abs(y) < x && x > 0)) )
{
light->LightsOn();
}
else
{
light->LightsOff();
}*/
}
void TeleopPeriodic()
{
if (stick->GetButtonA() == true) {
sold->OverrideEnable();
printf("It got here\n");
}
printf("after getbutton\n");
}
void TeleopPeriodic()
{
SmartDashboard::PutNumber("Pivot Encoder Value: ", bman->GetPivotEncoder());
SmartDashboard::PutNumber("Right X Axis: ", joystick->GetRightXAxis());
SmartDashboard::PutNumber("Right Y Axis: ", joystick->GetRightYAxis());
SmartDashboard::PutNumber("Left X Axis: ", joystick->GetLeftXAxis());
SmartDashboard::PutNumber("Left Y Axis: ", joystick->GetLeftYAxis());
SmartDashboard::PutNumber("Right Twist: ", joystick->GetRightTwist());
SmartDashboard::PutBoolean("Top: ", bman->GetTopLS());
SmartDashboard::PutBoolean("Bottom: ", bman->GetBottomLS());
if (joystick->GetButtonRB() == true) {
bman->ChangeSpeed();
}
if (joystick->GetButtonB() == true || pivotPressed == true) {
bman->PivotBall();
pivotPressed = true;
}
float leftYValue = joystick->GetLeftYAxis();
float rightYValue = joystick->GetRightYAxis();
// pull in/push out ball using left joystick on xbox
if (leftYValue < -MOVE_AXIS) {
bman->PushOut();
}
else if (leftYValue > MOVE_AXIS) {
//printf("%d\n", bman->GetPivotEncoder());
bman->PullIn();
}
else {
bman->StopBallMotor();
}
// move defenses up/down using right joystick on xbox
if (rightYValue < -MOVE_AXIS) {
bman->DefenseUp();
}
else if (rightYValue > MOVE_AXIS) {
bman->DefenseDown();
}
else {
bman->StopPivotMotor();
}
}
void OperatorControl(void)
{
bool buttonABool;
bool buttonBBool;
bool buttonXBool;
bool buttonYBool;
bool buttonLBBool;
bool buttonRBBool;
bool buttonBackBool;
bool buttonStartBool;
float leftTrigger;
float rightTrigger;
float leftYAxis;
float rightYAxis;
float leftXAxis;
float rightXAxis;
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
leftYAxis = newStick.GetLeftYAxis();
rightYAxis = newStick.GetRightYAxis();
SmartDashboard::PutNumber("Left Y Axis", leftYAxis);
SmartDashboard::PutNumber("Right Y Axis", rightYAxis);
leftXAxis = newStick.GetLeftXAxis();
rightXAxis = newStick.GetRightXAxis();
SmartDashboard::PutNumber("Left X Axis", leftXAxis);
SmartDashboard::PutNumber("Right X Axis", rightXAxis);
leftTrigger = newStick.GetLeftThrottle();
SmartDashboard::PutNumber("left trigger", leftTrigger);
rightTrigger = newStick.GetRightThrottle();
SmartDashboard::PutNumber("right trigger", rightTrigger);
buttonABool = newStick.GetButtonA();
SmartDashboard::PutNumber("buttonA",buttonABool);
buttonBBool = newStick.GetButtonB();
SmartDashboard::PutNumber("buttonB",buttonBBool);
buttonXBool = newStick.GetButtonX();
SmartDashboard::PutNumber("buttonX",buttonXBool);
buttonYBool = newStick.GetButtonY();
SmartDashboard::PutNumber("buttonY",buttonYBool);
buttonLBBool = newStick.GetButtonLB();
SmartDashboard::PutNumber("buttonLB",buttonLBBool);
buttonRBBool = newStick.GetButtonRB();
SmartDashboard::PutNumber("buttonRB",buttonRBBool);
buttonBackBool = newStick.GetButtonBack();
SmartDashboard::PutNumber("buttonBack",buttonBackBool);
buttonStartBool = newStick.GetButtonStart();
SmartDashboard::PutNumber("buttonStart",buttonStartBool);
Wait(0.005);
}
}
void TeleopPeriodic() {
//for gear piston
printf("in teleop per \n");
if (xbox->GetButtonB() && BnotPressed) {
gefu->GearOut();
BnotPressed=false;
printf("gearout \n");
}
else if (!xbox->GetButtonB()) {
BnotPressed=true;
}
if (xbox->GetButtonX() && XnotPressed) {
gefu->GearIn();
XnotPressed=false;
printf("gearin \n");
}
else if(!xbox->GetButtonX()) {
XnotPressed=true;
}
// for intake (piston 2)
if(xbox->GetButtonA() && AnotPressed) {
gefu->Horz();
AnotPressed=false;
printf("horz \n");
}
else if (!xbox->GetButtonA()) {
AnotPressed=true;
}
if (xbox->GetButtonY() && (YnotPressed)) {
gefu->Horz2();
YnotPressed=false;
printf("horz2 \n");
}
else if(!xbox->GetButtonY()) {
YnotPressed=true;
}
// for outtake (piston 3)
if (xbox->GetButtonLB() && (LBnotPressed)) {
gefu->Vert();
LBnotPressed=false;
printf("vert \n");
}
else if(!xbox->GetButtonLB()) {
LBnotPressed=true;
}
if (xbox->GetButtonRB() && (RBnotPressed)) {
gefu->Vert2();
RBnotPressed=false;
printf("vert2 \n");
}
else if(!xbox->GetButtonRB()) {
RBnotPressed=true;
}
}
