void TeleopPeriodic() {
if(m_driver->GetRawButton(BUTTON_LB)) {
// PYRAMID
m_PIDController->SetSetpoint(PLATE_PYRAMID_THREE_POINT);
m_PIDController->Enable();
} else if(m_driver->GetRawButton(BUTTON_RB)) {
// FEEDER
m_PIDController->SetSetpoint(PLATE_FEEDER_THREE_POINT);
m_PIDController->Enable();
} else if(m_driver->GetRawAxis(TRIGGERS) > 0.5) {
m_PIDController->SetSetpoint(PLATE_TEN_POINT_CLIMB);
m_PIDController->Enable();
} else {
// MANUAL CONTROL
m_PIDController->Disable();
m_plate1->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
m_plate2->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
}
// ----- PRINT -----
SmartDashboard::PutNumber("Plate Position: ", m_plateSensor->GetVoltage());
SmartDashboard::PutNumber("PID GET: ", m_plateSensor->PIDGet());
} // TeleopPeriodic()
void TeleopPeriodic()
{
myRobot.ArcadeDrive(-gPad.GetRawAxis(GP_RSTICK_YAXIS)*ROBOT_START_SPEED, -gPad.GetRawAxis(GP_RSTICK_XAXIS)*ROBOT_START_SPEED ); // drive with arcade style (use right stick)
if(gPad.GetRawButton(GP_Y_BUTTON) == true && yButton == false)
{
if(driveSpeed + 0.25 < 1 && driveSpeed - 0.25 > 0)
{
yButton = true;
driveSpeed = driveSpeed + 0.25;
}
else
{
yButton = false;
}
}
if(gPad.GetRawButton(GP_A_BUTTON) == true && aButton == false)
{
if(driveSpeed + 0.25 < 1 && driveSpeed - 0.25 > 0)
{
aButton = true;
driveSpeed = driveSpeed + 0.25;
}
else
{
aButton = false;
}
}
}
void ArcadeDriveWithJoystick::Execute() {
Joystick* stick = CommandBase::oi->GetJoystick();
float moveValue = stick->GetRawAxis(1);
float rotateValue = stick->GetRawAxis(4);
CommandBase::drivetrain->Drive(moveValue, rotateValue);
CommandBase::drivetrain->Log();
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
while (IsOperatorControl())
{
left.Set(cont.GetRawAxis(2));
right.Set(cont.GetRawAxis(4));
}
}
//void StartAutomaticCapture(std::shared_ptr<USBCamera>cam0);
void TeleopPeriodic() {
// ui.GetData(&wui);
// m_tank.Drive(wui.LeftSpeed, wui.RightSpeed);
//
// m_shooter.Rotate(wui.RotateSpeed*3); //70 degrees per second at full value
// m_shooter.Lift(wui.LiftSpeed*1.193); //4 seconds for 180 degree revolution
// if(wui.SpinUp) {
// m_shooter.Spinup(1);
// }
// if(wui.Shoot) {
// m_shooter.Shoot();
// }
// if(wui.Pickup) {
// m_shooter.Pickup();
// }
//
// m_suspension.SetFrontLeft(wui.DropFL);
// m_suspension.SetBackLeft(wui.DropBL);
// m_suspension.SetFrontRight(wui.DropFR);
// m_suspension.SetBackRight(wui.DropBR);
// m_leddar.GetDetections();
// m_shooter.Update();
//float RTrigger = m_lStick->GetRawAxis(3);
//float LTrigger = m_lStick->GetRawAxis(2);
//if (m_PWMTalonLeftFrontTop == .5)
//if (abs(RTrigger) < 0.2)
//RTrigger = 0;
//if (abs(LTrigger) < 0.2)
//LTrigger = 0;
float leftSpeed = m_lStick->GetRawAxis(1);
float rightSpeed = m_lStick->GetRawAxis(5);
if (abs(leftSpeed) < 0.2)
leftSpeed = 0;
if (abs(rightSpeed) < 0.2)
rightSpeed = 0;
//float LTrigger = m_lStick->GetRawAxis(3);
//float RTrigger = m_lStick->GetRawAxis(2);
SmartDashboard::PutNumber("Left Stick", leftSpeed);
SmartDashboard::PutNumber("Right Stick", rightSpeed);
//SmartDashboard::PutNumber("L Trigger", LTrigger);
//SmartDashboard::PutNumber("R Trigger", RTrigger);
SmartDashboard::PutNumber("Left Encoder", leftEncoder->Get());
SmartDashboard::PutNumber("Right Encoder", rightEncoder->Get());
drive->TankDrive(leftSpeed, rightSpeed, true);
//drive->TankDrive(RTrigger, LTrigger, true);
LEFTDRIVE1->Set(leftSpeed);
LEFTDRIVE2->Set(leftSpeed);
RIGHTDRIVE1->Set(rightSpeed);
RIGHTDRIVE2->Set(rightSpeed);
//m_PWMTalonLeftFrontTop->Set(RTrigger);
//m_PWMTalonRightFrontTop->Set(RTrigger);
//m_PWMTalonRightRearTop->Set(LTrigger);
//m_PWMTalonLeftRearTop->Set(LTrigger);
}
void TeleopContinuous(void) {
printf("Running in teleop continuous...\n");
GetWatchdog().Feed();
//Drive the robot
drivetrain->ArcadeDrive(driverJoystick->GetRawAxis(2) * -1,driverJoystick->GetRawAxis(4) * -1);
//Run the kicker
kicker->Act();
}
void OperatorControl()
{
tank.StartDrive();
kick.StartKicker();
while (IsOperatorControl())
{
comp.checkCompressor();
tank.Drive(PrimaryController.GetRawAxis(LEFT_JOYSTICK), PrimaryController.GetRawAxis(RIGHT_JOYSTICK), (int)PrimaryController.GetRawButton(BUTTON_HIGH_DRIVE_SHIFT), (int)PrimaryController.GetRawButton(BUTTON_LOW_DRIVE_SHIFT));
kick.StateMachine(PrimaryController.GetRawButton(BUTTON_KICK));
collect.runCollector(PrimaryController.GetRawButton(BUTTON_ROLLER_ON));
}
}
void OperatorControl(void) {
while(!IsDisabled()) {
GetWatchdog().Feed();
float speed = stick.GetRawAxis(2);
float strafe = -1*stick.GetRawAxis(1);
float turn = -1*stick.GetRawAxis(3);
Dlf->Set(speed + turn + strafe);
Dlb->Set(speed + turn - strafe);
Drf->Set(-speed + turn + strafe);
Drb->Set(-speed + turn - strafe);
Wait(.05);
}
}
void Intake::move (const Joystick& joystick) {
float left = joystick.GetRawAxis (OI::kIntakeTake);
float right = joystick.GetRawAxis (OI::kIntakeGive);
if (left > right)
move (left);
else if (right > left)
move (right * -1);
else
move (0);
}
virtual void TeleopPeriodic() {
rightDrive->SetSpeed(-(Driver->GetRawAxis(2)));
leftDrive->SetSpeed((Driver->GetRawAxis(5)));
shooterFWD->SetSpeed(-(Operator->GetRawAxis(2)));
shooterRear->SetSpeed(-(Operator->GetRawAxis(2)));
//shoioter angle
if(Operator->GetRawButton(5))
{
cout<<"Relay 1 forward"<<endl;
shooterAngle->Set(Relay::kForward);
}
if(Operator->GetRawButton(6))
{
cout<<"Relay 1 Reverse"<<endl;
shooterAngle->Set(Relay::kReverse);
}
//Fire button
if(Operator->GetRawButton(1))
{
cout<<"Relay 1 forward"<<endl;
shooterFire->Set(Relay::kForward);
}
if(Operator->GetRawButton(2))
{
cout<<"Relay 1 Reverse"<<endl;
shooterFire->Set(Relay::kReverse);
}
if(CompressorSwitch->Get() == 0){
CompressorRelay->Set(Relay::kForward);
}else{
CompressorRelay->Set(Relay::kOff);
}
//if(canPDP == 0){
// cout << "NULL" << endl;
//}else{
//canPDP->GetVoltage(Voltage) ;
//cout << "0" << endl;
//}
}
/* gets stick values into the x y and z values, these values are not returned by this but rather stored into the respective variables, the values are filtered*/
void RawControl::getStickValues(float &x, float &y, float &z) {
x=(fabs(stick->GetRawAxis(1))<.3 ? 0 : stick->GetRawAxis(1));
y=(fabs(stick->GetRawAxis(2))<.3 ? 0 : stick->GetRawAxis(2));
z=(fabs(stick->GetRawAxis(3))<.3 ? 0 : stick->GetRawAxis(3));
x=x*x*x*x*x*x*x;
y=y*y*y*y*y*y*y;
z*=.9;
z=z*z*z;
//y*=.9;
//stick->GetRawButton(2) ? z=0 : z=z;
//theres a lot more filtering of axes after this....but with comments on top of comments i dont remember what actually worked. Any other
//manipulation of joystick input for x-y-z axes goes here.
}
void Print ()
{
if (PrintTime.Get() > PRINT_TIME)
{
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Left Speed = %5.4f", PrimaryController.GetRawAxis(LEFT_JOYSTICK));
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Right Speed = %5.4f", PrimaryController.GetRawAxis(RIGHT_JOYSTICK));
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Charge State = %d", (int)Shooter.chargestate);
//lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Collector speed= %d", Collector.CollectorSpeed());
lcd->UpdateLCD();
PrintTime.Reset();
PrintTime.Start();
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl() && IsEnabled())
{
leftIntake.Set(0.8);
rightIntake.Set(0.8);
leftLift.Set(operatorStick.GetRawAxis(1));
rightLift.Set(operatorStick.GetRawAxis(1));
myRobot.TankDrive(driverStick.GetRawAxis(5), driverStick.GetRawAxis(1), true); // drive with arcade style (use right stick)
Wait(0.005); // wait for a motor update time
}
}
void OperatorControl(void)
{
PrintTime.Start();
DriveTrain.StartDriveTrain();
Shooter.StartShooterTeleop();
while (IsOperatorControl())
{
//Shooter.Safe = Collector.SafeToShoot();
DriveTrain.RunDriveTrain(PrimaryController.GetRawAxis(LEFT_JOYSTICK), PrimaryController.GetRawAxis(RIGHT_JOYSTICK), (int)PrimaryController.GetRawButton(BUTTON_HIGH_DRIVE_SHIFT), (int)PrimaryController.GetRawButton(BUTTON_LOW_DRIVE_SHIFT));
//Collector.RunCollector(PrimaryController.GetRawButton(BUTTON_COLLECTOR_DEPLOY), PrimaryController.GetRawButton(BUTTON_COLLECTOR_RETRACT), PrimaryController.GetRawButton(BUTTON_COLLECT_OUT), PrimaryController.GetRawButton(BUTTON_COLLECT_IN), PrimaryController.GetRawButton(BUTTON_COLLECT_STOP));
//Shooter.RunShooter(SecondaryController.GetRawButton(BUTTON_PRIME_SHOOTER), SecondaryController.GetRawButton(BUTTON_GOAL_SHOT), SecondaryController.GetRawButton(BUTTON_TRUSS_SHOT));
Shooter.RunPracticeShooter(SecondaryController.GetRawAxis(LEFT_JOYSTICK),SecondaryController.GetRawButton(BUTTON_PRIME_SHOOTER), SecondaryController.GetRawButton(BUTTON_GOAL_SHOT));
Print();
}
}
void DriveControl() { //Drives la wheels of the robot
flightX = flightStick->GetRawAxis(0); //Pull joystick side motion for later use
flightY = flightStick->GetRawAxis(1); //Pull joystick forward motion for later use (forward is -, backwards is +)
flightZ = flightStick->GetRawAxis(4); //Pull joystick twist motion for later use
flightThrottle = ((((shootStick->GetThrottle() - 1)*-1)/2) * .8 + .2); //Pull throttle to modify drive variables
//Throttle value is between .2 and 1.0
if (fabs(flightX) < deadZone) { //Deaden x
flightX = 0;
}
if (fabs(flightY) < deadZone) { //Deaden y
flightY = 0;
}
if (fabs(flightZ) < deadZone) { //Deaden z
flightZ = 0;
}
if (flightStick->GetRawButton(strafeButtonChannel)){ //Set drive to strafe mode
driveMode = 0;
}
if (flightStick->GetRawButton(arcadeButtonChannel)){ //Set drive to arcade mode
driveMode = 1;
}
if (flightStick->GetRawButton(fieldButtonChannel)){ //Set drive to field-centric mode
driveMode = 2;
}
if (shootStick->GetRawButton(gyroResetChannel)){ //Reset gyro with the trigger
yawGyro->Reset();
}
flightX = flightX * flightThrottle;
flightY = flightY * flightThrottle;
flightZ = flightZ * flightThrottle;
if(driveMode == 1){
robotDrive->MecanumDrive_Cartesian(flightZ, flightY, flightX, 0);
SmartDashboard::PutString("DriveMode", "Arcade");
}
else if(driveMode == 2){
robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, yawGyro->GetAngle());
SmartDashboard::PutString("DriveMode", "Field");
}
else{
robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, 0);
SmartDashboard::PutString("DriveMode", "Strafe");
}
SmartDashboard::PutNumber("GyroAngle", yawGyro->GetAngle());
}
/**
* Get the value of the axis.
*
* @param port The USB port for this joystick.
* @param axis The axis to read [1-6].
* @return The value of the axis.
*/
float GetRawAxis(UINT32 port, UINT32 axis)
{
Joystick *stick = getJoystick(port);
if (stick == NULL)
return 0;
return stick->GetRawAxis(axis);
}
void OperatorControl(void)
{
OperatorControlInit();
compressor.Start();
testActuator.Start();
while (IsOperatorControl())
{
ProgramIsAlive();
//No need to do waits because ProgramIsAlive function does a wait. //Wait(0.005);
bool isButtonPressed = stick.GetRawButton(3);
SmartDashboard::PutNumber("Actuator Button Status",isButtonPressed);
if (isButtonPressed)
{
testActuator.Go();
}
float leftYaxis = stick.GetY();
float rightYaxis = stick.GetRawAxis(5); //RawAxis(5);
TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
SmartDashboard::PutNumber("Left Axis",leftYaxis);
SmartDashboard::PutNumber("Right Axis",rightYaxis);
}
}
void TeleopPeriodic()
{
while(IsOperatorControl() && IsEnabled())
{
// FrontL->Set(.5*Driver->GetRawAxis(0)+.5*Driver->GetRawAxis(2));
//FrontR->Set(.5*Driver->GetRawAxis(0)-.5*Driver->GetRawAxis(2));
RearL->Set(.5*Driver->GetRawAxis(0)+.5*Driver->GetRawAxis(2));
RearR->Set(.5*Driver->GetRawAxis(0)-.5*Driver->GetRawAxis(2));
/**Manipulator->Set(.5*Operator->GetRawAxis(1));*/
/** Takes axes value of driver's and operator's joysticks to recieve values that the motors will be set to.
* Pushing left stick forward and backwards on driver controller causes all motors to move forward and backwards, respectively.
* Pushing right stick left on driver controller causes right motors to speed up making the robot turn left
* Pushing right stick right on driver controller causes left motors to speed up making the robot turn right
* Pushing left stick forward and backwards causes the arm to mover up and down, respectively.
*/
}
}
/**
* @brief Sets a cached joystick value.
* @param joy_id Which joystick to set the cached value for.
* @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons.
*/
void Proxy::SetJoystick(int joy_id, Joystick & stick)
{
wpi_assert(joy_id < NUMBER_OF_JOYSTICKS+1 && joy_id >= 0);
char tmp[32];
sprintf(tmp, "Joy%d", joy_id);
string name = tmp;
if(!disableAxes[joy_id-1]) {
set(name + 'X', stick.GetX());
set(name + 'Y', stick.GetY());
set(name + 'Z', stick.GetZ());
set(name + 'R', stick.GetTwist());
set(name + 'T', stick.GetThrottle());
for(int AxisId=1; AxisId<=6; AxisId++) {
sprintf(tmp, "%sA%d", name.c_str(), AxisId);
set(tmp, stick.GetRawAxis(AxisId));
}
} else {
if(!stick.GetRawButton(disableAxes[joy_id-1])) {
set(name + 'X', stick.GetX());
set(name + 'Y', stick.GetY());
set(name + 'Z', stick.GetZ());
set(name + 'R', stick.GetTwist());
set(name + 'T', stick.GetThrottle());
for(int AxisId=1; AxisId<=6; AxisId++) {
sprintf(tmp, "%sA%d", name.c_str(), AxisId);
set(tmp, stick.GetRawAxis(AxisId));
}
}
}
if(!disableButtons[joy_id-1]) {
for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) {
sprintf(tmp, "%sB%d", name.c_str(), i);
set(tmp,stick.GetRawButton(i));
}
set(name + "BT", stick.GetTrigger());
} else {
if(!stick.GetRawButton(disableButtons[joy_id-1])) {
for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) {
sprintf(tmp, "%sB%d", name.c_str(), i);
set(tmp,stick.GetRawButton(i));
}
set(name + "BT", stick.GetTrigger());
}
}
}
void TeleopPeriodic(void) {
// increment the number of teleop periodic loops completed
m_telePeriodicLoops++;
GetWatchdog().Feed();
// if(autoPilot == true)
// {
// Auto Align Disable Button
// if(operatorGamepad->GetButton(Joystick::kTopButton) == 2)
// {
// Goal_Align_PID->Disable(); // Stop outputs
// Goal_Align_PID->Enable(); // Start PIDContoller up again
// Goal_Align_PID->SetSetpoint(0.0);
// autoPilot = false;
// }
// }
// else
// {
// Calculate jaguar output based on exponent we pass from SmartDashboard
double leftOutput, rightOutput;
leftOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(2));
rightOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(5));
m_robotDrive->SetLeftRightMotorOutputs(leftOutput, rightOutput);
if(operatorGamepad->GetRawButton(1) && !buttonWasDown)
{
printf("LEFT ENCODER: %f\n", Front_L->GetPosition());
printf("RIGHT ENCODER: %f\n", Front_R->GetPosition());
}
buttonWasDown = operatorGamepad->GetRawButton(1);
// Auto Align Button
// if(operatorGamepad->GetButton(Joystick::kTopButton) == 1)
// {
// // Turn Auto Align on if we see a goal and we know the azimuth
// if(SmartDashboard::GetBoolean(FOUND_KEY) == true)
// {
// Goal_Align_PID->SetSetpoint(SmartDashboard::GetNumber(AZIMUTH_KEY));
// autoPilot = true;
// }
// }
// }
}
void TeleopPeriodic()
{
while(1)
{
fps->SetLeftRightMotorOutputs(0.5*-drivercontroller->GetRawAxis(1) + drivercontroller->GetRawAxis(2), 0.5*-drivercontroller->GetRawAxis(1) - drivercontroller->GetRawAxis(2));
if(operatorcontroller->GetRawAxis(1) > 0)
{
armminipluator->Set(0.6*operatorcontroller->GetRawAxis(1));
}
else if(operatorcontroller->GetRawAxis(1) < 0)
{
armminipluator->Set(0.3*operatorcontroller->GetRawAxis(1));
}
if(operatorcontroller->GetRawButton(5))
{
shro->Set(1);
}
else if(operatorcontroller->GetRawAxis(6))
{
shro->Set(-1);
}
else
{
shro->Set(0);
}
}
}
void BackgroundGather::Execute() {
Joystick *stick = Robot::oi->getDriveStick();
if(fabs(stick->GetRawAxis(Joystick::kThrottleAxis))>0.5) {
double speed = SmartDashboard::GetNumber("GatherSpeed");
if (Robot::gatherer->motor) {
Robot::gatherer->motor->Set(speed);
}
}
}
void Capturer::update(Joystick &controller)
{
float speed = controller.GetRawAxis(3);
if (speed < 0) {
speed = -speed;
}
std::cout << speed << '\n';
m_motor.Set(speed);
}
void Drive::arcadeDrive(Joystick stick){
double power = 0; //This is the power variable based on the left y axis
double turn = 0; //This is the turn variable based on right x axis
if((stick.GetRawAxis(2) < -.1 )||(stick.GetRawAxis(2) > .1)){ //Deadzone for the left y axis
power = .45*stick.GetRawAxis(2);
printf("The Thrust is " + power);
}
else {
power = 0;
}
if((stick.GetRawAxis(4) < -.1 )||(stick.GetRawAxis(4) > .1)){ //Deadzone for the right x axis
turn = .45*stick.GetRawAxis(4);
printf("The Turn is " + turn);
}
else {
turn = 0;
}
if (Drive.slowSpeed){
printf("Speed is slowed");
power = power*.45/.75;
turn = turn*.45/.75;
}
rDrive = (power - turn);
lDrive = (power + turn);
}
void OperatorControl(void) {
char count=0;
double target = 0, speed = 0;
while(!IsDisabled()) {
double tmpStick = -1*stick.GetRawAxis(2);
if(tmpStick < .2 && tmpStick > -.2) tmpStick=0;
target += tmpStick*1.5;
int location = enc.GetRaw();
if(stick.GetRawButton(5)) {
up.Set(true);
down.Set(false);
}else if(stick.GetRawButton(7) && location > 0) {
down.Set(true);
up.Set(false);
}else if(stick.GetRawButton(8)) {
down.Set(true);
up.Set(false);
}else if(stick.GetRawButton(9)){
speed = pid(target, location);
if(speed > 1) {
up.Set(true);
down.Set(false);
}else if(speed < -1) {
up.Set(false);
down.Set(true);
}else{
up.Set(false);
down.Set(false);
}
}else if(stick.GetRawButton(10)) {
enc.Reset();
}else{
up.Set(false);
down.Set(false);
}
if(stick.GetRawButton(1))
target = 2;
if(stick.GetRawButton(4))
target = 400;
if(stick.GetRawButton(3))
target = 200;
if(stick.GetRawButton(2))
target = 70;
Wait(.02);
while(count++%30==0) cerr << location << '\t' << target << '\t' << speed << endl;
}
}
void TeleopPeriodic(void)
{
bool flag = true; //flag object initial declaration to ensure passing Piston toggle works properly
float leftStick = gamePad->GetRawAxis(4);
float rightStick = gamePad->GetRawAxis(2);
bool rightBumper = gamePad->GetRawButton(6);
bool leftBumper = gamePad->GetRawButton(5);
bool buttonA = gamePad->GetRawButton(2);
if(fabs(leftStick) >= 0.05 || fabs(rightStick >= 0.05))
{
m_robotDrive->TankDrive(leftStick, rightStick);
}
else if(rightBumper || leftBumper)
{
if(rightBumper && !leftBumper)
{
ShiftHigh();
}
else if(leftBumper && !rightBumper)
{
ShiftLow();
}
}
else if(buttonA && flag)
{
flag = false;
passingPiston->Set(true);
}
else
{
if(!buttonA)
{
flag = false;
MotorControlLeft(0.0);
MotorControlRight(0.0);
}
else
{
MotorControlLeft(0.0);
MotorControlRight(0.0);
}
}
}
void OperatorControl(void)
{
//OperatorControlInit();
testTask.Start();
while (IsOperatorControl())
{
//ProgramIsAlive();
//No need to do waits because ProgramIsAlive function does a wait. //Wait(0.005);
bool isButtonPressed = stick.GetRawButton(3);
if (isButtonPressed)
{
int taskCounter = testTask.ReturnCounter();
SmartDashboard::PutNumber("Task Counter",taskCounter);
}
float leftYaxis = stick.GetY();
float rightYaxis = stick.GetRawAxis(5); //RawAxis(5);
//TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
SmartDashboard::PutNumber("Left Axis",leftYaxis);
SmartDashboard::PutNumber("Right Axis",rightYaxis);
}
}
void shootingModes(){
if (manShootMode == 0){
updateShooter();
}
if (manShootMode == 1){
if (stick2.GetRawAxis(triggerR) > 0){
t_motor.Set(scaler(-1*(stick2.GetRawAxis(triggerR))));
}
else if (stick2.GetRawAxis(triggerL) > 0){
t_motor.Set(scaler(stick2.GetRawAxis(triggerL)));
}
else if (stick2.GetRawAxis(triggerL) == 0 and stick2.GetRawAxis(triggerR) == 0){
t_motor.Set(0);
}
if (stick2.GetRawButton(buttonX) == true){
myServo->SetAngle(175);
}
if (stick2.GetRawButton(buttonX) == false){
myServo->SetAngle(5);
}
}
}
void OperatorControl (void) {
GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
while (IsOperatorControl() && IsEnabled()) {
GetWatchdog().Feed(); // Feed the Watchdog.
kinectMode = (bool) kinectModeSelector->GetSelected();
demoMode = (bool) demoModeSelector->GetSelected();
speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
if (kinectMode) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
if (!demoMode) {
if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else {
motorDriveLeft.Set(0);
motorDriveRight.Set(0);
}
}
if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
motorTurret.Set(TURRET_POWER);
} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
if (joystickDriveLeft.GetThrottle() == 0) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
motorTurret.Set(TURRET_POWER);
} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(MANIPULATOR_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
motorTurret.Set(joystickManipulator.GetX() * -1 * TURRET_POWER);
}
}
dsLCD->UpdateLCD();
}
GetWatchdog().SetEnabled(false); // Teleop is done, so let's turn off Watchdog.
}
void DoctaEight::OperatorControl(void)
{
REDRUM;
LTop.SetSafetyEnabled(0);
LTop.EnableControl();
LBot.SetSafetyEnabled(0);
LBot.EnableControl();
while (IsOperatorControl())
{
REDRUM;
output();
if (pilot.GetRawButton(6))
{
arm.Set (.3);
}
else if (pilot.GetRawButton(5))
{
arm.Set (-.3);
}
else
arm.Set (0);
//Set Shooter state and reset RPMoffset if necessary
if (copilot.GetRawButton(1)) //BUTTON A
{
REDRUM;
if(ShooterState != 1)
{
REDRUM;
ShooterState = 1;
RPMoffset = 0;
}
}
else if (copilot.GetRawButton(2)) //BUTTON B
{
REDRUM;
if(ShooterState != 2)
{
REDRUM;
ShooterState = 2;
RPMoffset = 0;
}
}
else if (copilot.GetRawButton(3)) //BUTTON X
{
REDRUM;
if(ShooterState != 3)
{
REDRUM;
ShooterState = 3;
RPMoffset = 0;
}
}
else if (copilot.GetRawButton(4)) //BUTTON Y
{
REDRUM;
if(ShooterState != 4)
{
REDRUM;
ShooterState = 4;
RPMoffset = 0;
}
}
//increment or decrement RPMoffset
if(copilot.GetRawButton(5)) //BUTTON LB
{
REDRUM;
FlagB5 = true;
}
else if(copilot.GetRawButton(6)) //BUTTON RB
{
REDRUM;
FlagB6 = true;
}
if(FlagB5 == true && copilot.GetRawButton(5) == false)
{
REDRUM;
RPMoffset -= 50;
FlagB5 = false;
}
else if(FlagB6 && !copilot.GetRawButton(6))
{
REDRUM;
RPMoffset += 50;
FlagB6 = false;
}
if (pilot.GetRawButton(1) && !cycle)
{
cycle = 1;
negate*=-1;
}
else
{
cycle=0;
}
//prepare shooter/launcher ouput speed
if(ShooterState == 1) {
REDRUM; //BUTTON A
LTopSpeed = MAX/12;
LBotSpeed = RPMMid + RPMoffset;
}
else if(ShooterState == 2) { //BUTTON B
REDRUM;
LTopSpeed = MAX/12;
LBotSpeed = RPMLow + RPMoffset;
}
else if(ShooterState == 3) { //BUTTON X
REDRUM;
LTopSpeed = MAX/12;
LBotSpeed = RPMHigh + RPMoffset;
}
else if(ShooterState == 4) { //BUTTON Y
REDRUM;
LTopSpeed = 0;
LBotSpeed = 0;
}
//set shooter launcher speed to CANJags
LTop.Set(LTopSpeed);
LBot.Set(-LBotSpeed);
//move simple platform arm
leftyrighty(-pilot.GetY(), -pilot.GetRawAxis(4));
intake.Set(-copilot.GetY());
}
//stops encoders
}