/**
* Get the twist value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*
* @param port The USB port for this joystick.
*/
float GetTwist(UINT32 port)
{
Joystick *stick = getJoystick(port);
if (stick == NULL)
return 0;
return stick->GetTwist();
}
/**
* @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 OperatorControl()
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl() && IsEnabled())
{
//MECANUM DRIVE
float yValue = stick->GetY();
float xValue = stick->GetX();
float rotation = stick->GetTwist();
myRobot.MecanumDrive_Cartesian(xValue, yValue, rotation, 0.0);
Wait(0.005);
}
}
/** Function to retrieve the user inputs
* Inputs (For all user input modes):
* * Potentiometers
* * Limit switches
* * the requested state for the robot (from the joystick buttons)
* For manual mode only:
* * The intake and ejection wheel speeds - based on the operator joystick throttle
* * The arm position - based on the operator joystick Y axis
*/
void GetUserInputs(RobotStates_t &requestedState)
{
float throttleRaw = 0.0;
//Read potentiometer goes from -5 to 28
potentiometerValueRight = armPotentiometer1.GetValue();
//potentiometerValueLeft = armPotentiometer2.GetValue();
ballCaughtLimitSwitch1 = limitSwitch1.Get(); //TODO 0 is no ball 1 is ball
//Read Joystick state buttons
requestedState = ReadJoystickButtons();
throttleRaw = DeadZone(Operator_Control_Stick.GetTwist(), EJWHEEL_MM_DEADZONE_VAL);
manualModeThrottlePosition = (throttleRaw * -1); //invert throttle value to match the joystick
manualModeArmPosition = DeadZone(Operator_Control_Stick.GetY(), ARM_MM_DEADZONE_VAL);
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool reset_yaw_button_pressed = stick.GetRawButton(1);
if ( reset_yaw_button_pressed ) {
ahrs->ZeroYaw();
}
bool rotateToAngle = false;
if ( stick.GetRawButton(2)) {
turnController->SetSetpoint(0.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(3)) {
turnController->SetSetpoint(90.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(4)) {
turnController->SetSetpoint(179.9f);
rotateToAngle = true;
} else if ( stick.GetRawButton(5)) {
turnController->SetSetpoint(-90.0f);
rotateToAngle = true;
}
double currentRotationRate;
if ( rotateToAngle ) {
turnController->Enable();
currentRotationRate = rotateToAngleRate;
} else {
turnController->Disable();
currentRotationRate = stick.GetTwist();
}
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and the current */
/* calculated rotation rate (or joystick Z axis), */
/* depending upon whether "rotate to angle" is active. */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
currentRotationRate ,ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void driveTrainValues(void) {
//Testing the encoder...
//Get the encoder value
//encoder_value = right_encoder->Get();
//Print encoder value
//printf("right encoder value: %f ", encoder_value);
//Shifting gears...
//Low gear
if (gamepad->GetRawButton(5))
{
printf("The left button is being pressed.\n");
drivetrain_pressure->Set(false);
drivetrain_vent->Set(true);
}
//Six is high gear
if (gamepad->GetRawButton(6))
{
printf("The right button is being pressed.\n");
drivetrain_pressure->Set(true);
drivetrain_vent->Set(false);
}
//assign right joystick value to 'right'
right=(gamepad->GetTwist());
//find the change in joystick values
rightchange=fabs(oldright-right);
// if the change in joystick values is less than 0.2 then use the old right value vs the new one
//Stops driver from accelerating the robot too quickly
if(rightchange<=threshold) {
useright=right;
}
//else... if the new right is greater than the old one use the old value plus the threshold (faster)
//if the new right is less than the old right use the old value minue the threshold (slower)
//so that really large movements of the joystick don't translate immediately into a really big acceleration
else {
if(oldright<right) {
useright=oldright+threshold;
}
if(oldright>right) {
useright=oldright-threshold;
}
}
//Same as right, but on the left side
left=(gamepad->GetY());
leftchange=fabs(oldleft-left);
if(leftchange<=threshold) {
useleft=left;
}
else {
if(oldleft<left) {
useleft=oldleft+threshold;
}
if(oldleft>left) {
useleft=oldleft-threshold;
}
}
//printf("hello world");
//make useright and useleft (the values sent to the robot) the old values for the next loop
oldright=useright;
oldleft=useleft;
}
/****************************************
* Runs the motors with arcade steering.*
****************************************/
void OperatorControl(void)
{
//TODO put in servo for lower camera--look in WPI to set
// Watchdog baddog;
// baddog.Feed();
myRobot.SetSafetyEnabled(true);
//SL Earth.Start(); // turns on Earth
// SmartDashboard *smarty = SmartDashboard::GetInstance();
//DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory)
//char debugout [100];
compressor.Start();
gyro.Reset(); // resets gyro angle
int rpmForShooter;
while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop"
{
// baddog.Feed();
//myRobot.SetSafetyEnabled(true);
//myRobot.SetExpiration(0.1);
float leftYaxis = driver.GetY();
float rightYaxis = driver.GetTwist();//RawAxis(5);
myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
float random = gamecomponent.GetY();
float lazysusan = gamecomponent.GetZ();
//bool elevator = Frodo.Get();
float angle = gyro.GetAngle();
bool balance = Smeagol.Get();
SmartDashboard::PutNumber("Gyro Value",angle);
int NumFail = -1;
//bool light = Pippin.Get();
//SL float speed = Earth.GetRate();
//float number = shooter.Get();
//bool highspeed = button1.Get()
//bool mediumspeed = button2.Get();
//bool slowspeed = button3.Get();
bool finder = autotarget.Get();
//bool targetandspin = autodistanceandspin.Get();
SmartDashboard::PutString("Targeting Activation","");
//dslcd->Clear();
//sprintf(debugout,"Number=%f",angle);
//dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
//SL sprintf(debugout,"Number=%f",speed);
//SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
//sprintf(debugout,"Number=%f",number);
//dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
//sprintf(debugout,"Finder=%u",finder);
//dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
//dslcd->UpdateLCD(); // update the Driver Station with the information in the code
// sprintf(debugout,"Number=%u",maxi);
// dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout)
bool basketballpusher = julesverne.Get();
bool bridgetipper = joystickbutton.Get();
if (bridgetipper) // if joystick button 7 is pressed (is true)
{
solenoid.Set(true); // then the first solenoid is on
}
else
{
//Wait(0.5); // and then the first solenoid waits for 0.5 seconds
solenoid.Set(false); //and then the first solenoid turns off
}
if (basketballpusher) // if joystick button 6 is pressed (is true)
{
shepard.Set(true); // then shepard is on the run
//Wait(0.5); // and shepard waits for 0.5 seconds
}
else
{
shepard.Set(false); // and then shepard turns off
} //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2
//}
//cheetah.Set(0.3*lazysusan);
// smarty->PutDouble("pre-elevator",lynx.Get());
lynx.Set(random);
// smarty->PutDouble("elevator",lynx.Get());
// smarty->PutDouble("joystick elevator",random);
if (balance) // this is the start of the balancing code
{
angle = gyro.GetAngle();
myRobot.Drive(-0.03*angle, 0.0);
Wait(0.005);
}
/*if (light) //button 5 turns light on oand off on game controller
flashring.Set(Relay::kForward);
else
flashring.Set(Relay::kOff);
*/
if (finder)
{
flashring.Set(Relay::kForward);
if (button_H.Get()==true)
{
targeting.SetLMHTarget(BOGEY_H);
SmartDashboard::PutString("Targeting","High Button Pressed");
}
if (button_M.Get()==true)
{
targeting.SetLMHTarget(BOGEY_M);
SmartDashboard::PutString("Targeting","Medium Button Pressed");
}
if (button_L.Get()==true)
{
targeting.SetLMHTarget(BOGEY_L);
SmartDashboard::PutString("Targeting","Low Button Pressed");
}
if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true)
{
if (targeting.ProcessOneImage())
{
NumFail = 0;
SmartDashboard::PutString("Targeting Activation","YES");
targeting.ChooseBogey();
targeting.MoveTurret();
#ifdef USE_HARDWIRED_RPM
shooter.setTargetRPM(HARDWIRED_RPM);
#else
rpmForShooter = targeting.GetCalculatedRPM();
shooter.setTargetRPM(rpmForShooter);
#endif
targeting.InteractivePIDSetup();
}
else
{
NumFail++;
if (NumFail > 10)
targeting.StopPID();
}
SmartDashboard::PutNumber("Numfail", NumFail);
shooter.setTargetRPM(rpmForShooter);
}
else
{
SmartDashboard::PutString("Targeting Activation","NO");
shooter.setTargetRPM(0);
targeting.StopPID();
}
}
else
{
flashring.Set(Relay::kOff);
targeting.StopPID();
turret.Set(lazysusan); // the lazy susan would turn right & left based on how far the person moves the right joystick#2 side to side
//targeting.StopPID();
//if (elevator) //shooter would move at full speed if button is pressed
//TODO Change RPM values
//TODO Disable calculation of RPM values
SmartDashboard::PutNumber("CurrentRPM",shooter.GetCurrentRPM());
if (button_H.Get() == true)
shooter.setTargetRPM((int)2100);
//From front of free throw line, should hit the backboard and go in
//used to be 2700 RPMs
else if (button_M.Get() == true)
shooter.setTargetRPM((int)1900);
//From front of free throw line, should go in the net--can shoot the next ball on the overshoot?
//Used to be 2250 RPMs
else if (button_L.Get() == true)
shooter.setTargetRPM((int)1350);
//From fender, should hit the backboard
//Used to be 2000 RPMs
//shooter.Set(0.5);
else
shooter.setTargetRPM(0);
// else if (mediumspeed)
//shooter.setTargetRPM((int)0);
//else if (slowspeed)
//shooter.setTargetRPM((int)0);
/*if (targetandspin) //code for autotargeting and speed will go here
{
shooter.setTargetRPM((int)1800);
}
else
{*/
//}
myRobot.TankDrive(leftYaxis,rightYaxis);
}
//Wait(0.005);
}
}
void OperatorControl(void)
{
bool button1Bool;
bool button2Bool;
bool button3Bool;
bool button4Bool;
bool button5Bool;
bool button6Bool;
bool button7Bool;
bool button8Bool;
bool button9Bool;
bool button10Bool;
bool button11Bool;
bool button12Bool;
float LeftaxisYValue;
float LeftaxisXValue;
float RightaxisXValue;
float RightaxisYValue;
float TriggerValue;
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
button1Bool = button1.Get();
SmartDashboard::PutNumber("button1",button1Bool);
button2Bool = button2.Get();
SmartDashboard::PutNumber("button2",button2Bool);
button3Bool = button3.Get();
SmartDashboard::PutNumber("button3",button3Bool);
button4Bool = button4.Get();
SmartDashboard::PutNumber("button4",button4Bool);
button5Bool = button5.Get();
SmartDashboard::PutNumber("button5",button5Bool);
button6Bool = button6.Get();
SmartDashboard::PutNumber("button6",button6Bool);
button7Bool = button7.Get();
SmartDashboard::PutNumber("button7",button7Bool);
button8Bool = button8.Get();
SmartDashboard::PutNumber("button8",button8Bool);
button9Bool = button9.Get();
SmartDashboard::PutNumber("button9",button9Bool);
button10Bool = button10.Get();
SmartDashboard::PutNumber("button10",button10Bool);
button11Bool = button11.Get();
SmartDashboard::PutNumber("button11",button11Bool);
button12Bool = button12.Get();
SmartDashboard::PutNumber("button12",button12Bool);
LeftaxisXValue = stick.GetX();
SmartDashboard::PutNumber("Joystick1 X Axis",LeftaxisXValue);
LeftaxisYValue = 0 - stick.GetY();
SmartDashboard::PutNumber("Joystick1 Y Axis",LeftaxisYValue);
RightaxisXValue = stick.GetTwist();
SmartDashboard::PutNumber("Joystick2 X Axis", RightaxisXValue);
RightaxisYValue = 0 - stick.GetRawAxis(5);
SmartDashboard::PutNumber("Joystick2 Y Axis", RightaxisYValue);
TriggerValue = stick.GetThrottle();
SmartDashboard::PutNumber("Trigger value", TriggerValue);
Wait(0.005); // wait for a motor update time
}
}
void OperatorControl(void)
{
// feed watchdog --- TheVessel.SetSafetyEnabled(true);
GetWatchdog().Kill();
if (!game){
AlmostZeroPi = manEnc->GetDistance() -1;//Zeros manipulator encoder-- only here for tele op practice w/out auto
cmp->Start();
}
cd->MecanumDrive(0, 0, 0);
minibot->Set(DoubleSolenoid::kForward);
while(IsOperatorControl() && IsEnabled())
{
GetWatchdog().Kill();
if(IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kReverse);
else if(!IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kForward);
cd->MecanumDrive(controller);//passes joystick controller's input to cd mechdrv
if(stick->GetRawButton(1))
{
}
else if(stick->GetRawButton(2))
{
}
else if(stick->GetRawButton(3))
{
reset = true;// ENABLES NO RESTRICTION MANIPULATOR MOVEMENT
}
else if(stick->GetRawButton(4))
{
cout << "Manipulator Encoder Value: " << manEnc->Get() << "/n";//prints manip encoder to console if open;
Wait(.1f);//pause so not above a billion times
}
else if(stick->GetRawButton(6))
{
open->Set(true);//self explanatory, eote pnuematics
closed->Set(false);
}
else if(stick->GetRawButton(8))
{
closed->Set(true);
open->Set(false);
}
ShoulderJag->Set(stick->GetY());//shoulder moves with stick->GetY()
if (stick->GetTwist() < 0 || manEnc->GetDistance() < AlmostZeroPi || reset)//man extending, enc not at 0, or reset true
{
manJag->Set(stick->GetTwist()*.3);//shoulder above but for wrist, also, only 3/10ths power
}
}
cmp->Stop();//stops compressor
while (manEnc->GetDistance() < AlmostZeroPi)//brings manipulator back to 0 so not need reset
{
manJag->Set(.1);
}
}