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 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);
}
}
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()
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
while (IsOperatorControl())
{
left.Set(cont.GetRawAxis(2));
right.Set(cont.GetRawAxis(4));
}
}
void AutonomousPeriodic(void)
{
// Basic motor test code
leftFrontDrive->Set(1);
leftRearDrive->Set(1);
rightRearDrive->Set(1);
rightFrontDrive->Set(1);
}
void RobotDemo::dumb_drive_code()
{
#if DUMB_DRIVE_CODE
left_drive_motor_A->Set(-drive_stick_sec ->GetY());
left_drive_motor_B->Set(-drive_stick_sec->GetY());
right_drive_motor_A->Set(drive_stick_prim->GetY());
right_drive_motor_B->Set(drive_stick_prim->GetY());
#endif
}
/**
* 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 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 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 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 OperatorControl(void)
{
// Teleoperated Code.
/*double WaitDash = 0.0;
double FireDash = 0.0;
double intPause = 0.0;
SmartDashboard::PutNumber("P", 3.0);
SmartDashboard::PutNumber("W", 0.0);
SmartDashboard::PutNumber("A", 0.0);
WaitDash = SmartDashboard::GetNumber("P");
FireDash = SmartDashboard::GetNumber("W");
intPause = SmartDashboard::GetNumber("A");
SmartDashboard::PutNumber("P", WaitDash);
SmartDashboard::PutNumber("W", FireDash);
SmartDashboard::PutNumber("A", WaitDash);
*/
// Enable and start the compressor.
//compressor->Enabled();
compressor->Start();
// Enable drive motor safety timeout.
myRobot.SetSafetyEnabled(true);
// Enable watchdog and initial feed.
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(1);
GetWatchdog().Feed();
// Set robot in low gear by default. Not active.
//s[0]->Set(false);
GetWatchdog().Feed();
//bool blnShoot = false;
bool blnLowHang = false;
bool blnShift = false;
GetWatchdog().Feed();
bool blnShooterSpd = false;
bool blnReverse = false;
float fltShoot;
float fltSpeed = 1;
int intFail = 0;
timerLowHang.Reset();
timerShift.Reset();
timerFire.Reset();
timerShooter.Reset();
timerDriveCtrl.Reset();
timerCamera.Reset();
timerReverse.Reset();
timerLowHang.Start();
timerShift.Start();
timerFire.Start();
timerShooter.Start();
timerDriveCtrl.Start();
timerCamera.Start();
timerReverse.Start();
GetWatchdog().Feed();
//sd->sendIOPortData();
// Local variables.
//float fltStick1X, fltStick1Y;
while (IsOperatorControl())
{
if(timerReverse.Get() > 0.5)
{
if(stick1->GetRawButton(8) && blnReverse == false )
{
blnReverse = true;
GetWatchdog().Feed();
timerReverse.Reset();
timerReverse.Start();
}
else if(stick1->GetRawButton(9) && blnReverse == true)
{
blnReverse = false;
GetWatchdog().Feed();
timerReverse.Reset();
timerReverse.Start();
}
}
if(blnReverse == false)
{
fltStick1Y = stick1->GetY();
fltStick1X = stick1->GetX();
SmartDashboard::PutBoolean("Reverse",false);
GetWatchdog().Feed();
}
else if(blnReverse == true)
{
fltStick1Y = ((stick1->GetY())*(-1));
fltStick1X = ((stick1->GetX())*(1));
SmartDashboard::PutBoolean("Reverse",true);
GetWatchdog().Feed();
}
myRobot.ArcadeDrive(fltStick1Y,fltStick1X);
//myRobot.ArcadeDrive(stick1);
GetWatchdog().Feed(); // Feed hungary demonic Watchdog.
SmartDashboard::PutBoolean("Touching Tower?",LimitSwitch->Get());
SmartDashboard::PutNumber("Throttle (%)",stick1->GetY()*(-100));
SmartDashboard::PutNumber("Steering (%)",stick1->GetX()*(100));
GetWatchdog().Feed();
//End Stick1 arcade drive code.
GetWatchdog().Feed();
fltShoot = (((-(stick2->GetRawAxis(3)))+1)/2);
GetWatchdog().Feed();
SmartDashboard::PutNumber("Shooter Power (%)", fltShoot);
SmartDashboard::PutNumber("Shooter Set Speed (%)", (fltSpeed*100));
//float fltPressureSwitch = m_pressureSwitch;
//float fltRelay = m_relay;
//SmartDashboard::PutNumber("Demo",3);
GetWatchdog().Feed();
if(timerShift.Get() > 0.2)
{
if(stick1->GetRawButton(7) || stick1->GetTrigger())
{
if(blnShift == false)
{
GetWatchdog().Feed();
s[0]->Set(false);
s[1]->Set(true);
SmartDashboard::PutString("Gear","Low");
blnShift = true;
timerShift.Stop();
timerShift.Reset();
timerShift.Start();
GetWatchdog().Feed();
}
else if (blnShift == true)
{
GetWatchdog().Feed();
s[0]->Set(true);
s[1]->Set(false);
SmartDashboard::PutString("Gear","High");
blnShift = false;
timerShift.Stop();
timerShift.Reset();
timerShift.Start();
GetWatchdog().Feed();
}
}
}
if(stick1->GetRawButton(2) && blnLowHang == false && timerLowHang.Get() > 0.5)
{
blnLowTime = true;
blnLowHang = true;
timerLowHang.Stop();
timerLowHang.Reset();
timerLowHang.Start();
GetWatchdog().Feed();
}
else if(stick1->GetRawButton(2) && blnLowHang == true && timerLowHang.Get() > 0.5)
{
blnLowTime = false;
blnLowHang = false;
timerLowHang.Stop();
timerLowHang.Reset();
timerLowHang.Start();
GetWatchdog().Feed();
}
if(blnLowTime == true)
{
s[3]->Set(true);
GetWatchdog().Feed();
}
else if (blnLowTime == false)
{
s[3]->Set(false);
GetWatchdog().Feed();
}
if(stick1->GetRawButton(3))
{
mtdCameraCode();
GetWatchdog().Feed();
}
if(stick2->GetTrigger() && intFail == 0 && timerFire.Get() > 0.8)
{
s[2]->Set(true);
SmartDashboard::PutString("Shooter Piston","In");
intFail = 1;
GetWatchdog().Feed();
timerFire.Stop();
timerFire.Reset();
timerFire.Start();
GetWatchdog().Feed();
}
else if(stick2->GetTrigger() && intFail == 1 && timerFire.Get() > 0.8)
{
s[2]->Set(false);
intFail = 0;
SmartDashboard::PutString("Shooter Piston","Out");
GetWatchdog().Feed();
timerFire.Stop();
timerFire.Reset();
timerFire.Start();
GetWatchdog().Feed();
}
if(stick2->GetRawButton(2) && blnShooterSpd == false && timerShooter.Get() > 0.5)
{
GetWatchdog().Feed();
myShooter1.Set(-fltSpeed);
myShooter2.Set(-fltSpeed);
SmartDashboard::PutString("Shooter","On");
SmartDashboard::PutNumber("Shooter Speed (%)",(fltSpeed)*(100));
blnShooterSpd = true;
GetWatchdog().Feed();
timerShooter.Stop();
timerShooter.Reset();
timerShooter.Start();
GetWatchdog().Feed();
}
else if(stick2->GetRawButton(2) && blnShooterSpd == true && timerShooter.Get() > 0.5)
{
GetWatchdog().Feed();
myShooter1.Set(0);
myShooter2.Set(0);
SmartDashboard::PutString("Shooter","Off");
SmartDashboard::PutNumber("Shooter Speed (%)",0);
blnShooterSpd = false;
GetWatchdog().Feed();
//Wait(0.2);
timerShooter.Stop();
timerShooter.Reset();
timerShooter.Start();
GetWatchdog().Feed();
}
if(stick2->GetRawButton(10))
{
fltSpeed = 0.6;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(9))
{
fltSpeed = 0.7;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(8))
{
fltSpeed = 0.8;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(7))
{
fltSpeed = 0.9;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(6))
{
fltSpeed = 1;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(11))
{
fltSpeed = fltShoot;
GetWatchdog().Feed();
}
GetWatchdog().Feed();
}
}
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 Autonomous(void)
{
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(1);
GetWatchdog().Feed();
s[0]->Set(true);
s[1]->Set(true);
SmartDashboard::PutString("Gear","High");
compressor->Start();
myRobot.SetSafetyEnabled(true);
int WaitDash = 0;
int FireDash = 0;
int intPause = 0;
GetWatchdog().Feed();
// Incase we need to recreate the controls for Autonomous.
//SmartDashboard::PutNumber("fire_wait", 3);
//SmartDashboard::PutNumber("fire_amount", 3);
//SmartDashboard::PutNumber("fire_pause", 5);
WaitDash = static_cast<int>(SmartDashboard::GetNumber("fire_wait"));
FireDash = static_cast<int>(SmartDashboard::GetNumber("fire_amount"));
intPause = static_cast<int>(SmartDashboard::GetNumber("fire_pause"));
GetWatchdog().Feed();
/* CAMERA THRESHOLD AND DECLARATIONS, UNCOMMENT IF CAMERA READDED TO AUTONOMOUS
//Their threshold values suck DDDD, from the NIvision assistant will be below.
//Threshold threshold(60, 100, 90, 255, 20, 255); //HSV threshold criteria, ranges are in that order ie. Hue is 60-100
//Threshold threshold(100, 255, 230, 255, 140, 255); //New threshold values
Threshold threshold(0, 255, 0, 255, 221, 255);
ParticleFilterCriteria2 criteria[] = {
{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
}; //Particle filter criteria, used to filter out small particles
// AxisCamera &camera = AxisCamera::GetInstance(); //To use the Axis camera uncomment this line
*/
while (IsAutonomous() && IsEnabled())
{
myShooter1.Set(-1);
myShooter2.Set(-1);
GetWatchdog().Feed();
GetWatchdog().Feed();
// Wait before firing.
for(intAutoCtrl = 0; intAutoCtrl < (1);intAutoCtrl++)
{
GetWatchdog().Feed();
for(intDelay = 0; intDelay< (((intPause)*2)+1);intDelay++)
{
GetWatchdog().Feed();
Wait(0.5);
}
GetWatchdog().Feed();
// Fire a number of frisbees as set in the dashboard.
//for(intCount = 0; intCount < (FireDash+1); intCount++)
// {
GetWatchdog().Feed();
s[2]->Set(true);
// Delay between firing each frisbees.
for(int wait = 0; wait<((WaitDash)*2);wait++)
{
GetWatchdog().Feed();
Wait(0.5);
}
s[2]->Set(false);
GetWatchdog().Feed();
for(intSecondWait = 0;intSecondWait < 3;intSecondWait++)
{
GetWatchdog().Feed();
Wait(0.5);
}
GetWatchdog().Feed();
}
GetWatchdog().Feed();
}
myShooter1.Set(0);
myShooter2.Set(0);
GetWatchdog().Feed();
//}
}
void TeleopPeriodic(void)
{
//Drive code
leftFrontDrive->Set(-1 * leftStick->GetY());
rightFrontDrive->Set(rightStick->GetY());
leftRearDrive->Set(-1 * leftStick->GetY());
rightRearDrive->Set(rightStick->GetY());
//airCompressor->Start();
// Simple Button Drive Forward
if(rightStick->GetRawButton(3))
{
leftFrontDrive->Set(-1);
rightFrontDrive->Set(1);
leftRearDrive->Set(-1);
rightRearDrive->Set(1);
}
// Simple Button Drive Backwards
if(rightStick->GetRawButton(2))
{
leftFrontDrive->Set(1);
rightFrontDrive->Set(-1);
leftRearDrive->Set(1);
rightRearDrive->Set(-1);
}
// Code to print to the user messages box in the Driver Station
LCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hello World");
LCD->Printf(DriverStationLCD::kUser_Line2,1,"Y Left: %f", leftStick->GetY());
LCD->Printf(DriverStationLCD::kUser_Line3,1,"Y Right: %f", rightStick->GetY());
LCD->UpdateLCD();
Wait(0.2);
}
void 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();
}
/******************************************************************************
* 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();
}
}
}
/**
* 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 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;
}
}
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::stop(){
victorFL->Set(0.0);
victorFR->Set(0.0);
victorBL->Set(0.0);
victorBR->Set(0.0);
}
void OperatorControl (void) {
GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
while (IsOperatorControl() && IsEnabled()) {
GetWatchdog().Feed(); // Feed the Watchdog.
kinectMode = (bool) kinectModeSelector->GetSelected();
demoMode = (bool) demoModeSelector->GetSelected();
speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
if (kinectMode) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
if (!demoMode) {
if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else {
motorDriveLeft.Set(0);
motorDriveRight.Set(0);
}
}
if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
motorTurret.Set(TURRET_POWER);
} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
if (joystickDriveLeft.GetThrottle() == 0) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
motorTurret.Set(TURRET_POWER);
} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(MANIPULATOR_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
motorTurret.Set(joystickManipulator.GetX() * -1 * TURRET_POWER);
}
}
dsLCD->UpdateLCD();
}
GetWatchdog().SetEnabled(false); // Teleop is done, so let's turn off Watchdog.
}
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(false);
AxisCamera &camera = AxisCamera::GetInstance("10.28.53.11");
camera.WriteResolution(AxisCamera::kResolution_320x240);
camera.WriteRotation(AxisCamera::kRotation_180);//Flip image upside-down.
camera.WriteCompression(80);//Compresses the image(?)
camera.WriteBrightness(50);//Sets the brightness to 80 on a scale from 0-100
DriverStationLCD *screen = DriverStationLCD::GetInstance();
int count = 0;
while(IsOperatorControl())
{
screen->UpdateLCD();
count++;
HSLImage* imgpointer; //declares a new hue saturation lum image
imgpointer = camera.GetImage(); //grabs an image to initialize that image
//imaqCreateImage(IMAQ_IMAGE_U8,)
//imaqCast(NULL, imgpointer, IMAQ_IMAGE_U8, NULL, -1);
BinaryImage* binImg = NULL; //declares a new binary image
//ThresholdHSL changes our regular image into a binary image.
/*hueLow Low value for hue
hueHigh High value for hue
saturationLow Low value for saturation
saturationHigh High value for saturation
luminenceLow Low value for luminence
luminenceHigh High value for luminence
*/
binImg = imgpointer->ThresholdHSL(1, 255, 1, 255, 230, 255);
//Saves the image to a temp directory
binImg->Write("/tmp/thresh.jpg");
//Writes the binary image to disk
imaqWriteFile(binImg->GetImaqImage(), "/tmp/thresh2.jpg", NULL);
delete imgpointer;
Image* Convex = imaqCreateImage(IMAQ_IMAGE_U8, 0);
int returnvalue;
returnvalue = imaqConvexHull(Convex, binImg->GetImaqImage(), TRUE);
// imaqWriteFile(Convex, "/tmp/convex.jpg", NULL);
delete binImg;
// screen->PrintfLine(DriverStationLCD::kUser_Line4,"convex is %d",returnvalue);
screen->UpdateLCD();
float lookuptable[256];
lookuptable[0] = 0;
lookuptable[1] = 65535;
//Converts image to 16 bit, then back to 8 bit.
Image* cast = imaqCreateImage(IMAQ_IMAGE_U16, 0);
imaqCast(cast, Convex, IMAQ_IMAGE_U16, lookuptable, 0);
imaqDispose(Convex);
Image* bitcast = imaqCreateImage(IMAQ_IMAGE_U8, 0);
imaqCast(bitcast, cast, IMAQ_IMAGE_U8, NULL, 0);
imaqDispose(cast);
// screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());//Notifies the user
imaqWriteFile(bitcast, "/tmp/bitcast.jpg", NULL);
//Image* SuperSize = im
//imaqCreateImage(IMAQ_IMAGE_U8, 2240);
//int returnvalue2;
//returnvalue2 = imaqSizeFilter(SuperSize, Convex, TRUE, 1, IMAQ_KEEP_LARGE, NULL);
//screen->UpdateLCD();
//imaqDispose (Convex);
// ROI *roi;
// Rect rectangle;
// rectangle.top = 0;
// rectangle.left = 0;
// rectangle.width = 320;
// rectangle.height = 120;
// imaqAddRectContour(roi,rectangle);
static RectangleDescriptor rectangleDescriptor =
{
30, // minWidth (All values are estimated)
200, // maxWidth
20, // minHeight
200 // maxHeight
};
static CurveOptions curveOptions = //extraction mode specifies what the VI identifies curves in the image. curve options are all the
{
IMAQ_NORMAL_IMAGE, // extractionMode
75, // threshold
IMAQ_NORMAL, // filterSize
25, // minLength
15, // rowStepSize
15, // columnStepSize
10, // maxEndPointGap
FALSE, // onlyClosed
FALSE // subpixelAccuracy
};
static ShapeDetectionOptions shapeOptions =
{
IMAQ_GEOMETRIC_MATCH_ROTATION_INVARIANT, // mode
NULL, // angle ranges
0, // num angle ranges
{75, 125}, // scale range
300 // minMatchScore
};
int matches = 0;
//double highscore = 0;
//int highestindex = -1;
float difference = 0;
float time = 0;
float average = 0;
double y = 0;
//The big important line of code that does important stuff
RectangleMatch* recmatch = imaqDetectRectangles(bitcast, &rectangleDescriptor, &curveOptions, &shapeOptions, NULL, &matches);
// DashboardDataSender *dds;
// dds = new DashboardDataSender;
//screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());
//screen->PrintfLine(DriverStationLCD::kUser_Line4,"Matches: %d",matches);//Notifies the user
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Matches: %i",matches);//Notifies the user
screen->UpdateLCD();
/*for(int i = 0; i < matches; i++)
{
//screen->PrintfLine((DriverStationLCD::Line)(i+1),"%i,%i,%i",recmatch[i].height,recmatch[i].width,recmatch[i].score);
if(recmatch[i].score > highscore)
{
highscore = recmatch[i].score;
highestindex = i;
}
screen->PrintfLine(DriverStationLCD::kUser_Line1,"score %i, i %i", recmatch[i].score, i);
screen->UpdateLCD();
}*/
average = (recmatch->corner[1].x + recmatch->corner[3].x)/2;
y = (472/(recmatch->height-4));
screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f",average);
screen->PrintfLine(DriverStationLCD::kUser_Line3," %f",distance(recmatch->height));
//screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f,%f",recmatch->height, y);
screen->PrintfLine(DriverStationLCD::kUser_Line6,"%f,%f",ceil(recmatch->corner[1].x),ceil(recmatch->corner[3].x));
screen->UpdateLCD();
//screen->PrintfLine(DriverStationLCD::kUser_Line6,"%d, %d, %d, %d",recmatch->corner[1].x,recmatch->corner[2].x,recmatch->corner[3].x,recmatch->corner[4].x);
//screen->PrintfLine(DriverStationLCD::kUser_Line5,"%d, %d",recmatch->height,recmatch->width);
if(average == 0)
{
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Image Not Found");
}
else if(average < 145)
{
difference = (160 - average);
time = difference*0.0062;
vic.Set(0.1);
Wait(time);
vic.Set(0.0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Less!");
screen->UpdateLCD();
}
else if (average > 175)
{
difference = (average - 160);
time = difference*0.0062;
vic.Set(-0.1);
Wait(time);
vic.Set(0.0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"More!");
screen->UpdateLCD();
}
else
{
vic.Set(0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Perfection!");
screen->UpdateLCD();
}
screen->PrintfLine(DriverStationLCD::kUser_Line2,"Time: %f",time);
screen->PrintfLine(DriverStationLCD::kUser_Line4,"difference %f",difference);
// screen->PrintfLine(DriverStationLCD::kUser_Line2,"Score %i, Index %i", highscore, highestindex);
/* if(matches > 0)
{
int counter = 0;
while (counter < 20)
{
average = (recmatch[highestindex].corner[1].x + recmatch[highestindex].corner[2].x + recmatch[highestindex].corner[3].x +recmatch[highestindex].corner[4].x)/4;
average2 = (recmatch[highestindex].corner[1].y + recmatch[highestindex].corner[2].y + recmatch[highestindex].corner[3].y +recmatch[highestindex].corner[4].y)/4;
// screen->PrintfLine(DriverStationLCD::kUser_Line5,"Center = %f, %f",average, average2);
screen->UpdateLCD();
if (average > 170)
{
vic.Set(-0.1);
//screen->PrintfLine(DriverStationLCD::kUser_Line1,"Left %f",vic.Get());
//screen->UpdateLCD();
}
else if (average < 150)
{
vic.Set(0.1);
//screen->PrintfLine(DriverStationLCD::kUser_Line1,"Right %f",vic.Get());
//screen->UpdateLCD();
}
else
{
counter = 20;
vic.Set(0);
// screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get());
screen->UpdateLCD();
}
counter++a;
}
*/
// imaqWriteFile(cast, "/tmp/linedetect.jpg", NULL);
// }
/* else
{
vic.Set(0);
// screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get());
screen->UpdateLCD();
}*/
imaqDispose(bitcast);
imaqDispose(recmatch);
//imaqDispose(roi);
vic.Set(0);
}
}
