//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 AutonomousPeriodic()
{
Scheduler::GetInstance()->Run(); // Was in default code don't know what it does
if (autoTimer.Get() > 0.0 ) { // If timer is greater then zero
if (autoTimer.Get() < 1.5 ) { // If the timer is less then 2.5
myDrive_all->TankDrive(0.8,0.8); // Make all motors go backwards
} else {// If the timer is greater then 2.5
myDrive_all->TankDrive(0.0,0.0); // Make all motors stop
}
} else { // If the timer is less then zero
autoTimer.Start(); // Starts timer
}
}
void AutonomousPeriodic()
{
if((Auto==1)||(Auto==2))
{
while(gyro.GetAngle()<ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/2,motorspeed/-2);
}
}
if((Auto==3)||(Auto==4))
{
while(gyro.GetAngle()>ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/-2,motorspeed/2);
}
}
while(EncDist.GetDistance()<EncVal1)
{
myRobot.Drive(-0.5, 0.0);
}
ang = -ang;
if((Auto==1)||(Auto==2))
{
while(gyro.GetAngle()<ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/2,motorspeed/-2);
}
}
if((Auto==3)||(Auto==4))
{
while(gyro.GetAngle()>ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/-2,motorspeed/2);
}
}
launcher.Set(1.0);
sonic.SetAutomaticMode(true);
while(sonic.GetRangeInches()>DefToShot)
{
myRobot.Drive(-0.5,0.0);
}
myRobot.Drive(0.0,0.0);
intake.Set(1.0);
}
void RobotDemo::arcade_tank_code()
{
if (drive_stick_prim ->GetRawButton(arcade_button))
{
arcadedrive = true;
}
else
{
if (drive_stick_prim ->GetRawButton(tank_button))
{
arcadedrive = false;
}
}
if (drive_stick_prim->GetRawButton(2) || drive_stick_sec->GetRawButton(2))
{
slow_control = true;
}
else
{
slow_control = false;
}
if (arcadedrive)
{
if (slow_control)
{
drive->ArcadeDrive(Adjustment_Speed * (drive_stick_prim->GetY()),
Adjustment_Speed * (drive_stick_prim->GetX()), true);
}
else
{
drive->ArcadeDrive((drive_stick_prim->GetY()),
(drive_stick_prim->GetX()), true);
}
}
else
{
if (slow_control)
{
drive->TankDrive(Adjustment_Speed * (drive_stick_prim->GetY()),
Adjustment_Speed * (drive_stick_sec->GetY()), true);
}
else
{
drive->TankDrive((drive_stick_prim->GetY()),
(drive_stick_sec->GetY()), true);
}
}
}
void TeleopPeriodic(void )
{
/*
* Code placed in here will be called only when a new packet of information
* has been received by the Driver Station. Any code which needs new information
* from the DS should go in here
*/
//Start compressor
compressor->Start();
driveTrainValues();
deadzone();
//Drivetrain.....
//When button eight is pressed robot drives at 25% speed
printf("right: %f and left: %f\n", useright, useleft);
if (gamepad->GetRawButton(8))
{
drivetrain->TankDrive((-0.5*(useleft)), (-0.5*(useright)));
//Negative for switched wires
}
else
{
drivetrain->SetLeftRightMotorOutputs(-useleft, -useright);
//Normal driving
//Negative for switched wires
}
}
/**
* 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()
{
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);
}
}
void AutonomousLowBar() {
// Strategy 2 - start in a normal position lined up with low bar, go through low bars and score boulder in lower goal.
// -------------------------------------------------------------------------------------------------------------------
// backUp straight for a little bit
// drop arm
// backup more under lowbar
// stop (we might add going to lowgoal later)
switch(currentState)
{
case 1:
timer->Reset();
timer->Start();
currentState = 2;
break;
case 2:
drive->TankDrive(autoSpeed,autoSpeed);
if(timer->Get() >= .4)
{
drive->TankDrive(0.0,0.0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
intakeLever->Set(autoIntakeSpeed);
if(timer->Get() >= .5)
{
intakeLever->Set(0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
drive->TankDrive(autoSpeed,autoSpeed);
if(timer->Get() >= autoLength)
{
drive->TankDrive(0.0,0.0);
currentState = 5;
timer->Reset();
timer->Stop();
}
break;
}
}
void TeleopPeriodic()
{
drive_mode_t new_mode = drive_mode_chooser.GetSelected();
SmartDashboard::PutString("current mode", new_mode == TANK_DRIVE ? "Tank" : "Arcade");
if (new_mode != drive_mode)
SetDriveMode(new_mode);
if (drive_mode == TANK_DRIVE) {
left_speed = accel(left_speed, pilot->LeftY(), TICKS_TO_ACCEL);
right_speed = accel(right_speed, pilot->RightY(), TICKS_TO_ACCEL);
drive->TankDrive(left_speed, right_speed);
}
else {
rot_speed = accel(rot_speed, pilot->RightX(), TICKS_TO_ACCEL);
SmartDashboard::PutNumber("rotation speed", rot_speed);
rot_speed = pilot->RightX();
move_speed = accel(move_speed, pilot->LeftY(), TICKS_TO_ACCEL);
drive->ArcadeDrive(move_speed * MOVE_SPEED_LIMIT, -rot_speed * MOVE_SPEED_LIMIT, false);
}
SmartDashboard::PutBoolean("clamp open", clamp->isOpen());
SmartDashboard::PutBoolean("sword in", clamp->isSwordIn());
SmartDashboard::PutData("gyro", gyro);
// for (uint8 i = 0; i <= 15; ++i)
// SmartDashboard::PutNumber(std::string("current #") + std::to_string(i), pdp->GetCurrent(i));
SmartDashboard::PutNumber("Current", pdp->GetTotalCurrent());
if (pilot->ButtonState(GamepadF310::BUTTON_A)) {
clamp->open();
}
else if (pilot->ButtonState(GamepadF310::BUTTON_B)){
clamp->close();
}
clamp->update();
SmartDashboard::PutNumber("accelerometer Z", acceler->GetZ());
SmartDashboard::PutNumber("Encoder", encoder->Get());
flywheel->Set(pilot->RightTrigger());
if (pilot->LeftTrigger() != 0)
flywheel->Set(-pilot->LeftTrigger());
SmartDashboard::PutNumber("Left Trigger:", pilot->LeftTrigger());
if (pilot->ButtonState(GamepadF310::BUTTON_X)) {
cameraFeeds-> changeCam(cameraFeeds->kBtCamFront);
}
if (pilot->ButtonState(GamepadF310::BUTTON_Y)){
cameraFeeds-> changeCam(cameraFeeds->kBtCamBack);
}
cameraFeeds->run();
}
void AutonomousAdjustableStraight() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
turnController->Reset();
turnController->SetSetpoint(ahrs->GetYaw());
turnController->Enable();
currentState = 2;
break;
case 2:
intakeLever->Set(autoIntakeSpeed);
if (timer->Get() >= 1) {
intakeLever->Set(0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
drive->TankDrive(autoSpeed, autoSpeed);
intakeLever->Set(-0.1);
if (timer->Get() >= autoLength) {
intakeLever->Set(0.0);
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
intake->Set(0.5);
shooter->Set(-0.5);
if (timer->Get() >= 2) {
currentState = 5;
}
break;
case 5:
intake->Set(0.0);
shooter->Set(0.0);
drive->TankDrive(0.0, 0.0);
break;
}
}
//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);
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
float speed = 0;
while (IsOperatorControl())
{
myRobot.TankDrive(leftstick, rightstick);
//driveleft.Set(1); //Why are we setting driveleft and driveright to 1 and -1?
//driveright.Set(-1);
}
}
void AutonomousStraightSpy() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
turnController->Reset();
turnController->SetSetpoint(ahrs->GetYaw());
turnController->Enable();
currentState = 2;
break;
case 2:
intakeLever->Set(0.25);
if (timer->Get() >= 1) {
intakeLever->Set(0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 5) {
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
intake->Set(0.5);
if (timer->Get() >= 2) {
currentState = 5;
}
break;
case 5:
intake->Set(0.0);
drive->TankDrive(0.0, 0.0);
break;
}
}
void GamepadDrive(Gamepad *gp)
{
float throttle;
if ( gp->GetButton(Gamepad::kRightTopTrigger) )
throttle = DRIVE_SPEED_HIGH;
else if ( gp->GetButton(Gamepad::kLeftTopTrigger) )
throttle = DRIVE_SPEED_LOW;
else
throttle = DRIVE_SPEED_MID;
_robotDrive->SetMaxOutput(throttle);
_robotDrive->TankDrive(gp->GetLeftY(), gp->GetRightY());
}
/**
* unchanged from SimpleDemo:
* Runs the motors under driver control with either tank or arcade steering selected
* by a jumper in DS Digin 0.
*/
void OperatorControl(void)
{
DPRINTF(LOG_DEBUG, "OperatorControl");
GetWatchdog().Feed();
while (1)
{
// 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)
}
}
} // end operator control
/**
* 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 AutonomousGyroTurn() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
//State: Stopped
//Transition: Driving State
currentState = 2;
break;
case 2:
//State: Driving
//Stay in State until 2 seconds have passed--`
//Transition: Gyroturn State
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) {
drive->TankDrive(0.0, 0.0);
ahrs->ZeroYaw();
currentState = 3;
turnController->SetSetpoint(90);
turnController->Enable();
}
break;
case 3:
//State: Gyroturn
//Stay in state until navx yaw has reached 90 degrees
//Transition: Driving State
drive->TankDrive(0.5 * rotateRate, -0.5 * rotateRate);
// if (ahrs->GetYaw() >= 90) {
if (turnController->OnTarget()) {
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
//State:Driving
//Stay in state until 2 seconds have passed
//Transition: State Stopped
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) {
currentState = 5;
timer->Stop();
}
break;
case 5:
//State: Stopped
drive->TankDrive(0.0, 0.0);
break;
}
}
void OperatorControl(void)
{
myRobot->SetSafetyEnabled(false);
LEDLights (true); //turn camera lights on
shooterspeedTask->Start((UINT32)this); //start counting shooter speed
kickerTask->Start((UINT32)this); //turns on the kicker task
kicker_in_motion = false;
while (IsOperatorControl() && !IsDisabled())
{
if (ControllBox->GetDigital(3)) //turn tracking on with switch 3 on controll box
{
tracking(ControllBox->GetDigital(7));
}
else
{
myRobot->TankDrive(leftstick, rightstick); //if tracking is off, enable human drivers
Wait(0.005); // wait for a motor update time
}
Shooter_onoff=ControllBox->GetDigital(4); //shoot if switch 4 is on
ballgatherer(ControllBox->GetDigital(5), rightstick->GetRawButton(10));
kicker_onoff=lonelystick->GetRawButton(1);
bridgeboot(ControllBox->GetDigital(6));
kicker_cancel=lonelystick->GetRawButton(2);
//kicker_down=rightstick->GetRawButton(11));
}
LEDLights (false);
shooterspeedTask->Stop();
kickerTask->Stop();
ballgatherer(false, false);
kickermotor->Set(Relay::kOff);
}
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
}
}
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);
}
}
}
// Our autonomous
void AutonomousPeriodic() override {
UpdateDashboard();
switch (autoSelected) {
default:
case 0:
drive->TankDrive(0.0, 0.0);
break;
case 1:
AutonomousGyroTurn();
break;
case 2:
AutonomousSpy();
break;
case 3:
AutonomousLowBar();
break;
case 4:
AutonomousStraightSpy();
break;
case 5:
AutonomousAdjustableStraight();
break;
}
}
// To determine which drive control type the robot to use
// In case user operator wants different controls
void Robot::DriverControl(int driveControl) {
switch (driveControl) {
// Arcade drive (think racing games) w/ 1 joysticks
case ARCADE_1:
// If the turn is outside the joystick's standard drift, robot will assume the robot is turning
// If not, robot will assume it is pushed, or has naturally drifted
if (fabs(rStick.GetRawAxis(X_AXIS)) > JOYSTICK_STANDARD_DRIFT) {
myRobot.ArcadeDrive(rStick.GetRawAxis(Y_AXIS), -rStick.GetRawAxis(X_AXIS));
gyro.Reset();
} else {
// The turn will be the opposite of what the gyro says the angle of unintentional drift is,
// which will have the robot go straight
myRobot.ArcadeDrive(rStick.GetRawAxis(Y_AXIS), editedGyroRate*GYRO_SCALE_FACTOR);
}
break;
// Arcade drive w/ 2 joysticks
case ARCADE_2:
if (fabs(rStick.GetRawAxis(X_AXIS)) > JOYSTICK_STANDARD_DRIFT) {
myRobot.ArcadeDrive(-lStick.GetRawAxis(Y_AXIS), -rStick.GetRawAxis(X_AXIS));
gyro.Reset();
} else {
myRobot.ArcadeDrive(-lStick.GetRawAxis(Y_AXIS), editedGyroRate*GYRO_SCALE_FACTOR);
}
break;
// Arcade drive w/ left stick on gamepad (the knockoff xbox controller)
case ARCADE_GAMEPAD_1:
if (fabs(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_X)) > JOYSTICK_STANDARD_DRIFT) {
myRobot.ArcadeDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_X));
gyro.Reset();
} else {
myRobot.ArcadeDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), editedGyroRate*GYRO_SCALE_FACTOR);
}
break;
// Arcade drive w/ BOTH gamepad
case ARCADE_GAMEPAD_2:
if (fabs(gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_X)) > JOYSTICK_STANDARD_DRIFT) {
myRobot.ArcadeDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_X));
gyro.Reset();
} else {
myRobot.ArcadeDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), editedGyroRate*GYRO_SCALE_FACTOR);
}
break;
// Tank drive (requires 2 sticks; each stick controls its respective 'tread' or side;
// Ex: moving the right stick moves only the wheels on the right) w/ gamepad
case TANK_GAMEPAD:
joystickDifference = gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y) - gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y);
joystickAverage = (gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y) + gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y))/2;
if (fabs(joystickDifference) > TANK_TURN_THRESHOLD) {
myRobot.TankDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y));
gyro.Reset();
} else {
myRobot.ArcadeDrive(joystickAverage, editedGyroRate*GYRO_SCALE_FACTOR);
}
break;
// Tank drive w/ joysticks
case TANK_2:
joystickDifference = lStick.GetRawAxis(LEFT_STICK_Y) - rStick.GetRawAxis(RIGHT_STICK_Y);
joystickAverage = (lStick.GetRawAxis(LEFT_STICK_Y) + rStick.GetRawAxis(RIGHT_STICK_Y))/2;
if (fabs(joystickDifference) > TANK_TURN_THRESHOLD) {
myRobot.TankDrive(lStick.GetRawAxis(LEFT_STICK_Y), rStick.GetRawAxis(RIGHT_STICK_Y));
gyro.Reset();
} else {
myRobot.ArcadeDrive(joystickAverage, -rStick.GetRawAxis(RIGHT_STICK_Y));
}
break;
}
}
void OperatorControl()
{
if(!m_FromAutonomous){
init();
}
m_FromAutonomous = false;
driveTrain.SetSafetyEnabled(true);
int printDelay = 0;
int shootDelay = 0;
//bool SavePreferencesToFlash = false;
while (IsOperatorControl() && IsEnabled())
{
/*
bool SavePreferences = gamePad.GetRawButton(8);
if (SavePreferences){
double elevatorAngleValue = SmartDashboard::GetNumber("Angle");
dashboardPreferences->PutDouble("Angle", elevatorAngleValue);
SavePreferencesToFlash = true;
}
*/
printDelay ++;
float rJoyStick = limitSpeed(rightJoyStick.GetY());
float lJoyStick = limitSpeed(leftJoyStick.GetY());
bool button6 = gamePad.GetRawButton(6);
//speedLimiter.SetMaxOutput(SmartDashboard::GetNumber("Slider 1"));
driveTrain.TankDrive(lJoyStick, rJoyStick);
//manual mode(no PID) for elevator
//float dPadThumbstick = TestMode::GetThumbstickWithZero(&gamePad);
//ballGrabber.DriveElevatorTestMode(dPadThumbstick);
//Sets motor equal to the elevator sensor.
//TODO Probably don't need but want to test because called inside operate grabber.
ballGrabber.OperatePIDLoop();
if(printDelay == 100){
lcd->Clear();
if(m_display_page_1)
{
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Teleop pg1");
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "FR %4.0f, BA %4.0f",
frontUltrasonic.GetAverageDistance(),
backUltrasonic.GetAverageDistance());
shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd);
SmartDashboard::PutNumber("UltrasonicF", 1);
SmartDashboard::PutNumber("UltrasonicB", 1);
SmartDashboard::PutNumber("ElvatorAngle", 2);//Change keyname to ElavatorAngle from (ElvatorAngle)
//^^
if(button6){
m_display_page_1 = false;
}
}
else{
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Teleop pg2 %c", button6 ? '1':'0');
ballGrabber.DisplayDebugInfo(DriverStationLCD::kUser_Line2,lcd);
//lcd->PrintfLine(DriverStationLCD::kUser_Line3, "G%f", ballGrabber.ballDetector.GetDistance());
//ballGrabber.UpDateWithState(DriverStationLCD::kUser_Line3,lcd);
shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd);
//lcd->PrintfLine(DriverStationLCD::kUser_Line4, "EV%6.2f", ballGrabber.elevatorAngleSensor.GetVoltage());
shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line4, lcd);
//lcd->PrintfLine(DriverStationLCD::kUser_Line4, "%5.3f %5.3f %5.3f", lJoyStick, rJoyStick, SmartDashboard::GetNumber("Slider 1"));
lcd->PrintfLine(DriverStationLCD::kUser_Line5, "DEV=%6.3f", ballGrabber.m_desiredElevatorVoltage);
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "CEV=%5.2f",
ballGrabber.elevatorAngleSensor.GetVoltage());
if(button6){
m_display_page_1 = true;
}
}
lcd->UpdateLCD();
printDelay = 0;
}
//int rotation = elevation.Get();
//the above is commented because we are not using it yet
bool shooterButton = gamePad.GetRawButton(7) || gamePad.GetRawButton(8);//TODO make constants
bool automaticAimButton = gamePad.GetRawButton(1);
//float distanceToWall = frontUltrasonic.GetAverageDistance();
//bool loadShooterButton = gamePad.GetRawButton(8);
if (shooterButton && shootDelay == 0){
shootDelay++;
}
if(shootDelay>0){
shootDelay++;
}
bool ReadyToShoot = (shootDelay>PHOENIX2014_LOOP_COUNT_FOR_SHOOT_DELAY);
shooter.OperateShooter(ReadyToShoot);
if (ReadyToShoot){
shootDelay = 0;
}
bool okToGrab = (shootDelay == 0);//Normaly 0 unless delaying
ballGrabber.OperateGrabber(shooterButton, okToGrab);
//Trying to make some things happen automatically during teleoperated
if(automaticAimButton){
ballGrabber.m_desiredElevatorVoltage = PHOENIX2014_TELEOP_ELEVATOR_ANGLE;
}
//((distanceToWall > (12.0*11.0)) && distanceToWall < (12.0*13.0)){
//lightBulb.Set(Relay::kOn);
//}
//else{
#ifdef WANTWEIRDPULSING
if (printDelay < 30) {
lightBulb.Set(Relay::kForward);
}
else if (printDelay >= 30 && printDelay < 65) {
lightBulb.Set(Relay::kReverse);
}
else {
lightBulb.Set(Relay::kOff);
}
#endif
Wait(0.005);// wait for a motor update time
} // end of while enabled
driveTrain.StopMotor();
ballGrabber.StopPidLoop();
shooter.motorShutOff();
/*
if(SavePreferencesToFlash){
dashboardPreferences->Save();
SavePreferencesToFlash = false;
}
*/
} // end of OperatorControl()
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous()
{
init();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Entered Autonomous");
driveTrain.SetSafetyEnabled(false);
bool checkBox1 = SmartDashboard::GetBoolean("Checkbox 1");
m_FromAutonomous = true;
//float rightDriveSpeed = -1.0;
//float leftDriveSpeed = -1.0;
//int rangeToWallClose = 60;
//int rangeToWallFar = 120;
//Initialize encoder.
//int distanceToShoot = 133;
//int shootDelay = 0;
//ballGrabber.elevatorController.SetSetpoint(PHOENIX2014_INITIAL_AUTONOMOUS_ELEVATOR_ANGLE);
//TODO Remove encoders from code??
driveDistanceRight.Reset();
driveDistanceLeft.Reset();
driveDistanceRight.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_RIGHT);
driveDistanceLeft.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_LEFT);
driveDistanceRight.Start();
driveDistanceLeft.Start();
//int printDelay = 0;
float minDistance = 52.0; // Closer to the wall than this is too close
float maxDistance = 12.0*11.0; // Once at least this close, within shooting range
//float closeDistance = maxDistance + 24.0;
float autoDriveSpeed = 0.55;
//float autoDriveSlowSpeed = 0.38;
int time = 0;
int maxDriveLoop = 4*200; // 4 seconds @200 times/sec
bool shootingBall = false;
bool wantFirstShot = true;
if(checkBox1 == false){
int printDelay = 0;
//Ultrasonic Autonomous
//bool isInRange = false;
while(IsAutonomous() && IsEnabled())
{
float currentDistance = frontUltrasonic.GetAverageDistance();
// Transition isInRange from false to true once
if((minDistance < currentDistance) && (currentDistance < maxDistance)){
//isInRange = true;
}
time++;
bool motorDriveTimeExceeded = time > maxDriveLoop;
bool motorMinMet = time > m_MinDriveLoop;
if(/*isInRange ||*/ motorMinMet){
driveTrain.TankDrive(0.0,0.0);
if((ballGrabber.elevatorAngleSensor.GetVoltage() > PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE - 0.1) &&
(ballGrabber.elevatorAngleSensor.GetVoltage() < PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE + 0.1)){
//Enough to cover break release and start winding again.
shootingBall = shooter.OperateShooter(wantFirstShot);
}
if(shootingBall == true){
wantFirstShot = false;
}
}
else if(motorDriveTimeExceeded){
driveTrain.TankDrive(0.0,0.0);
}
else{
//if((currentDistance < closeDistance) && motorMinMet){
// autoDriveSpeed = autoDriveSlowSpeed;
//}
driveTrain.TankDrive(-0.97 * autoDriveSpeed, -1.0 * autoDriveSpeed);//the DriveTrain is BACKWARD
}
ballGrabber.RunElevatorAutonomous(PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE);
printDelay = printDelay++;
if(printDelay >= 200){
lcd->Clear();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "In Autonomous");
shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line2, lcd);
shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd);
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "Ulra=%f", frontUltrasonic.GetAverageDistance());
//lcd->PrintfLine(DriverStationLCD::kUser_Line5, "CEV=%f", ballGrabber.elevatorAngleSensor.GetVoltage());
//lcd->PrintfLine(DriverStationLCD::kUser_Line6, "DEV=%f", ballGrabber.m_desiredElevatorVoltage);
lcd->UpdateLCD();
printDelay = 0;
}
Wait(0.005);
}
lcd->Clear();
lcd->UpdateLCD();
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "Exiting Autonomous");
} else {
//Timer Autonomous
driveTrain.TankDrive(-0.5,-0.5);
ballGrabber.DriveElevatorTestMode(-1.0);
lcd->Clear();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Skip Auto");
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "CheckBox Checked");
lcd->UpdateLCD();
Wait(2.0);
bool shooting = true;
driveTrain.TankDrive(0.0,0.0);
int counter = 0;
while(counter < 600){
shooter.OperateShooter(shooting);
Wait(0.005);
}
}
}
/****************************************
* 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);
}
}
/**
* Tank drive using the Kinect.
*/
void Autonomous(void)
{
double leftAxis, rightAxis;
double leftAngle, rightAngle, headAngle, rightLegAngle, leftLegAngle, rightLegYZ, leftLegYZ=0;
bool dataWithinExpectedRange;
bool buttons[8];
while (IsAutonomous())
{
/* Only process data if skeleton is tracked */
if (kinect->GetTrackingState() == Kinect::kTracked) {
/* Determine angle of each arm and map to range -1,1 */
leftAngle = AngleXY(kinect->GetSkeleton().GetShoulderLeft(), kinect->GetSkeleton().GetWristLeft(), true);
rightAngle = AngleXY(kinect->GetSkeleton().GetShoulderRight(), kinect->GetSkeleton().GetWristRight(), false);
leftAxis = CoerceToRange(leftAngle, -70, 70, -1, 1);
rightAxis = CoerceToRange(rightAngle, -70, 70, -1, 1);
/* Check if arms are within valid range and at approximately the same z-value */
dataWithinExpectedRange = (leftAngle < ARM_MAX_ANGLE) && (leftAngle > ARM_MIN_ANGLE)
&& (rightAngle < ARM_MAX_ANGLE) && (rightAngle > ARM_MIN_ANGLE);
dataWithinExpectedRange = dataWithinExpectedRange &&
InSameZPlane(kinect->GetSkeleton().GetShoulderLeft(),
kinect->GetSkeleton().GetWristLeft(),
Z_PLANE_TOLERANCE) &&
InSameZPlane(kinect->GetSkeleton().GetShoulderRight(),
kinect->GetSkeleton().GetWristRight(),
Z_PLANE_TOLERANCE);
/* Determine the head angle and use it to set the Head buttons */
headAngle = AngleXY(kinect->GetSkeleton().GetShoulderCenter(), kinect->GetSkeleton().GetHead(), false);
buttons[0] = headAngle > HEAD_LEFT;
buttons[1] = headAngle < HEAD_RIGHT;
/* Calculate the leg angles in the XY plane and use them to set the Leg Out buttons */
leftLegAngle = AngleXY(kinect->GetSkeleton().GetHipLeft(), kinect->GetSkeleton().GetAnkleLeft(), true);
rightLegAngle = AngleXY(kinect->GetSkeleton().GetHipRight(), kinect->GetSkeleton().GetAnkleRight(), false);
buttons[2] = leftLegAngle > LEG_OUT;
buttons[3] = rightLegAngle > LEG_OUT;
/* Calculate the leg angle in the YZ plane and use them to set the Leg Forward and Leg Back buttons */
leftLegYZ = AngleYZ(kinect->GetSkeleton().GetHipLeft(), kinect->GetSkeleton().GetAnkleLeft(), false);
rightLegYZ = AngleYZ(kinect->GetSkeleton().GetHipRight(), kinect->GetSkeleton().GetAnkleRight(), false);
buttons[4] = rightLegYZ < LEG_FORWARD;
buttons[5] = rightLegYZ > LEG_BACKWARD;
buttons[6] = rightLegYZ < LEG_FORWARD;
buttons[7] = rightLegYZ > LEG_BACKWARD;
if (dataWithinExpectedRange){
/**
* Drives using the Kinect axes scaled to 1/3 power
* Axes are inverted so arms up == joystick pushed away from you
*/
myRobot.TankDrive(-leftAxis*.33, -rightAxis*.33);
/**
* Do something with boolean "buttons" here
*/
/* Optional SmartDashboard display of Kinect values */
//dash->PutDouble("Left Arm", -leftAxis);
//dash->PutDouble("Right Arm", -rightAxis);
//dash->PutBoolean("Head Left", buttons[0]);
//dash->PutBoolean("Head Right", buttons[1]);
//...etc...
}
else{
/* Arms are outside valid range */
myRobot.TankDrive(0.0, 0.0);
/**
* Do default behavior with boolean "buttons" here
*/
/* Optional SmartDashboard display of Kinect values */
//dash->PutDouble("Left Arm", 0);
//dash->PutDouble("Right Arm", 0);
//dash->PutBoolean("Head Left", false);
//dash->PutBoolean("Head Right", false);
//...etc...
}
}
else{
/* Skeleton not tracked */
myRobot.TankDrive(0.0, 0.0);
/**
* Do default behavior with boolean "buttons" here
*/
/* Optional SmartDashboard display of Kinect values */
//dash->PutDouble("Left Arm", 0);
//dash->PutDouble("Right Arm", 0);
//dash->PutBoolean("Head Left", false);
//dash->PutBoolean("Head Right", false);
//...etc...
}
Wait(0.01);
}
}
/******************************************************************************
* 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()
{
GetWatchdog().SetEnabled(true);
intake->LiftIntake();
LEDLight->Set(Relay::kOff);
while (IsOperatorControl() && !IsDisabled())
{
myRobot->TankDrive(leftStick, rightStick);
rpi->Read();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "L: %f", leftEnco->GetDistance());
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "R: %i", rpi->GetMissingPacketcount());
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "%i", rpi->GetXPos());
lcd->Printf(DriverStationLCD::kUser_Line4, 1, "%i", catapult->GetLoadedLimit1());
lcd->Printf(DriverStationLCD::kUser_Line5, 1, "%i", catapult->GetLoadedLimit2());
lcd->UpdateLCD();
//Driver controls
//Move the intake in if the R-2 trigger is pressed
if(rightStick->GetRawButton(2))
{
intake->RollIn();
}
//Move it out if the R-3 trigger is pressed
else if(rightStick->GetRawButton(3))
{
intake->RollOut();
}
//Intake the ball only far enough for a pass if R-3 is pressed
/*else if(rightStick->GetRawButton(3))
{
intake->GetBallForPass();
}*/
//If none of them are pressed, stop the intake
else
{
intake->Stop();
}
//Catapult stuff
if(rightStick->GetRawButton(1) && leftStick->GetRawButton(1))
{
catapult->StartShoot();
}
else if(leftStick->GetRawButton(10))
{
catapult->StartLoad();
}
else if(leftStick->GetRawButton(11))
{
catapult->StartRelease();
}
//If the right-6 button and l-10 button are pressed, stop the catapult
if(rightStick->GetRawButton(6) && leftStick->GetRawButton(10))
{
catapult->Stop();
}
//These functions need to called all of the time, but don't do anything until
//Their start method is called
catapult->ReleaseHold();
catapult->Shoot();
catapult->Load();
GetWatchdog().Feed();
Wait(0.005); // wait for a motor update time
}
}
/**
* unchanged from SimpleDemo:
*
* Runs the motors under driver control with either tank or arcade
* steering selected by a jumper in DS Digin 0.
*
* added for vision:
*
* Adjusts the servo gimbal based on the color tracked. Driving the
* robot or operating an arm based on color input from gimbal-mounted
* camera is currently left as an exercise for the teams.
*/
void OperatorControl(void)
{
char funcName[] = "OperatorControl";
DPRINTF(LOG_DEBUG, "OperatorControl");
//GetWatchdog().Feed();
TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT);
/* for controlling loop execution time */
float loopTime = 0.05;
double currentTime = GetTime();
double lastTime = currentTime;
double savedImageTimestamp = 0.0;
bool foundColor = false;
bool staleImage = false;
while (IsOperatorControl())
{
setServoPositions(rightStick->GetX(), rightStick->GetY());
/* calculate gimbal position based on color found */
if (FindColor
(IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, &par,
&cReport))
{
foundColor = true;
if (par.imageTimestamp == savedImageTimestamp)
{
// This image has been processed already,
// so don't do anything for this loop
staleImage = true;
}
else
{
staleImage = false;
savedImageTimestamp = par.imageTimestamp;
// compute final H & V destination
horizontalDestination = par.center_mass_x_normalized;
verticalDestination = par.center_mass_y_normalized;
}
}
else
{
foundColor = false;
}
PrintReport(&cReport);
if (!staleImage)
{
if (foundColor)
{
/* Move the servo a bit each loop toward the
* destination. Alternative ways to task servos are
* to move immediately vs. incrementally toward the
* final destination. Incremental method reduces the
* need for calibration of the servo movement while
* moving toward the target.
*/
ShowActivity
("** %s found: Servo: x: %f y: %f",
td.name, horizontalDestination, verticalDestination);
}
else
{
ShowActivity("** %s not found", td.name);
}
}
dashboardData.UpdateAndSend();
// sleep to keep loop at constant rate
// elapsed time can vary significantly due to debug printout
currentTime = GetTime();
lastTime = currentTime;
if (loopTime > ElapsedTime(lastTime))
{
Wait(loopTime - ElapsedTime(lastTime));
}
}
while (IsOperatorControl())
{
// determine if tank or arcade mode; default with no jumper is
// for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0)
{
// drive with tank style
myRobot->TankDrive(leftStick, rightStick);
}
else
{
// drive with arcade style (use right stick)
myRobot->ArcadeDrive(rightStick);
}
}
} // end operator control
bool tracking (bool use_alternate_score) //camera tracking function
{
//takes one camera frame and turns towards tallest target
//returns true if target is within deadzone, returns false otherwise
Threshold tapeThreshold(0, 255, 0, 90, 220, 255); //red hsl as of 20110303, this is the hue, saturation and luminosicity ranges that we want
BinaryImage *tapePixels;//
Image *convexHull;
BinaryImage *convexHullBinaryImage;
ParticleAnalysisReport par;//analyzed blob (pre convex hull)
ParticleAnalysisReport convexpar;// ONE filled-in blob
vector<ParticleAnalysisReport>* pars;//where many analyzed blob goes (pre)
vector<ParticleAnalysisReport>* convexpars; //where MANY filled-in blobs go
bool foundAnything = false;
double best_score = 120;
double best_speed;
double particle_score;
ImageType t;
int bs;
img = cam->GetImage();
printf("cam->GetImage() returned frame %d x %d\n",img->GetWidth(),img->GetHeight());
tapePixels = img->ThresholdHSL(tapeThreshold);
imaqGetBorderSize(tapePixels->GetImaqImage(),&bs);
imaqGetImageType(tapePixels->GetImaqImage(),&t);
convexHull = imaqCreateImage(t,bs);
convexHullBinaryImage = new BinaryImageWrapper(convexHull);
convexHullBinaryImage->GetOrderedParticleAnalysisReports();
//tapePixels = img->ThresholdHSL(int 0,int 50,int -100,int -50,int luminenceLow,int luminanceHigh);
pars = tapePixels->GetOrderedParticleAnalysisReports();
imaqConvexHull(convexHull,tapePixels->GetImaqImage(),true);
convexHullBinaryImage = new BinaryImageWrapper(convexHull);
convexpars = convexHullBinaryImage->GetOrderedParticleAnalysisReports();
//imaqGetParticleInfo()
//convexpars = convexHull->GetOrderedParticleAnalysisReports();
for (int i=0;i < convexHullBinaryImage->GetNumberParticles();i++)
{
//par = (*pars)[0];
//convexpar = (*convexpars)[i];
convexpar = convexHullBinaryImage->GetParticleAnalysisReport(i);
par = tapePixels->GetParticleAnalysisReport(i);
if((convexpar.boundingRect.width < 10) || (convexpar.boundingRect.height < 7))
{
continue;
}
// printf("%d par:%f convex:%f particle area\n",i,par.particleArea,convexpar.particleArea);
if ((par.particleArea/convexpar.particleArea > 0.4))
{
printf("%d skip max fillness ratio\n",i);
continue;
}
if ((par.particleArea/convexpar.particleArea < 0.10))
{
printf("%d skip min fillness ratio\n",i);
continue;
}
if((double)(convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)>1.8)
{
printf("%d skip max aspect ratio\n",i);
continue;
}
if((double)(convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)<.8)
{
printf("%d skip min aspect ratio\n",i);
continue;
}
//printf("%f center of mass x\n",par.center_mass_x_normalized);
//printf("%f center of mass y\n",par.center_mass_y_normalized);
distanceInInches = (18.0*179.3)/(convexpar.boundingRect.height);
double pwidth = convexpar.boundingRect.width;
double mwidth = ((double)convexpar.boundingRect.left+(double)convexpar.boundingRect.width*0.5);
double angle = ((180.0/3.14159)*acos (pwidth * distanceInInches/179.3/24.0) );
if(angle != angle) angle = 0.0; // if angle is NaN, set to zero
printf("%f distance in inches\n",distanceInInches);
//printf("%f angle\n",(180.0/3.14159)*acos (pwidth * distanceInInches/415.0/24.0) );
printf("%d BBctrX:%f CMX:%f\n", i, (double)convexpar.boundingRect.left + (double)convexpar.boundingRect.width*0.5, (double)par.center_mass_x);
//printf("%f angle2\n",(((pwidth * distanceInInches)/415.0)/24.0));
//printf("%f center of mass x\n",par.center_mass_x_normalized);
printf("%d %f %f center of mass x\n",i,convexpar.center_mass_x_normalized,par.center_mass_x_normalized);
printf("%d %f %f center of mass y\n",i,convexpar.center_mass_y_normalized,par.center_mass_y_normalized);
printf("%d %f %f rectangle score\n",i,(convexpar.particleArea)/((convexpar.boundingRect.width)*(convexpar.boundingRect.height))*(100),(par.particleArea)/((par.boundingRect.width)*(par.boundingRect.height))*(100));
printf("%d %f fillness ratio\n",i,par.particleArea/convexpar.particleArea);
printf("%d %d %d width and height\n",i,(convexpar.boundingRect.width),(convexpar.boundingRect.height));
printf("%d %f aspect ratio\n",i,((convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)));
if ((double)(par.center_mass_x)>mwidth)
{
angle=angle*(-1.0);
}
printf("%f true angle\n",angle);
//Wait(1.0);
double aiming_target_offset = 0.0;
//aiming_target_offset = pwidth * angle * (-0.5 / 45.0); numbers are iffy -> NaN
double speed = trackingFeedbackFunction(mwidth + aiming_target_offset - 80.0);
printf("%f aiming_target_offset due to %f degree angle\n", aiming_target_offset, angle);
printf("%f x offset \n",mwidth + aiming_target_offset - 80.0);
printf("%f speed \n", speed);
foundAnything = true;
if (use_alternate_score == false){
particle_score = convexpar.center_mass_y;
}
else{
particle_score = 2.0*fabs((double)convexpar.center_mass_y - 60.0) + fabs((double)convexpar.center_mass_x - 80.0);
}
// keep track of the *lowest* score
if (best_score > particle_score)
{
best_score = particle_score;
best_speed = speed;
}
}
if(foundAnything == false) {
myRobot->TankDrive(0.0, 0.0);
}
else
{
myRobot->TankDrive(-best_speed,best_speed);
}
delete img;
delete tapePixels;
delete pars;
delete convexHullBinaryImage;
delete convexpars;
//imaqDispose(convexHull);
if (foundAnything && best_speed == 0.0){
return true;
}
else
{
return false;
}
}