void RobotDemo::MecanumDrive(float x, float y, float rotation, float gyroAngle)
{
const float EPSILONFL=1.f, EPSILONFR=1.f, EPSILONBL=1.f, EPSILONBR=1.f;
double xIn = x;
double yIn = y;
// Negate y for the joystick.
yIn = -yIn;
// Compenstate for gyro angle.
RotateVector(xIn, yIn, gyroAngle);
double wheelSpeeds[kMaxNumberOfMotors];
wheelSpeeds[0] = xIn + yIn + rotation;
wheelSpeeds[1] = -xIn + yIn - rotation;
wheelSpeeds[2] = -xIn + yIn + rotation;
wheelSpeeds[3] = xIn + yIn - rotation;
Normalize(wheelSpeeds);
uint8_t syncGroup = 0x80;
victorFL->Set(EPSILONFL*wheelSpeeds[0] * 1, syncGroup);
victorFR->Set(EPSILONFR*wheelSpeeds[1] * -1, syncGroup);
victorBL->Set(EPSILONBL*wheelSpeeds[2] * 1, syncGroup);
victorBR->Set(EPSILONBR*wheelSpeeds[3] * -1, syncGroup);
CANJaguar::UpdateSyncGroup(syncGroup);
}
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 RobotDemo::dumb_climber_code()
{
//printf("Top:%i Bottom:%i " , top_claw_limit_switch->Get() , bottom_claw_limit_switch->Get());
if (drive_stick_prim ->GetRawButton(climber_hold_up))
{
if (top_claw_limit_switch->Get() == 1)
{
climbing_motor->Set(0.0);
//printf("STOPPED\n");
}
else
{
climbing_motor->Set(1);
//printf("GOING UP\n");
}
} // not climber hold up
else if (drive_stick_prim->GetRawButton(climber_hold_down))
{
if (bottom_claw_limit_switch->Get() == 0)
{
climbing_motor->Set(0.0);
//printf("STOPED\n");
}
else
{
climbing_motor->Set(-1);
//printf("GOING DOWN\n");
}
} // hold down button
else
{ // no js buttons pushed
climbing_motor->Set(0.0);
}
}
/**
* Get the PWM value directly from the hardware.
*
* Read a raw value from a PWM channel.
*
* @param channel The PWM channel used for this Victor
* @return Raw PWM control value. Range: 0 - 255.
*/
UINT8 GetVictorRaw(UINT32 channel)
{
Victor *victor = (Victor *) AllocatePWM(channel, CreateVictorStatic);
if (victor)
return victor->GetRaw();
else
return 0;
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
while (IsOperatorControl())
{
left.Set(cont.GetRawAxis(2));
right.Set(cont.GetRawAxis(4));
}
}
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
}
void AutonomousPeriodic(void)
{
// Basic motor test code
leftFrontDrive->Set(1);
leftRearDrive->Set(1);
rightRearDrive->Set(1);
rightFrontDrive->Set(1);
}
/**
* This method is called every 20ms to update the robots components during autonomous.
* There should be no blocking calls in this method (connection requests, large data collection, etc),
* failing to comply with this could result in inconsistent robot behavior
*/
void AutonomousPeriodic()
{
//In the first two seconds of auto, drive forwardat half power
if(Timer::GetFPGATimestamp() - autoTimer >= 0 && Timer::GetFPGATimestamp() < 2)
{
rd->SetLeftRightMotorOutputs(.5, .5);
}
else if(Timer::GetFPGATimestamp() - autoTimer >= 2 && Timer::GetFPGATimestamp() < 4)
{
//2 to 4 seconds, stop drivetrain and spin shooter wheels
rd->SetLeftRightMotorOutputs(0, 0);
shoot1->Set(1);
shoot2->Set(1);
}
else if(Timer::GetFPGATimestamp() - autoTimer >= 4 && Timer::GetFPGATimestamp() < 8 && !kickerSwitch->Get())
{
//4 to 8 seconds and while kicker switch is not true, spin shooter wheels and kicker
shoot1->Set(1);
shoot2->Set(1);
kicker->Set(-1);
}
else
{
//After all that, stop everything
rd->SetLeftRightMotorOutputs(0, 0);
shoot1->Set(0);
shoot2->Set(0);
kicker->Set(0);
}
}
void Reset() {
m_talonCounter->Reset();
m_victorCounter->Reset();
m_jaguarCounter->Reset();
m_talon->Set(0.0f);
m_victor->Set(0.0f);
m_jaguar->Set(0.0f);
}
/**
* This method is called every 20ms to update the robots components during teleop.
* There should be no blocking calls in this method (connection requests, large data collection, etc),
* failing to comply with this could result in inconsistent robot behavior
*/
void TeleopPeriodic()
{
//Make the robot move like an arcade stick controlled device
rd->ArcadeDrive(j1);
//If button 1 on the joystick is pressed, spin the shooter wheels up,
//otherwise stop them
if(j1->GetRawButton(1))
{
shoot1->Set(1);
shoot2->Set(2);
}
else
{
shoot1->Set(0);
shoot2->Set(0);
}
//If button 2 on the joystick is pressed, spin the kicker, otherwise stop the kicker
if(j1->GetRawButton(2))
{
kicker->Set(-1);
}
else
{
kicker->Set(0);
}
}
void Autonomous()
{
compressor.Start();//start compressor
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
while (!shoot.Get()){//while shooter isnt cocked pull it back
shooter.SetSpeed(1); //set motor
}
shooterSole.Set(shooterSole.kForward);//Sets Solenoid forward, shoot ball
shooterSole.Set(shooterSole.kReverse);//Sets Solenoid backward
myRobot.Drive(1.0, -1.0);//turn around for 0.5 seconds, we have to check to see if it takes that long
Wait(0.5);//wait for 0.5 seconds
myRobot.Drive(0.0, 0.0);//stop robot
}
void ShootModeSet()
{
if (controller.GetRawButton(BTN_MODE_RELOAD) == true && ShootMode == eShoot)
{
ShootMode = eReload;
spinspeed = 0;
shootertime.Stop();
shootertime.Reset();
shottime.Stop();
shottime.Reset();
}
else if (controller.GetRawButton(BTN_MODE_SHOOT) == true && ShootMode == eReload)
{
ShootMode = eShoot;
spinspeed = 1; // CHANGE BACK TO 1. 40% IS BUTTERZONE FOR CHILDREN
shootertime.Start();
shottime.Start();
}
spin1.Set(-spinspeed);
}
void Disabled(void)
{
bool stopped = false;
while(!IsOperatorControl() && !IsAutonomous())
{
if(!stopped)
{
frontLeftMotor->Set(0.0);
rearLeftMotor->Set(0.0);
frontRightMotor->Set(0.0);
rearRightMotor->Set(0.0);
liftMotor1->Set(0.0);
liftMotor2->Set(0.0);
gripMotor1->Set(0.0);
//gripMotor2->Set(0.0);
PIDLift->Reset();
PIDDriveLeft->Reset();
PIDDriveRight->Reset();
PIDGrip->Reset();
stopped = true;
}
}
}
void TeleopPeriodic()
{
char myString[64];
float y;
// get the joystick position and filter the deadband
y = -joystick->GetY();
if (y > -0.1 && y < 0.1)
y = 0;
sprintf(myString, "joystick setting: %f\n", y);
SmartDashboard::PutString("DB/String 1", myString);
motor->Set(y, 0); // set the speed of the motor
sprintf(myString, "gear count: %d\n", gearToothCounter->Get());
SmartDashboard::PutString("DB/String 2", myString);
sprintf(myString, "gear stopped: %d\n", gearToothCounter->GetStopped());
SmartDashboard::PutString("DB/String 3", myString);
// sprintf(myString, "In val: %d\n", toothInput->GetValue());
// SmartDashboard::PutString("DB/String 2", myString);
}
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.
}
/********************************* Teleop methods *****************************************/
void MotorControlLeft(float speed)
{
left_1->SetSpeed(speed);
left_2->SetSpeed(speed);
}
void MotorControlRight(float speed)
{
right_1->SetSpeed(speed);
right_2->SetSpeed(speed);
}
void RobotDemo::stop(){
victorFL->Set(0.0);
victorFR->Set(0.0);
victorBL->Set(0.0);
victorBR->Set(0.0);
}
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);
}
/******************************************************************************
* Function: UpdateActuatorCmnds
*
* Description: Update the commands sent out to the RoboRIO.
******************************************************************************/
void UpdateActuatorCmnds(float ballRoller,
float desiredPos,
bool LgtB3,
bool LgtB5,
bool LgtB6,
bool LgtB7,
bool LgtB8,
bool LgtB9,
bool LgtB11,
float DrvLeft,
float DrvRight,
float fishTape,
float lowArm,
float wench)
{
float ArmError;
float ballRotate;
float ArmP = ballarm.GetDistance();
/* Set the output for the angle of the ball arm: */
if (desiredPos > 0)
{
/* If the desired position is above zero, the operator is commanding
* the arm to a given position */
ArmError = (desiredPos - ArmP) * (-0.01);
ballRotate = ArmError;
}
else if ((LgtB11 == true) && (ArmP < 211))
{
ballRotate = -1;
}
else if (LgtB5 && ArmP < 211)
{
ballRotate = -0.4;
}
else if (LgtB3 && ArmP > -10)
{
ballRotate = 0.4;
}
else if (LgtB6 && ArmP > 20)
{
ballRotate = 0.8;
}
else
{
/* If the desired position is zero, the operator is not wanting the arm
* to be controlled. Set the output to the motor to zero which will
* disable the motor and allow the arm to naturally fall. */
ArmError = 0;
ballRotate = ArmError;
}
/* Limit the allowed arm command if it could go past the allowed vertical position: */
if ((ArmP > 230 && LgtB5 == true) ||
(ArmP < -10 && LgtB3 == true))
{
ballRotate = 0;
}
/* This is limiting for when the arm goes past the vertical position: */
if (ArmP > 211)
{
ballRotate = 0.5;
}
BallRoller.Set(ballRoller);
BallRotate.Set(ballRotate);
myDrive.TankDrive(DrvLeft, DrvRight);
FishTape.Set(fishTape);
LowArm.Set(lowArm);
Wench.Set(wench);
}
void OperatorControl()
{
timer.Start();
while (IsOperatorControl())
{
x = Stick.GetRawAxis(1);
y = -Stick.GetRawAxis(2);
if (fabs(x) < 0.1)
{
x = 0;
}
if (fabs(y) < 0.1)
{
y = 0;
}
x2 = x*x;
y2 = y*y;
magnitude = pow((x2+y2),0.5);
angle = atan2(y,x)*180/PI;
magnitude = max(magnitude, -1);
magnitude = min(magnitude, 1);
if (angle < 0)
{
angle += 180;
magnitude *= -1;
}
/*front = 730 - (angle * 2.7888);
if ((front + 5) < float(fpot.GetValue()))
{
fturn.Set(0.5);
}
else if ((front - 5) > float(fpot.GetValue()))
{
fturn.Set(-0.5);
}
else
{
fturn.Set(0);
}*/
back = 235 + (angle * 2.8611);
if ((back + 10) < float(bpot.GetValue()))
{
bturn.Set(0.5);
}
else if ((back - 10) > float(bpot.GetValue()))
{
bturn.Set(-0.5);
}
else
{
bturn.Set(0);
}
if (timer.Get() > 0.1)
{
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Front = %5.4f", front);
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Back = %5.4f", back);
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Front Pot = %5.4f", float(fpot.GetValue()));
lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Back Pot = %5.4f", float(bpot.GetValue()));
lcd->Printf(DriverStationLCD::kUser_Line5, 1, "Angle = %5.4f", angle);
lcd->Printf(DriverStationLCD::kUser_Line6, 1, "Magnitude = %5.4f", magnitude);
lcd->UpdateLCD();
timer.Reset();
timer.Start();
}
}
}
void TeleopPeriodic(void) {
float x = gamepad->GetLeftX();
float y = gamepad->GetLeftY();
float rot = gamepad->GetRightX();
//small gamepad values are ignored
if (x < 0.1f && x > -0.1f)
{
x = 0;
}
if (y < 0.1f && y > -0.1f)
{
y = 0;
}
if (rot < 0.1f && rot > -0.1f)
{
rot = 0;
}
drive->MecanumDrive_Cartesian(SPEED_LIMIT * x, SPEED_LIMIT * y, SPEED_LIMIT * rot);
//shoot smoke if button is pressed
if (gamepad2->GetNumberedButton(FIRE_SMOKE_BUTTON)){
//SHOOT SMOKE!
//makingSmoke = !makingSmoke;
smoke_cannon->Set(SMOKE_CANNON_SPEED);
lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Shooting");
firing_smoke_timer->Start(); //measure how long we've fired smoke, so we know if it's ok to make more
}
else
{
smoke_cannon->Set(0.0f);
lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Not shooting");
firing_smoke_timer->Stop(); //stop the timer, since we're not firing smoke.
//don't reset, cuz we need to how much smoke we've fired.
}
//Eye Code
// float eye_pos = gamepad2->GetLeftX();
//
// right_eye_x->Set((eye_pos * 60) + default_eye_position);
// left_eye_x->Set((eye_pos * 60) + default_eye_position - LEFT_EYE_OFFSET);
//
// //button lock code
// if(gamepad2->GetNumberedButtonPressed(EYE_LOCK_BUTTON)){
// default_eye_position = eye_pos;
// }
//
//left eye control
//If A isn't pressed the value should stay the same as before
if (!gamepad2->GetNumberedButton(1)){
float left_joystick_x = gamepad2->GetLeftX();
float left_eye_x_axis = (1 - left_joystick_x)*60;
left_eye_val = left_eye_x_axis + 50;
float right_joystick_x = gamepad2->GetRawAxis(4);//right x axis
float right_eye_x_axis = (1-right_joystick_x)*60;
right_eye_val = right_eye_x_axis+20;
}
left_eye_x->SetAngle(left_eye_val);
right_eye_x->SetAngle(right_eye_val);
//float right_joystick_y = gamepad2->GetRawAxis(4);
//float right_eye_y_axis = (right_joystick_y+1)*60;
//right_eye_y->SetAngle(right_eye_y_axis);
/*
bool rbutton = gamepad2->GetNumberedButton(HEAD_UP_BUTTON);
bool lbutton = gamepad2->Ge tNumberedButton(HEAD_DOWN_BUTTON);
if (rbutton){
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "rb pressed");
jaw_motor->Set(0.2f);
}else if(lbutton){
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "lb pressed");
jaw_motor->Set(-0.15f);
}else{
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "no buttons");
jaw_motor->Set(0.0f);
}
*/
//REAL head & jaw code
//move head down
if(gamepad2->GetRightX()<=-0.5f && can_move_head_down() && can_move_jaw_down()){
head_motor->Set(-0.3f);
jaw_motor->Set(0.3f);
}
//move head up
else if(gamepad2->GetNumberedButton(HEAD_UP_BUTTON) && can_move_head_up()){
head_motor->Set(0.3f);
jaw_motor->Set(-0.3f);
}
//move jaw down
else if(gamepad2->GetRightX()>=0.5f && can_move_jaw_down()){
jaw_motor->Set(0.3f);
}
//move jaw up
else if(gamepad2->GetNumberedButton(JAW_UP_BUTTON) && can_move_jaw_up()){
jaw_motor->Set(-0.3f);
}
//sets to zero if no buttons pressed
else {
jaw_motor->Set(0.0f);
head_motor->Set(0.0f);
}
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "b:%d t:%d c:%d", bottomjaw_limit->Get(), tophead_limit->Get(), crash_limit->Get());
//Smoke code
if (gamepad2->GetNumberedButton(MAKE_SMOKE_BUTTON)){
//MAKE SMOKE!!!!!!!!!!!
//only if we don't have too much excess smoke
if (making_smoke_timer->Get() - firing_smoke_timer->Get() < MAX_EXCESS_SMOKE_TIME){
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "smoke");
smoke_machine->Set(true);
} else {
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "too much smoke");
smoke_machine->Set(false);
}
making_smoke_timer->Start(); //measure how long we've been making smoke, so we don't overflow the machine
//doesn't do anything if we've already started the timer
}
else
{
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "not smoke");
smoke_machine->Set(false);
making_smoke_timer->Stop(); //stop the timer, since we're not making smoke
//don't reset it, cuz we need to know how much smoke we've made
}
//if both timers are the same, we can set them both to zero to ensure we don't overflow them or something
if (making_smoke_timer->Get() == firing_smoke_timer->Get()){
making_smoke_timer->Reset();
firing_smoke_timer->Reset();
}
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "x:%f", x);
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "y:%f", y);
lcd->PrintfLine(DriverStationLCD::kUser_Line3, "r:%f", rot);
lcd->UpdateLCD();
}
/**
* Set the PWM value directly to the hardware.
*
* Write a raw value to a PWM channel.
*
* @param slot The slot the digital module is plugged into
* @param channel The PWM channel used for this Victor
* @param value Raw PWM value. Range 0 - 255.
*/
void SetVictorRaw(UINT32 slot, UINT32 channel, UINT8 value)
{
Victor *victor = (Victor *) AllocatePWM(slot, channel, CreateVictorStatic);
if (victor) victor->SetRaw(value);
}
void OperatorControl()
{
compressor.Start();
myRobot.SetSafetyEnabled(true);
myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);//Invert rear left Motor
myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);//Invert rear right motor
myRobot.SetInvertedMotor(myRobot.kFrontLeftMotor, true);//Invert rear right motor
myRobot.SetInvertedMotor(myRobot.kFrontRightMotor, true);
DriverStation *ds;
DriverStationLCD *dsLCD;
ds = DriverStation::GetInstance();
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1,1, "Starting Teleop");
dsLCD->UpdateLCD();
while (true)
{
if (!compressor.GetPressureSwitchValue()){
compressor.Start();
}
myRobot.ArcadeDrive(stick);
/*PNEUMATIC CODE*/
if (stick.GetRawButton(8)){
shooterSole.Set(shooterSole.kForward);
}
if (stick.GetRawButton(1)){
shooterSole.Set(shooterSole.kReverse);
}
/*SHOOTER CODE*/
if (stick.GetRawButton(2)){
shooter.SetSpeed(1);
}
if (stick.GetRawButton(4)){
shooter.SetSpeed(-1);
dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Shooter Should be negative");
dsLCD->UpdateLCD();
}
if (!stick.GetRawButton(4) || !stick.GetRawButton(2)){
shooter.SetSpeed(0);
}
/* FORKLIFT CODE*/
if (!stick.GetRawButton(5) || !stick.GetRawButton(6)){
forklift.SetSpeed(0);
}
if (stick.GetRawButton(5)){
forklift.SetSpeed(1);
}
if (stick.GetRawButton(6)){
forklift.SetSpeed(-1);
dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Forklift Should be negative");
dsLCD->UpdateLCD();
}
if (!shoot.Get()){
shooter.SetSpeed(0);
shooterSole.Set(shooterSole.kForward);
}
if (stick.GetRawButton(11)){
//myRobot.m_rearLeftMotor.SetSpeed(1);
//myRobot.m_rearRightMotor.SetSpeed(1);
//myRobot.m_frontLeftMotor.SetSpeed(1);
//myRobot.m_frontRightMotor.SetSpeed(1);
}
//Wait(0.005);// wait for a motor update time
}
}
/**
* Set the PWM value.
*
* The PWM value is set using a range of -1.0 to 1.0, appropriately
* scaling the value for the FPGA.
*
* @param channel The PWM channel used for this Victor
* @param speed The speed value between -1.0 and 1.0 to set.
*/
void SetVictorSpeed(UINT32 channel, float speed)
{
Victor *victor = (Victor *) AllocatePWM(channel, CreateVictorStatic);
if (victor) victor->Set(speed);
}
void OperatorControl(void)
{
autonomous = false;
//myRobot.SetSafetyEnabled(false);
//myRobot.SetInvertedMotor(kFrontLeftMotor, true);
// myRobot.SetInvertedMotor(3, true);
//variables for great pid
double rightSpeed,leftSpeed;
float rightSP, leftSP, liftSP, lastLiftSP, gripSP, tempLeftSP, tempRightSP;
float stickY[2];
float stickYAbs[2];
bool lightOn = false;
AxisCamera &camera = AxisCamera::GetInstance();
camera.WriteResolution(AxisCameraParams::kResolution_160x120);
camera.WriteMaxFPS(5);
camera.WriteBrightness(50);
camera.WriteRotation(AxisCameraParams::kRotation_0);
rightEncoder->Start();
leftEncoder->Start();
liftEncoder->Start();
rightEncoder->Reset();
leftEncoder->Reset();
liftEncoder->Reset();
bool fastest = false; //transmission mode
float reduction = 1; //gear reduction from
bool bDrivePID = false;
//float maxSpeed = 500;
float liftPower = 0;
float liftPos = -10;
bool disengageBrake = false;
int count = 0;
//int popCount = 0;
double popStart = 0;
double popTime = 0;
int popStage = 0;
int liftCount = 0;
int liftCount2 = 0;
const float LOG17 = log(17.61093344);
float liftPowerAbs = 0;
float gripError = 0;
float gripErrorAbs = 0;
float gripPropMod = 0;
float gripIntMod = 0;
bool shiftHigh = false;
leftEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //6-inch wheels, 1000 raw counts per revolution,
rightEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //about 1:1 gear ratio
DriverStationEnhancedIO &myEIO = driverStation->GetEnhancedIO();
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(0.3);
PIDDriveLeft->SetOutputRange(-1, 1);
PIDDriveRight->SetOutputRange(-1, 1);
//PIDDriveLeft->SetInputRange(-244,244);
//PIDDriveRight->SetInputRange(-244,244);
PIDDriveLeft->SetTolerance(5);
PIDDriveRight->SetTolerance(5);
PIDDriveLeft->SetContinuous(false);
PIDDriveRight->SetContinuous(false);
//PIDDriveLeft->Enable();
//PIDDriveRight->Enable();
PIDDriveRight->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN);
PIDDriveLeft->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN);
liftSP = 0;
PIDLift->SetTolerance(10);
PIDLift->SetContinuous(false);
PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
PIDLift->Enable();
gripSP = 0;
float goalGripSP = 0;
bool useGoalSP = true;
bool gripPIDOn = true;
float gripP[10];
float gripI[10];
float gripD[10];
PIDGrip->SetOutputRange(-0.5, 0.28); //Negative goes up
PIDGrip->SetTolerance(5);
PIDGrip->SetSetpoint(0);
PIDGrip->Enable();
miniDep->Set(miniDep->kForward);
int i = 0;
while(i < 10)
{
gripP[i] = GRIPPROPGAIN;
i++;
}
i = 0;
while(i < 10)
{
gripI[i] = GRIPINTGAIN;
i++;
}
i = 0;
while(i < 10)
{
gripD[i] = GRIPDERIVGAIN;
i++;
}
while (IsOperatorControl())
{
GetWatchdog().Feed();
count++;
#if !(SKELETON)
sendVisionData();
#endif
/*
if(LIFTLOW1BUTTON && !(counts%10)) printf("LIFTLOW1BUTTON\n");
if(LIFTLOW2BUTTON && !(counts%10)) printf("LIFTLOW2BUTTON\n");
if(LIFTMID1BUTTON && !(counts%10)) printf("LIFTMID1BUTTON\n");
if(LIFTMID2BUTTON && !(counts%10)) printf("LIFTMID2BUTTON\n");
if(LIFTHIGH1BUTTON && !(counts%10)) printf("LIFTHIGH1BUTTON\n");
if(LIFTHIGH2BUTTON && !(counts%10)) printf("LIFTHIGH2BUTTON\n");
*/
/*
if(lsLeft->Get()) printf("LSLEFT\n");
if(lsMiddle->Get()) printf("LSMIDDLE\n");
if(lsRight->Get()) printf("LSRIGHT\n");
*/
stickY[0] = stickL.GetY();
stickY[1] = stickR.GetY();
stickYAbs[0] = fabs(stickY[0]);
stickYAbs[1] = fabs(stickY[1]);
if(bDrivePID)
{
#if 0
frontLeftMotor->Set(stickY[0]);
rearLeftMotor->Set(stickY[0]);
frontRightMotor->Set(stickY[1]);
rearRightMotor->Set(stickY[1]);
if(!(count%5)) printf("Speeds: %4.2f %4.2f Outputs: %f %f \n", leftEncoder->GetRate(),
rightEncoder->GetRate(), frontLeftMotor->Get(), frontRightMotor->Get());
#endif
if(stickYAbs[0] <= 0.05 )
{
leftSP = 0;
if(!(count%3) && !BACKWARDBUTTON)
{
PIDDriveLeft->Reset();
PIDDriveLeft->Enable();
}
}
else leftSP = stickY[0] * stickY[0] * (stickY[0]/stickYAbs[0]); //set points for pid control
if(stickYAbs[1] <= 0.05)
{
rightSP = 0;
if(!(count%3) && !BACKWARDBUTTON)
{
PIDDriveRight->Reset();
PIDDriveRight->Enable();
}
}
else rightSP = stickY[1] * stickY[1] * (stickY[1]/stickYAbs[1]);
if (BACKWARDBUTTON)
{
tempRightSP = rightSP;
tempLeftSP = leftSP;
rightSP = -tempLeftSP;
leftSP = -tempRightSP; //This line and above line sets opposite values for the controller. ...Theoretically.
}
PIDDriveLeft->SetSetpoint(leftSP);
PIDDriveRight->SetSetpoint(rightSP);
leftSpeed = leftEncoder->GetRate();
rightSpeed = rightEncoder->GetRate();
if(!(count++ % 5))
{
printf("rate L: %2.2f R: %2.2f SP %2.4f %2.4f ERR %2.2f %2.2f Pow: %1.2f %1.2f\n",
leftPIDSource->PIDGet(), rightPIDSource->PIDGet(), leftSP, rightSP,
PIDDriveLeft->GetError(), PIDDriveRight->GetError(), frontLeftMotor->Get(),
frontRightMotor->Get());
//printf("Throttle value: %f", stickR.GetThrottle());
if(PIDDriveRight->OnTarget()) printf("Right on \n");
if(PIDDriveLeft->OnTarget()) printf("Left on \n");
}
if(PIDRESETBUTTON)
{
//PIDDriveRight->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN);
//PIDDriveLeft->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN);
PIDDriveLeft->Reset();
PIDDriveRight->Reset();
PIDDriveLeft->Enable();
PIDDriveRight->Enable();
}
}
else
{
if(PIDDriveLeft->IsEnabled()) PIDDriveLeft->Reset();
if(PIDDriveRight->IsEnabled()) PIDDriveRight->Reset();
if(DEMOSWITCH)
{
stickY[0] = stickY[0]*(1 - lift->getPosition()); //reduces power based on lift height
stickY[1] = stickY[0]*(1 - lift->getPosition());
}
if(stickYAbs[0] > 0.05)
{
frontLeftMotor->Set(stickY[0]);
rearLeftMotor->Set(stickY[0]);
}
else
{
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
}
if(stickYAbs[1] > 0.05)
{
frontRightMotor->Set(-stickY[1]);
rearRightMotor->Set(-stickY[1]);
}
else
{
frontRightMotor->Set(0);
rearRightMotor->Set(0);
}
}
if(stickL.GetRawButton(2) && stickL.GetRawButton(3) && stickR.GetRawButton(2) &&
stickR.GetRawButton(3) && BACKWARDBUTTON && !(count%5)) bDrivePID = !bDrivePID;
if((SHIFTBUTTON && shiftHigh) || DEMOSWITCH)
{
shifter->Set(shifter->kReverse);
shiftHigh = false;
maxSpeed = 12;
}
else if(!SHIFTBUTTON && !shiftHigh && !DEMOSWITCH)
{
shifter->Set(shifter->kForward);
shiftHigh = true;
maxSpeed = 25; //last value 35
}
sendIOPortData();
#if !(SKELETON)
/*
if(LIGHTBUTTON) lightOn = true;
else lightOn = false;
if(!lightOn) light->Set(light->kOff);
if(lightOn) light->Set(light->kOn);
*/
if(!MODESWITCH)
{
lastLiftSP = liftSP;
if(!PIDLift->IsEnabled()) PIDLift->Enable();
if(LIFTLOW1BUTTON) liftSP = LIFTLOW1;
if(LIFTLOW2BUTTON) liftSP = LIFTLOW2;
if(LIFTMID1BUTTON) liftSP = LIFTMID1;
if(LIFTMID2BUTTON) liftSP = LIFTMID2;
if(LIFTHIGH1BUTTON) liftSP = LIFTHIGH1;
if(LIFTHIGH2BUTTON && !(DEMOSWITCH && (stickYAbs[0] > 0.05 || stickYAbs[1] > 0.05))) liftSP = LIFTHIGH2;
if(!lift->isBraking() && !disengageBrake)
{
PIDLift->SetSetpoint(liftSP);
if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1) //BUGBUG
{
//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1.); //BUGBUG
}
else PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
}
if(lift->isBraking() && lastLiftSP != liftSP)
{
PIDLift->SetSetpoint(lastLiftSP + 0.06);
PIDLift->SetPID(12.5 + 1.5*lift->getPosition(), LIFTINTGAIN + 0.6 + 3*lift->getPosition()/10, 0);
lift->brakeOff();
disengageBrake = true;
if(!liftCount) liftCount = count;
}
//set brake (because near)
if(fabs(PIDLift->GetError()) < 0.01 && !lift->isBraking() && !disengageBrake)
{
if(liftCount == 0) liftCount = count;
if(count - liftCount > 3)
{
PIDLift->Reset();
liftMotor1->Set(LIFTNEUTRALPOWER);
liftMotor2->Set(LIFTNEUTRALPOWER);
Wait(0.02);
lift->brakeOn();
Wait(0.02);
liftMotor1->Set(0.0);
liftMotor2->Set(0.0);
PIDLift->Enable();
PIDLift->SetSetpoint(lift->getPosition());
liftCount = 0;
}
//if(!(count%50)) printf("Braking/Not PID \n");
}
if(lift->isBraking() && !(count%10)) PIDLift->SetSetpoint(lift->getPosition());
if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && count - liftCount > 3)
{
disengageBrake = false;
if(liftEncoder->PIDGet() < liftSP) PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN - 0.015);
else PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN + 0.015);
liftCount = 0;
}
//pid
/*
else if(!(fabs(PIDLift->GetError()) < 0.04) && !lift->isBraking() && liftPos == -20)
{
PIDLift->Enable();
liftPos = -10;
printf("PID GO PID GO PID GO PID GO PID GO \n");
}
*/
//when liftPos is positive, measures position
//when liftPos = -10, is pidding
//when liftPos = -20, has just released brake
}
else //(MODESWITCH)
{
if(PIDLift->IsEnabled()) PIDLift->Reset();
liftPower = .8*pow(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116), 2)*(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)/fabs(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)));
liftPowerAbs = fabs(liftPower);
if(liftPowerAbs > 1) liftPower /= liftPowerAbs;
//if(!(count%5)) printf("Slider: %f", liftPower);
if(lift->isBraking() && liftPowerAbs > 0.05) lift->brakeOff();
else if(!lift->isBraking() && liftPowerAbs <= 0.05 && !(count%5)) lift->brakeOn();
if (liftPowerAbs > 0.05)
{
liftMotor1->Set(liftPower);
liftMotor2->Set(liftPower);
}
else
{
liftMotor1->Set(0);
liftMotor2->Set(0);
}
}
if(MODESWITCH && LIFTLOW1BUTTON && LIFTMID1BUTTON && LIFTHIGH1BUTTON) liftEncoder->Reset();
/*
if(!(count%5))
{
printf("Lift pos: %f Lift error: %f Lift SP: %f \n", liftPIDSource->PIDGet(),
PIDLift->GetError(), PIDLift->GetSetpoint());
}
*/
if(!(count%5) && MODESWITCH && GRIPPERPOSUP && GRIPPERPOSDOWN && GRIPPERPOP)
{
gripPIDOn = !gripPIDOn;
printf("Switch'd\n");
}
if(gripPIDOn)
{
if(!PIDGrip->IsEnabled()) PIDGrip->Enable();
if(GRIPPERPOSUP && !GRIPPERPOSDOWN)
{
goalGripSP = 0;
}
else if(GRIPPERPOSDOWN && !GRIPPERPOSUP && lift->getPosition() < 0.5)
{
goalGripSP = 1;
}
/*
else if(!GRIPPERPOSDOWN && !GRIPPERPOSUP)
{
goalGripSP = 0.5;
}
*/
gripError = PIDGrip->GetError();
gripErrorAbs = fabs(gripError);
PIDGrip->SetSetpoint(goalGripSP);
if(gripErrorAbs < 0.4) PIDGrip->SetOutputRange(-0.4, 0.6); //negative is up
else PIDGrip->SetOutputRange(-0.9, 0.8); //negative is up
if(gripErrorAbs > 0.0 && gripErrorAbs < 0.2)
{
PIDGrip->SetPID(GRIPPROPGAIN - 1.25*(1 - gripErrorAbs) + gripPropMod, GRIPINTGAIN + gripIntMod, 0.3 + 0.1*(1 - gripPIDSource->PIDGet()));
}
else
{
PIDGrip->SetPID(GRIPPROPGAIN - 1.*(1 - gripErrorAbs) + gripPropMod, 0, 0.02);
}
if(fabs(gripPIDSource->PIDGet()) < 0.03 && PIDGrip->GetSetpoint() == 0)
{
gripLatch1->Set(true);
gripLatch2->Set(false);
}
else if(!(gripLatch1->Get() && PIDGrip->GetSetpoint() == 0) ||
gripPIDSource->PIDGet() < 0)
{
gripLatch1->Set(false);
gripLatch2->Set(true);
}
if(gripLatch1->Get() && !(count%20))
{
PIDGrip->Reset();
PIDGrip->Enable();
}
/*
if(stickL.GetRawButton(1) && !(count%5)){
gripIntMod -= 0.001;
}
if(stickR.GetRawButton(1) &&!(count%5))
{
gripIntMod += 0.001;
}
if(stickL.GetRawButton(2) && !(count%5))
{
gripPropMod -= 0.1;
}
if(stickL.GetRawButton(3) && !(count%5))
{
gripPropMod += 0.1;
}
*/
/*
if(LIFTBOTTOMBUTTON)
{
PIDGrip->Reset();
PIDGrip->Enable();
}
*/
if(!(count%5))
{
printf("Gripper pos: %f Gripper error: %f Grip power: %f \n",
gripPIDSource->PIDGet(), PIDGrip->GetError(), gripMotor1->Get());
}
}
//Calibration routine
else
{
if(gripLatch1->Get() == true)
{
gripLatch1->Set(false);
gripLatch2->Set(true);
}
if(PIDGrip->IsEnabled()) PIDGrip->Reset();
if(GRIPPERPOSUP)
{
gripMotor1->Set(-0.5);
//gripMotor2->Set(0.5);
}
else if(GRIPPERPOSDOWN)
{
gripMotor1->Set(0.5);
//gripMotor2->Set(-0.5);
}
else
{
gripMotor1->Set(0);
//gripMotor2->Set(0);
}
}
//if(!(count%5)) printf("Grip volts: %f \n", gripPot->GetVoltage());
//if(!(count%5)) printf("Grip 1 voltage: %f \n", gripMotor1->Get());
if(GRIPPEROPEN && !popStage)
{
#if !(INNERGRIP)
gripOpen1->Set(true);
gripOpen2->Set(false);
#else
gripOpen1->Set(false);
gripOpen2->Set(true);
#endif
}
else if(!popStage)
{
#if !(INNERGRIP)
gripOpen1->Set(false);
gripOpen2->Set(true);
#else
gripOpen1->Set(true);
gripOpen2->Set(false);
#endif
}
if(GRIPPERPOP && !popStage && goalGripSP == 0 && !(GRIPPEROPEN && GRIPPERCLOSE)) popStage = 1;
if(popStage) popTime = GetClock();
if(popStage == 1)
{
//popCount = count;
popStart = GetClock();
popStage++;
//printf("POP START POP START POP START \n");
}
if(popStage == 2)
{
#if !(INNERGRIP)
gripOpen1->Set(true);
gripOpen2->Set(false);
#else
gripOpen1->Set(false);
gripOpen2->Set(true);
#endif
popStage++;
//printf("GRIP OPEN GRIP OPEN GRIP OPEN \n");
}
if(popStage == 3 && popTime - popStart > 0.0) //used to be 0.15
{
gripPop1->Set(true);
gripPop2->Set(false);
popStage++;
//printf("POP OUT POP OUT POP OUT \n");
}
if(popStage == 4 && popTime - popStart > .75) //time was 0.9
{
gripPop1->Set(false);
gripPop2->Set(true);
popStage++;
//printf("POP IN POP IN POP IN \n");
}
if(popStage == 5 && popTime - popStart > 0.9) popStage = 0; //time was 1.05
if(MINIBOTSWITCH && !(MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1))) miniDep->Set(miniDep->kReverse);
else if(MINIBOTSWITCH && MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1)) miniDep->Set(miniDep->kOn);
#endif
if(!compSwitch->Get()) compressor->Set(compressor->kReverse);
else compressor->Set(compressor->kOff);
/*
if(stickR.GetRawButton(1)) compressor->Set(compressor->kReverse);
else compressor->Set(compressor->kForward);
*/
Wait(0.02); // wait for a motor update time
}
}
void RobotDemo::climber_code()
{
if (drive_stick_prim ->GetRawButton(climber_off_button))
{
climbing_motor-> Set(0.0);
claw_go_down = false;
claw_ = false;
}
else
{
if (drive_stick_prim ->GetRawButton(climber_hold_up))
{
claw_go_down = false;
claw_ = false;
if (top_claw_limit_switch->Get())
{
climbing_motor->Set(0.0);
}
else
{
climbing_motor->Set(1);
}
}// hold up
else
{
if (drive_stick_prim ->GetRawButton(climber_hold_down))
{
claw_go_down = false;
claw_ = false;
if (bottom_claw_limit_switch->Get() == false)
{
climbing_motor ->Set(0.0);
}
else
{
climbing_motor ->Set(-1);
}
} // hold down
else if (drive_stick_sec ->GetRawButton(climber_send_top))
{
claw_ = true;
claw_go_down = false;
}
else if (drive_stick_sec ->GetRawButton(climber_send_bottom))
{
claw_go_down = true;
claw_ = false;
}
else
{ // no holding or sending
if ((claw_ == false) && (claw_go_down == false))
{
climbing_motor ->Set(0.0);
}
}
} // no climber hold up button
} // no climber off button
if (claw_go_down)
{
if (bottom_claw_limit_switch->Get() == 1)
{
climbing_motor ->Set(-1);
}
else
{
climbing_motor ->Set(0.0);
}
}
if (claw_)
{
if (top_claw_limit_switch->Get() == 0)
{
climbing_motor->Set(1);
}
else
{
climbing_motor ->Set(0.0);
}
}
}
void OperatorControl(void)
{
float counter;
timer.Start();
float percent;
deadband = .05;
float pi = 3.141592653589793238462643;
float bpotval, fpotval;
float min = 600, max;
float fchgval = .5;
float bchgval = .5;
while (IsOperatorControl())
{
// comp.checkCompressor();
ShootModeSet();
Shoot(true);
fpotval = fpot.GetValue();
bpotval = bpot.GetValue();
counter = timer.Get();
if (controller.GetRawButton(3))
{
frot.SetSpeed(0);
brot.SetSpeed(0);
flmot.SetSpeed(0);
frmot.SetSpeed(0);
blmot.SetSpeed(0);
brmot.SetSpeed(0);
}
else if (controller.GetRawButton(BTN_L1) == false)
{
if (controller.GetRawButton(7))
{
if (fpotval < 860)
frot.SetSpeed(-0.5);
if (bpotval < 725)
brot.SetSpeed(-0.5);
if (fpotval > 860 and bpotval > 725)
{
frot.Set(0);
brot.Set(0);
flmot.Set(0.5);
frmot.Set(0.5);
blmot.Set(0.5);
brmot.Set(0.5);
}
}
else if (controller.GetRawButton(8))
{
if (fpotval < 860)
frot.SetSpeed(-0.5);
if (bpotval < 725)
brot.SetSpeed(-0.5);
if (fpotval > 860 and bpotval > 725)
{
frot.Set(0);
brot.Set(0);
flmot.Set(-0.5);
frmot.Set(-0.5);
blmot.Set(-0.5);
brmot.Set(-0.5);
}
}
else
{
if (controller.GetRawAxis(4) <= 0)
percent = ((acos(controller.GetRawAxis(3)) / pi));
else if (controller.GetRawAxis(4) > 0)
percent = ((acos(-controller.GetRawAxis(3)) / pi));
fpotval = percent * 550 + 330;
percent = (fpotval - 330) / 550;
bpotval = percent * 500 + 245;
if (fpot.GetValue() < fpotval)
fchgval = -.5;
else if (fpot.GetValue() > fpotval)
fchgval = .5;
if (fpot.GetValue() < fpotval + 10 && fpot.GetValue() > fpotval - 10)
fchgval = 0;
if (bpot.GetValue() > bpotval)
bchgval = -.5;
else if (bpot.GetValue() < bpotval)
bchgval = .5;
if (bpot.GetValue() < bpotval + 20 && bpot.GetValue() > bpotval - 20)
bchgval = 0;
frot.Set(fchgval);
brot.Set(bchgval);
if (pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2) > deadband && controller.GetRawButton(BTN_R1))
{
if (controller.GetRawAxis(4) > 0)
{
flmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
frmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
blmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
brmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
}
else if (controller.GetRawAxis(4) < 0)
{
flmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
frmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
blmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
brmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
}
}
else
{
flmot.Set(0);
frmot.Set(0);
blmot.Set(0);
brmot.Set(0);
}
contaxis4 = 0;
}
}
else
{
if (contaxis4 == 0)
contaxis4 = controller.GetRawAxis(4);
if (controller.GetRawButton(BTN_R1))
{
if (contaxis4 > 0)
{
flmot.Set(-0.25);
frmot.Set(0.25);
blmot.Set(-0.25);
brmot.Set(0.25);
}
else if (contaxis4 < 0)
{
flmot.Set(0.25);
frmot.Set(-0.25);
blmot.Set(0.25);
brmot.Set(-0.25);
}
}
}
if (float (fpot.GetValue()) < min)
min = float (fpot.GetValue());
else if (float (fpot.GetValue()) > max)
max = float (fpot.GetValue());
if (counter >= .1)
{
lcda->Clear();
lcda->Printf(DriverStationLCD::kUser_Line3, 1, "FPot :: %d", fpotval);
lcda->Printf(DriverStationLCD::kUser_Line4, 1, "BPot :: %d", bpotval);
lcda->UpdateLCD();
timer.Reset();
}
}
}
void AutonomousPeriodic()
{
drive->SetMaxOutput(1.0);
printf("Distance: %f\n", rightEnc->GetDistance());
// if (!launcherDown) {
// if (timer->Get() > 1.0) {
// winch->Set(0.5);
// otherWinch->Set(0.5);
// } else if (timer->Get() < 3.0) {
// winch->Set(0.5);
// otherWinch->Set(0.0);
// } else {
// winch->Set(0.0);
// otherWinch->Set(0.0);
// launcherDown = true;
// }
// }
if(done) {
winch->Set(0.0);
otherWinch->Set(0.0);
drive->ArcadeDrive(0.0,0.0);
if (autoCounter > 10) {
launchPiston->Set(0);
launch1->Set(0.0);
launch2->Set(0.0);
} else {
autoCounter++;
if (shoot == "Yes") {
launch1->Set(1.0);
launch2->Set(1.0);
}
}
} else {
if (autoSelected == "Approach Only") {
done = Autonomous::approachOnly();
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
} else if (!defenseCrossed) {
if(Autonomous::crossFunctions.find(autoSelected) != Autonomous::crossFunctions.end()) {
bool (*crossFunction)() = Autonomous::crossFunctions.at(autoSelected);
defenseCrossed = crossFunction();
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
} else {
done = true;
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
drive->ArcadeDrive(0.0,0.0);
}
timer->Reset();
} else {
if (autoSelected == "Spy Bot") {
rotation = 90;
}
//after we cross...
float difference = gyro->GetAngle() - rotation;
if (difference > 10) {
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
drive->ArcadeDrive(0.0,difference * 0.3);
timer->Reset();
} else {
if (goal == "Yes") {
if (!Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer)) {
launchPiston->Set(0);
} else {
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
drive->ArcadeDrive(0.0,0.0);
launchPiston->Set(1);
done = true;
}
} else {
launch1->Set(0.0);
launch2->Set(0.0);
winch->Set(0.0);
otherWinch->Set(0.0);
done = true;
}
}
}
}
}
void TeleopPeriodic()
{
//camera->GetImage(frame);
//imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f);
//CameraServer::GetInstance()->SetImage(frame);
printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle());
drive->ArcadeDrive(driveStick);
drive->SetMaxOutput((1-driveStick->GetThrottle())/2);
//printf("%f\n", (1-stick->GetThrottle())/2);
//leftMotor->Set(0.1);
//rightMotor->Set(0.1);
if (shootStick->GetRawAxis(3) > 0.5) {
launch1->Set(1.0);
launch2->Set(1.0);
} else if (shootStick->GetRawAxis(2) > 0.5) {
printf("Power Counter: %i\n", powerCounter);
if (powerCounter < POWER_MAX) {
powerCounter++;
launch1->Set(-0.8);
launch2->Set(-0.8);
} else {
launch1->Set(-0.6);
launch2->Set(-0.6);
}
} else {
launch1->Set(0.0);
launch2->Set(0.0);
powerCounter = 0.0;
}
//use this button to spin only one winch, to lift up.
if (shootStick->GetRawButton(7)) {
otherWinch->Set(0.5);
} else if (shootStick->GetRawButton(8)) {
otherWinch->Set(-0.5);
} else {
otherWinch->Set(0.0);
}
if (shootStick->GetRawButton(5)) {
winch->Set(-0.7);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
// otherWinch->Set(-0.5);
}
} else if (shootStick->GetRawButton(6)) {
winch->Set(0.7);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
// otherWinch->Set(0.5);
}
} else {
winch->Set(0.0);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//, otherWinch->Set(0.0);
}
}
if (shootStick->GetRawButton(1)) {
launchPiston->Set(1);
} else {
launchPiston->Set(0);
}
if (shootStick->GetRawButton(3)) {
Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer);
}
if (shootStick->GetRawButton(3) && debounce == false) {
debounce = true;
if (defenseUp) {
defensePiston->Set(DoubleSolenoid::Value::kReverse);
defenseUp = false;
} else {
defenseUp =true;
defensePiston->Set(DoubleSolenoid::Value::kForward);
}
} else if (!shootStick->GetRawButton(3)){
debounce = false;
}
}