//Drive base control
void DriveBaseControl(void){
//Local declarations
float driveThreshold = 0.005;
//Get the y-axis of the joystick
float yAxis1 = 1 * leftStick.GetY();
float yAxis2 = 1 * rightStick.GetY();
//std::cout << "yAxisVal: " << yAxisVal << std::endl;
//Convert input to [-1.0, 1.0] range
//Don't need to - already in the correct range
//Drive the drive motors when any input is within +-driveThreshold of 0.0
//NOTE - currently this doesn't scale up the input from 0.0 after the deadband region -- it just uses the raw value.
if(yAxis1 >= driveThreshold || yAxis2 >= driveThreshold || yAxis1 <= -driveThreshold || yAxis2 <= -driveThreshold )
{
robotDrive.TankDrive(-yAxis1,-yAxis2); // drive Forwards
} else {
robotDrive.TankDrive(0.0, 0.0); // stop robot
}
}
void OperatorControl(void)
{
NetTest();
return;
myRobot.SetSafetyEnabled(true);
digEncoder.Start();
const double ppsTOrpm = 60.0/250.0; //Convert from Pos per Second to Rotations per Minute by multiplication
// (See the second number on the back of the encoder to replace 250 for different encoders)
const float VoltsToIn = 41.0; // Convert from volts to cm by multiplication (volts from ultrasonic).
// This value worked for distances between 1' and 10'.
while (IsOperatorControl())
{
if (stick.GetRawButton(4)) {
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1);
}
else if (stick.GetRawButton(5))
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1);
}
else
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
}
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm));
SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch);
SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage());
Wait(0.1);
}
digEncoder.Stop();
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
HSLImage *Himage;
Threshold targetThreshold(247, 255, 60, 140, 10, 50);
BinaryImage *matchingPixels;
vector<ParticleAnalysisReport> *pReport;
//myRobot->SetSafetyEnabled(true);
Saftey->SetEnabled(false);
AxisCamera &mycam = AxisCamera::GetInstance("10.15.10.11");
mycam.WriteResolution(AxisCamera::kResolution_640x480);
mycam.WriteCompression(20);
mycam.WriteBrightness(25);
Wait(3.0);
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
float X[2];
float Y[2];
float Z[2];
while(IsOperatorControl())
{
X[1] = Stick1->GetX();
X[2] = Stick2->GetX();
Y[1] = Stick1->GetY();
Y[2] = Stick2->GetY();
Z[1] = Stick1->GetZ();
Z[2] = Stick2->GetZ();
Jaguar1->Set(Y[1]);
Jaguar2->Set(Y[2]);
Wait(0.005);
if (mycam.IsFreshImage())
{
Himage = mycam.GetImage();
matchingPixels = Himage->ThresholdHSL(targetThreshold);
pReport = matchingPixels->GetOrderedParticleAnalysisReports();
for (unsigned int i = 0; i < pReport->size(); i++)
{
printf("Index: %d X Center: %d Y Center: %d \n", i, (*pReport)[i].center_mass_x, (*pReport)[i].center_mass_y);
}
delete Himage;
delete matchingPixels;
delete pReport;
}
}
//myRobot->ArcadeDrive(stick); // drive with arcade style (use right stick)
//Wait(0.005); // wait for a motor update time
}
void SquareInputs(void)
{
if(stick.GetY() < 0)
{
if(DoubleSolenoid::kReverse == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * -4.0), stick.GetX());
}
else if(DoubleSolenoid::kForward == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * -1.0), stick.GetX());
}
}
else if(stick.GetY() > 0)
{
if(DoubleSolenoid::kReverse == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * 4.0), stick.GetX());
}
else if(DoubleSolenoid::kForward == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * 1.0), stick.GetX());
}
}
}
void TeleopPeriodic()
{
rightDrive = rightStick->GetY();
leftDrive = leftStick->GetY();
rightDrive = .6*rightDrive;
leftDrive = .6*leftDrive;
robotDrive->TankDrive(rightDrive, leftDrive);
ax = accel-> GetX();
ay = accel-> GetY();
az = accel-> GetZ();
SmartDashboard::PutData("Auto Modes", chooser);
SmartDashboard::PutNumber("ax",ax);
SmartDashboard::PutNumber("ay",ay);
SmartDashboard::PutNumber("az",az);
bool triggerRight = rightStick->GetRawButton(1);
bool triggerLeft = leftStick->GetRawButton(1);
SmartDashboard::PutBoolean("trigger", triggerRight);
SmartDashboard::PutBoolean("trigger", triggerLeft);
if(triggerRight || triggerLeft){
pickup->Set(.3);
}
else{
pickup->Set(0);
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
left1.Set(leftStick.GetY());
left2.Set(leftStick.GetY());
right1.Set(rightStick.GetY());
right2.Set(rightStick.GetY());
}
void TeleopPeriodic(void)
{
myarm->prepareSignal();
// Call the drive routine to drive the robot.
if(rightStick->GetRawButton(1)|| leftStick->GetRawButton(1))
drive->MecanumDrive_Cartesian(rightStick->GetX()/2,leftStick->GetY()/2,leftStick->GetX()/2,0.00);
else
drive->MecanumDrive_Cartesian(rightStick->GetX(),leftStick->GetY(),leftStick->GetX(),0.00);
GetStateForArm();
//Wait(.1);
if(modeArm==DDCArm::kManualOveride)
{
myarm->OperateArm(0,0,0,modeArm);
//Shoulder movement
if(leftStick->GetRawButton(3))
myarm->MoveShoulder(1);
else if(leftStick->GetRawButton(2))
myarm->MoveShoulder(-1);
else
myarm->MoveShoulder(0);
//Elbow Movement
if(rightStick->GetRawButton(3))
myarm->MoveElbow(1);
else if (rightStick->GetRawButton(2))
myarm->MoveElbow(-1);
else
myarm->MoveElbow(0);
//Wrist Movement
if(rightStick->GetRawButton(4))
myarm->MoveWrist(-1);
else if(rightStick->GetRawButton(5))
myarm->MoveWrist(1);
else
myarm->MoveWrist(0);
}
else
myarm->OperateArm(0.0,0.0,peg,modeArm);
if(leftStick->GetRawButton(4))
myarm->MoveClaw(-1);
else if(leftStick->GetRawButton(5))
myarm->MoveClaw(1);
else
myarm->MoveClaw(0);
if(rightStick->GetRawButton(10))
{
printf("S: %f \n E: %f \n W: %f \n \n",myarm->GetShoulderVoltage(),myarm->GetElbowVoltage(), myarm->GetWristVoltage());
}
deploy->OperateDeployment(fire,pull);
// Send Data to the Driver Station for Monitoring (w/in .
//sendIOPortData();
//Wait(.1);
}
void RobotDemo::dumb_drive_code()
{
#if DUMB_DRIVE_CODE
left_drive_motor_A->Set(-drive_stick_sec ->GetY());
left_drive_motor_B->Set(-drive_stick_sec->GetY());
right_drive_motor_A->Set(drive_stick_prim->GetY());
right_drive_motor_B->Set(drive_stick_prim->GetY());
#endif
}
/**
* Function to control and actuate all us er inputs related to the drive base
* * Left Joystick - the Y axis will control the left drive base motors forward or back.
* * Right Joystick - the Y axis will control the right drive base motors forward or back.
*/
void ManualDriveBaseActuation()
{
//get joystick inputs with GetY and filter
float lefty = DeadZone(leftDriveStick.GetY(), DRIVE_MM_DEADZONE_VAL);
float righty = DeadZone(rightDriveStick.GetY(), DRIVE_MM_DEADZONE_VAL);
//float arm = DeadZone(Operator_Control_Stick.GetY());
//driving left side inverted
motor1.Set(-1 * lefty);
motor3.Set(-1 * lefty);
motor5.Set(-1 * lefty);
//driving right Side
motor2.Set(righty);
motor4.Set(righty);
motor6.Set(righty);
}
void TeleopPeriodic() {
// Comment the next line out to disable movement
drive->doDrive(stick->GetX(), -stick->GetY());
intake->periodic();
shooter->periodic();
camSystem->periodic();
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
GetWatchdog().SetEnabled(true);
compressor->Start();
GetWatchdog().SetExpiration(0.5);
bool valve_state = false;
while (IsOperatorControl())
{
motor->Set(stick->GetY());
if (stick->GetRawButton(1) && !valve_state)
{
valve->Set(true);
valve_state = true;
}
if (!stick->GetRawButton(1) && valve_state)
{
valve->Set(false);
valve_state = false;
}
// Update driver station
//dds->sendIOPortData(valve);
GetWatchdog().Feed();
}
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool collisionDetected = false;
double curr_world_linear_accel_x = ahrs->GetWorldLinearAccelX();
double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x;
last_world_linear_accel_x = curr_world_linear_accel_x;
double curr_world_linear_accel_y = ahrs->GetWorldLinearAccelY();
double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y;
last_world_linear_accel_y = curr_world_linear_accel_y;
if ( ( fabs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G ) ||
( fabs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G) ) {
collisionDetected = true;
}
SmartDashboard::PutBoolean( "CollisionDetected", collisionDetected);
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and Z axis for rotation. */
/* Use navX MXP yaw angle to define Field-centric transform */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
stick.GetZ(),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 TeleopPeriodic()
{
static int targetSpeed = 0;
targetSpeed+= stick->GetY()*-3;
static int fakerange = 0;
if (stick->GetRawButton(2))
{
targetSpeed=0;
fakerange = 250;
}
if (stick->GetRawButton(3))
{
targetSpeed += 150;
}
if (stick->GetRawButton(4))
{
targetSpeed -= 150;
}
if(stick->GetRawButton(5))
{
fakerange+=3;
}
if(stick->GetRawButton(6))
{
fakerange-=3;
}
launch->Obey();
launch->SetTargetSpeed(targetSpeed);
float currAngle= angle->PIDGet();
float range = fakerange*cos(currAngle*3.1415/180);
std::cout<<"range estimate = "<<range<<std::endl;
launch->Aim(range/100.0);
int current =rightWheel->GetEncVel();
std::cout<<targetSpeed<<" "<<current<<std::endl;
}
/**
* Get the Y value of the joystick.
* This depends on the mapping of the joystick connected to the current port.
*
* @param port The USB port for this joystick.
*/
float GetY(UINT32 port, JoystickHand hand)
{
Joystick *stick = getJoystick(port);
if (stick == NULL)
return 0;
return stick->GetY((Joystick::JoystickHand) hand);
}
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);
}
}
/**
* Runs the motors under driver control with either tank or arcade steering selected
* by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis.
*/
void OperatorControl(void)
{
Victor armMotor(5); // create arm motor instance
while (IsOperatorControl())
{
GetWatchdog().Feed();
// determine if tank or arcade mode; default with no jumper is for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0) {
myRobot->TankDrive(leftStick, rightStick); // drive with tank style
} else{
myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick)
}
// Control the movement of the arm using the joystick
// Use the "Y" value of the arm joystick to control the movement of the arm
float armStickDirection = armStick->GetY();
// if at a limit and telling the arm to move past the limit, don't drive the motor
if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
armStickDirection = 0;
} else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
armStickDirection = 0;
}
// Set the motor value
armMotor.Set(armStickDirection);
}
}
void TeleopPeriodic(void)
{
Scheduler::GetInstance()->Run();
myDrive_all->TankDrive(drivestick_left->GetY(), drivestick_right->GetY());
ABCheck = Xbox_Button_A->Get(); // Gets the value of the A button on the xbox
BBCheck = Xbox_Button_B->Get(); // Gets the value of the B button on the xbox
if (ABCheck == 1) // If A button is pressed
{
FirstSolenoid->Set(DoubleSolenoid::kForward); // Make piston extend
}
if (BBCheck == 1) // If B button is pressed
{
FirstSolenoid->Set(DoubleSolenoid::kReverse); // Make piston retract
}
if ((ABCheck == 0) && (BBCheck == 0)) // If A and B buttons are not pressed
{
FirstSolenoid->Set(DoubleSolenoid::kOff); // Make piston stay where it is
}
XBCheck = Xbox_Button_X->Get(); // Gets value of the X button on the xbox
YBCheck = Xbox_Button_Y->Get(); // Get value of the Y button on the xbox
if (XBCheck == 1)//If X button is pressed
{
if (limitSwitch->Get() == 0) // If limitswitch is pressed
{
intake_Motor->Set(0); // Stop test motor
}
if (limitSwitch->Get() == 1) // If limitswitch is not pressed
{
intake_Motor->Set(1); // Test motor goes forward at 0.5 speed
}
}
if (YBCheck == 1) // If Y button is pressed
{
intake_Motor->Set(-1); // Test motor goes backward at -0.5 speed
}
if ((XBCheck == 0) && (YBCheck == 0)) // If X and Y buttons are not pressed
{
intake_Motor->Set(0); // Test motor stops
}
}
/**
* @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());
}
}
}
/**
* Runs the motor from the output of a Joystick.
*/
void OperatorControl() {
while (IsOperatorControl() && IsEnabled()) {
// Set the motor controller's output.
// This takes a number from -1 (100% speed in reverse) to +1 (100% speed forwards).
m_motor.Set(m_stick.GetY());
Wait(kUpdatePeriod); // Wait 5ms for the next update.
}
}
// Called repeatedly when this Command is scheduled to run
void DriveBot::Execute() {
Joystick* stick = Robot::oi->getGamePad();
if(Robot::mover->speedSwitch->Get()){
Robot::mover->rightMotor0->Set(deadband(stick->GetThrottle()*-0.75,.01));
Robot::mover->rightMotor1->Set(deadband(stick->GetThrottle()*-0.75,.01));
Robot::mover->leftMotor2->Set(deadband(stick->GetY()*0.75,.01));
Robot::mover->leftMotor3->Set(deadband(stick->GetY()*0.75,.01));
}else{
Robot::mover->rightMotor0->Set(stick->GetThrottle()*-0.25);
Robot::mover->rightMotor1->Set(stick->GetThrottle()*-0.25);
Robot::mover->leftMotor2->Set(stick->GetY()*0.25);
Robot::mover->leftMotor3->Set(stick->GetY()*0.25);
}
}
void TeleopPeriodic()
{
SmartDashboard::PutNumber("joystickX",stick.GetX());
SmartDashboard::PutNumber("joystickY",stick.GetY());
//SmartDashboard::PutBoolean("f*****g buttons", stick.GetRawButton(1));
//SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage());
SmartDashboard::PutBoolean("infra",infra.Get());
SmartDashboard::PutNumber("accelX",accel.GetX());
SmartDashboard::PutNumber("accelY",accel.GetY());
SmartDashboard::PutNumber("accelZ",accel.GetZ());
servo.Set(
trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick
);
SmartDashboard::PutNumber("servo", servo.Get());
jag1.Set(stick.GetY());
jag2.Set(stick.GetY());
//tal1.Set(stick.GetY());
SmartDashboard::PutNumber("jag1", jag1.Get());
SmartDashboard::PutNumber("jag2", jag2.Get());
/*SmartDashboard::PutNumber("encpos", enc.Get());
SmartDashboard::PutNumber("encspd", enc.GetRate());*/
if (stick.GetRawButton(1) && !actuatePressed) {
pistonVal=!pistonVal;
piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse);
actuatePressed = true;
}
else if (!stick.GetRawButton(1))
actuatePressed = false;
SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward);
}
/**
* Drive based upon joystick inputs, and automatically control
* motors if the robot begins tipping.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled()) {
double xAxisRate = stick.GetX();
double yAxisRate = stick.GetY();
double pitchAngleDegrees = ahrs->GetPitch();
double rollAngleDegrees = ahrs->GetRoll();
if ( !autoBalanceXMode &&
(fabs(pitchAngleDegrees) >=
fabs(kOffBalanceThresholdDegrees))) {
autoBalanceXMode = true;
}
else if ( autoBalanceXMode &&
(fabs(pitchAngleDegrees) <=
fabs(kOnBalanceThresholdDegrees))) {
autoBalanceXMode = false;
}
if ( !autoBalanceYMode &&
(fabs(pitchAngleDegrees) >=
fabs(kOffBalanceThresholdDegrees))) {
autoBalanceYMode = true;
}
else if ( autoBalanceYMode &&
(fabs(pitchAngleDegrees) <=
fabs(kOnBalanceThresholdDegrees))) {
autoBalanceYMode = false;
}
// Control drive system automatically,
// driving in reverse direction of pitch/roll angle,
// with a magnitude based upon the angle
if ( autoBalanceXMode ) {
double pitchAngleRadians = pitchAngleDegrees * (M_PI / 180.0);
xAxisRate = sin(pitchAngleRadians) * -1;
}
if ( autoBalanceYMode ) {
double rollAngleRadians = rollAngleDegrees * (M_PI / 180.0);
yAxisRate = sin(rollAngleRadians) * -1;
}
try {
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
robotDrive.MecanumDrive_Cartesian(xAxisRate, yAxisRate,stick.GetZ());
} catch (std::exception ex ) {
std::string err_string = "Drive system error: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void HandleDriverInputsAutomatic(void)
{
//myRobot.ArcadeDrive(stick);
if(DoubleSolenoid::kReverse == shifter.Get())
{
if(stick.GetY() < -0.25)
{
shifter.Set(DoubleSolenoid::kForward);
}
}
// If the robot is in low gear and is over 0.2 input,
// then switch into high gear.
else if(stick.GetY() > -0.2)
{
shifter.Set(DoubleSolenoid::kReverse);
}
SquareInputs();
}
// the loop routine runs over and over again forever:
void loop() {
if(enabled)
{
S1overflow = 0.0;
S2overflow = 0.0;
Stick.Update();
driveSpeed = Stick.GetX();
rotSpeed = Stick.GetY();
S1 = driveSpeed + rotSpeed;
S2 = driveSpeed + (-rotSpeed);
if(S1 > 1) { //calculate S1 overflow
S1overflow = S1 - 1.00;
S1 = 1.00;
}
else{
if(S1 < -1.00) {
S1overflow = S1 + 1.00;
S1 = -1.00;
}
}
if(S2 > 1) { //calculate S2 overflow
S2overflow = S2 - 1.00;
S2 = 1.00;
}
else{
if(S2 < -1.00) {
S2overflow = S2 + 1.00;
S2 = -1.00;
}
}
S1 = S1 + (-S2overflow);
S2 = S2 + (-S1overflow);
if(reverseS1) {
S1 = (-S1);
}
if(reverseS2) {
S2 = (-S2);
}
talon1s = (S1 + 1.00) * 90;
talon2s = (S2 + 1.00) * 90;
//Talon1.write(talon1s);
// Talon2.write(talon2s);
}
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
// This sample does not use field-oriented drive, so the gyro input is set to zero.
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ());
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void TeleopPeriodic(void)
{
//Drive code
leftFrontDrive->Set(-1 * leftStick->GetY());
rightFrontDrive->Set(rightStick->GetY());
leftRearDrive->Set(-1 * leftStick->GetY());
rightRearDrive->Set(rightStick->GetY());
//airCompressor->Start();
// Simple Button Drive Forward
if(rightStick->GetRawButton(3))
{
leftFrontDrive->Set(-1);
rightFrontDrive->Set(1);
leftRearDrive->Set(-1);
rightRearDrive->Set(1);
}
// Simple Button Drive Backwards
if(rightStick->GetRawButton(2))
{
leftFrontDrive->Set(1);
rightFrontDrive->Set(-1);
leftRearDrive->Set(1);
rightRearDrive->Set(-1);
}
// Code to print to the user messages box in the Driver Station
LCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hello World");
LCD->Printf(DriverStationLCD::kUser_Line2,1,"Y Left: %f", leftStick->GetY());
LCD->Printf(DriverStationLCD::kUser_Line3,1,"Y Right: %f", rightStick->GetY());
LCD->UpdateLCD();
Wait(0.2);
}
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);
}
}
/**
* @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 Proxy166::SetJoystick(int joy_id, Joystick & stick)
{
wpi_assert(joy_id < NUMBER_OF_JOYSTICKS && joy_id >= 0);
//semTake(JoystickLocks[joy_id], WAIT_FOREVER);
Joysticks[joy_id].X = stick.GetX();
Joysticks[joy_id].Y = stick.GetY();
Joysticks[joy_id].Z = stick.GetZ();
Joysticks[joy_id].throttle = stick.GetThrottle();
for(unsigned i=0;i<NUMBER_OF_JOY_BUTTONS;i++) {
Joysticks[joy_id].button[i] = stick.GetRawButton(i);
}
//semGive(JoystickLocks[joy_id]);
}
/** 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
}
}