void LEDLights (bool onoff) //light function
{
if (onoff) {
rlyLED->Set(Relay::kForward);// turn on LEDs
} else {
rlyLED->Set(Relay::kOff);// turn off LEDs
}
}
SebastianRobot(void) {
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Sebastian V24.2");
dsLCD->UpdateLCD();
GetWatchdog().SetEnabled(false);
led0 = new Relay(2);
led1 = new Relay(3);
flashCount = 0;
led0->Set(Relay::kOff);
led1->Set(Relay::kOff);
isFlashing=false;
}
void toggleBrake()
{
if(isBraking())
{
m_brake->Set(m_brake->kForward);
m_bIsBraking = false;
}
else
{
m_brake->Set(m_brake->kReverse);
m_bIsBraking = true;
}
}
/**
* Set the relay state.
*
* Valid values depend on which directions of the relay are controlled by the object.
*
* When set to kBothDirections, the relay can only be one of the three reasonable
* values, 0v-0v, 0v-12v, or 12v-0v.
*
* When set to kForwardOnly or kReverseOnly, you can specify the constant for the
* direction or you can simply specify kOff and kOn. Using only kOff and kOn is
* recommended.
*
* @param slot The slot that the digital module is plugged into
* @param channel The relay channel number for this object
* @param value The state to set the relay.
*/
void SetRelay(UINT32 slot, UINT32 channel, RelayValue value)
{
Relay *relay = AllocateRelay(slot, channel);
if (relay != NULL)
{
switch (value)
{
case kOff: relay->Set(Relay::kOff); break;
case kOn: relay->Set(Relay::kOn); break;
case kForward: relay->Set(Relay::kForward); break;
case kReverse: relay->Set(Relay::kReverse); break;
}
}
}
void OperatorControl(void) {
XboxController *xbox = XboxController::getInstance();
bool isEndGame = false;
GetWatchdog().SetEnabled(true);
_driveControl.initialize();
//_poleVaultControl.initialize();
shooterControl.InitializeOperator();
while (IsEnabled() && IsOperatorControl()) {
GetWatchdog().Feed();
dsLCD->Clear();
if (xbox->isEndGame()) {
isEndGame = !isEndGame;
}
if (!isEndGame) {
shooterControl.Run();
//_poleVaultControl.act();
_driveControl.act();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Normal");
led0->Set((shooterControl.getLED1())?Relay::kOn: Relay::kOff);
led1->Set((shooterControl.getLED2())?Relay::kOn: Relay::kOff);
}
else {
shooterControl.RunEndGame();
//_poleVaultControl.actEndGame();
_driveControl.actEndGame();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "End Game");
flashCount--;
if (flashCount<=0){
isFlashing = !isFlashing;
flashCount=FLASHTIME;
}
led0->Set((isFlashing)?Relay::kOn: Relay::kOff);
led1->Set((isFlashing)?Relay::kOn: Relay::kOff);
}
// dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "Flash: %i", flashCount);
dsLCD->UpdateLCD();
Wait(WAIT_TIME); // wait for a motor update time
}
GetWatchdog().SetEnabled(false);
}
void DisabledInit() {
//Config loading
try {
cameraLight->Set(Relay::kOff);
if (!Config::LoadFromFile("config.txt")) {
cout << "Something happened during the load." << endl;
}
Config::Dump();
myDrive->DisablePID();
myDrive->ResetPID();
if(fout.is_open() && !freshStart && !ds->IsFMSAttached()){
fout.close();
myShooter->ResetShooterProcess();
lc->holdState(false);
}
} catch (exception ex) {
cout << "Disabled exception. Trying again." << endl;
cout << "Exception: " << ex.what() << endl;
}
//ResetShooterMotors();
/*
SmartDashboard::PutNumber("Target Info S",1741);
cout<<SmartDashboard::GetNumber("Target Info S");
*/
}
RobotDemo()
{
//Initialize the objects for the drive train
myRobot = new RobotDrive(1, 2);
leftEnco = new Encoder(LEFT_ENCO_PORT_1, LEFT_ENCO_PORT_2);
rightEnco = new Encoder(RIGHT_ENCO_PORT_1, RIGHT_ENCO_PORT_2);
leftEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
leftEnco->Start();
rightEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
rightEnco->Start();
leftStick = new Joystick(1);
rightStick = new Joystick(2);
//Initialize the gyro
gyro = new Gyro(GYRO_PORT);
gyro->Reset();
//Initialize the manipulators
intake = new Intake(INTAKE_ROLLER_PORT, BALL_SENSOR_PORT, LEFT_SERVO_PORT, RIGHT_SERVO_PORT);
catapult = new Catapult(LOADING_MOTOR_PORT, HOLDING_MOTOR_PORT, LOADED_LIMIT_1_PORT,
LOADED_LIMIT_2_PORT, HOLDING_LIMIT_PORT);
//Initialize the objects needed for camera tracking
rpi = new RaspberryPi("17140");
LEDLight = new Relay(1);
LEDLight->Set(Relay::kForward);
//Set the autonomous modes
autonMode = ONE_BALL_AUTON;
autonStep = AUTON_ONE_SHOOT;
//Initialize the lcd
lcd = DriverStationLCD::GetInstance();
}
/**
* Initialization code for autonomous mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters autonomous mode.
*/
void RA14Robot::AutonomousInit() {
Config::LoadFromFile("config.txt");
auto_case = (int) Config::GetSetting("auto_case", 1);
alreadyInitialized = true;
auto_timer->Reset();
auto_timer->Start();
missionTimer->Start();
myDrive->ResetOdometer();
myCamera->Set(Relay::kForward);
myCollection->ExtendArm();
cout<<"Reseting Gyro"<<endl;
gyro->Reset();
//myOdometer->Reset();
//myDrive->ShiftUp();
myDrive->ShiftDown();
//shift to high gear
if (!fout.is_open()) {
cout << "Opening logging.csv..." << endl;
fout.open("logging.csv");
logheaders();
}
auto_state = 0;
#ifndef DISABLE_SHOOTER
myCam->Reset();
myCam->Enable();
#endif //Ends DISABLE_SHOOTER
}
void AutonomousInit() {
init();
currentDistance = 0;
goalDistance = 6.0*12.0; // 6 feet
autonomousShooterDelay = 0;
shooterMotor->Set(0);
loaderMotor->Set(Relay::kOff);
}
/**
* Initialization code for test mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters test mode.
*/
void RA14Robot::TestInit() {
myCam->Disable();
myCompressor->Start();
Config::LoadFromFile("config.txt");
Config::Dump();
myCamera->Set(Relay::kForward); // turn on light
}
//
// Main Tele Operator Mode function. This function is called once, therefore a while loop that checks IsOperatorControl and IsEnabled is used
// to maintain control until the end of tele operator mode
//
void OperatorControl()
{
//myRobot.SetSafetyEnabled(true);
timer->Start();
Relay* reddlight = new Relay(4);
//Timer* lighttimer = new Timer();
//lighttimer->Start();
while (IsOperatorControl() && IsEnabled())
{
reddlight->Set(reddlight->kForward);
/*
if (lighttimer->Get()<=0.5) {
reddlight->Set(reddlight->kForward);
}
else if(lighttimer->Get()<1){
reddlight->Set(reddlight->kOff);
}
else {
lighttimer->Reset();
}
*/
//
// Get inputs
//
driverInput->GetInputs();
drive->GetInputs();
catapult->GetInputs();
feeder->GetInputs();
//
// Pass values between components as necessary
//
//catapult->SetSafeToFire(feeder->GetAngle()<95);
//
// Execute one step on each component
//
drive->ExecStep();
catapult->ExecStep();
feeder->ExecStep();
//
// Set Outputs on all components
//
catapult->SetOutputs();
feeder->SetOutputs();
//
// Wait for step timer to expire. This allows us to control the amount of time each step takes. Afterwards, restart the
// timer for the next loop
//
while (timer->Get()<(PERIOD_IN_SECONDS));
timer->Reset();
}
}
virtual void AutonomousInit() {
shooterAngle->Set(Relay::kForward);
rightDrive->SetSpeed(0);
leftDrive->SetSpeed(0);
shooterFWD->SetSpeed(1);
shooterRear->SetSpeed(1);
sleep(4);
cout << "BEGIN" << endl;
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(1);
//nanosleep(6);
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(1);
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(1);
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(2);
shooterFWD->SetSpeed(0);
shooterRear->SetSpeed(0);
sleep(2);
shooterAngle->Set(Relay::kReverse);
cout << "END" << endl;
}
void Test()
{
DriverStationLCD *screen = DriverStationLCD::GetInstance();
while (IsTest())
{
if ((buttonOne.Get()) == 1)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Not Pressed!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Pressed!");
}
if ((buttonTwo.Get()) == 1)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Not Pressed!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Pressed!");
}
if ((togglebuttonOne.Get()) == 0)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Is_On!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Is_Off!");
}
if ((togglebuttonTwo.Get()) == 0)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Is_On!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Is_Off!");
}
if (buttonOne.Get() == 0)
{
emergencyCompressor.Set(Relay::kOff);
}
#if 0
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"ButtonOneIs_%d", buttonOne.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"toggle_%d", buttonTwo.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Button_%d", togglebuttonOne.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"toggle_%d", togglebuttonTwo.Get());
#endif
Wait(0.005);
screen -> UpdateLCD();
}
}
void Disabled()
{
while(IsDisabled())
{
LEDLight->Set(Relay::kForward);
rpi->Read();
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "R: %i", rpi->GetMissingPacketcount());
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "x: %i", rpi->GetXPos());
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "y: %i", rpi->GetYPos());
lcd->UpdateLCD();
}
}
void TeleopInit() {
//Open logging
if(!fout.is_open()) {
fout.open("logging.csv");
logheaders();
}
cameraLight->Set(Relay::kOff);
myDrive->SetPID(KP,KI,KD);
myDrive->EnablePID();
manualAngle = 0;
compressor->Start();
myDrive->setGyroDrive(true);
lc->setMode(0);
freshStart = false;
}
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 AutonomousPeriodic() {
currentDistance = leftDriveEncoder->GetDistance();
shooterMotor->Set(-shooterMotorVolts);
if (currentDistance >= goalDistance){
drive->setLeft(0);
drive->setRight(0);
loaderMotor->Set(Relay::kForward);
} else {
drive->setLeft(.49);
drive->setRight(.5);
}
//log->info("LRD %f %f",
// leftDriveEncoder->GetDistance(),
// rightDriveEncoder->GetDistance());
//log->print();
}
virtual void TeleopPeriodic() {
rightDrive->SetSpeed(-(Driver->GetRawAxis(2)));
leftDrive->SetSpeed((Driver->GetRawAxis(5)));
shooterFWD->SetSpeed(-(Operator->GetRawAxis(2)));
shooterRear->SetSpeed(-(Operator->GetRawAxis(2)));
//shoioter angle
if(Operator->GetRawButton(5))
{
cout<<"Relay 1 forward"<<endl;
shooterAngle->Set(Relay::kForward);
}
if(Operator->GetRawButton(6))
{
cout<<"Relay 1 Reverse"<<endl;
shooterAngle->Set(Relay::kReverse);
}
//Fire button
if(Operator->GetRawButton(1))
{
cout<<"Relay 1 forward"<<endl;
shooterFire->Set(Relay::kForward);
}
if(Operator->GetRawButton(2))
{
cout<<"Relay 1 Reverse"<<endl;
shooterFire->Set(Relay::kReverse);
}
if(CompressorSwitch->Get() == 0){
CompressorRelay->Set(Relay::kForward);
}else{
CompressorRelay->Set(Relay::kOff);
}
//if(canPDP == 0){
// cout << "NULL" << endl;
//}else{
//canPDP->GetVoltage(Voltage) ;
//cout << "0" << endl;
//}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
// bool closed = true;
while (IsOperatorControl())
{
if (stick.GetRawButton(1))
{
spike.Set(Relay::kOn);
}
}
// if (stick.GetRawButton(2)){
// if (closed){
// door.Set(0);
// closed = false;
// }
// else{
// door.Set(1);
// closed = true;
// }
// }
}
inline void setDriveMotors()
{
//cannot turn belts if the limit switch is pressed
if(handlerState == BallHandlerState::up_off)
{
drive.Set(Relay::kOff);
spinnermotor.Set(0);
handlerposition.Set(Relay::kOff);
}
else if(handlerState == BallHandlerState::down_in_grab)
{
drive.Set(Relay::kReverse);
handlerposition.Set(Relay::kOff);
if(ballhandlerstick.GetRawButton(ARM_OUT_SPIN_BUTTON))
{
spinnermotor.Set(-0.75f);
}
else
{
spinnermotor.Set(0.75f);
}
}
else if(handlerState == BallHandlerState::down_out_shoot)
{
drive.Set(Relay::kForward);
spinnermotor.Set(-0.75f);
handlerposition.Set(Relay::kOff);
}
else if(handlerState == BallHandlerState::goingdown_off)
{
drive.Set(Relay::kReverse);
spinnermotor.Set(0);
handlerposition.Set(Relay::kReverse);
}
else if(handlerState == BallHandlerState::goingup_off)
{
drive.Set(Relay::kOff);
spinnermotor.Set(0);
handlerposition.Set(Relay::kForward);
}
else
{
spinnermotor.Set(0);
drive.Set(Relay::kOff);
handlerposition.Set(Relay::kOff);
}
}
void TeleopPeriodic()
{
//Drive
if(SmartDashboard::GetBoolean("DB/Button 0", false))
{
sc_left -> Set(-evan -> GetRawAxis(1));
sc_right -> Set(evan -> GetRawAxis(5));
}
else
{
sc_left -> Set(-evan->GetRawAxis(1) + .5*evan->GetRawAxis(0));
sc_right -> Set(evan->GetRawAxis(1) + .5*evan->GetRawAxis(0));
}
//Intake
if (i_limit->Get()) {
if(hunter->GetRawButton(1))
{
l_shoot->Set(-1);
r_shoot->Set(1);
if(count == 0)
{
timer1->Start();
count = 1;
}
}
else if(evan->GetRawAxis(2) > 0.2)
{
intake->Set(-evan->GetRawAxis(2));
}
else{
intake->Set(0);
}
}
else
{
if(evan->GetRawAxis(3) > 0.2)
{
intake->Set(.40*evan->GetRawAxis(3));
}
else if(evan->GetRawAxis(2) > 0.2)
{
intake->Set(-evan->GetRawAxis(2));
}
else {
intake->Set(0);
}
}
if(timer1->Get()>2){
intake->Set(1);
timer2->Start();
if(timer2->Get()>.5){
l_shoot->Set(0);
r_shoot->Set(0);
intake->Set(0);
timer1->Stop();
timer1->Reset();
timer2->Stop();
timer2->Reset();
count = 0;
}
}
//Fleshlight
fleshlight->Set(evan->GetRawButton(1) ? Relay::Value::kForward : Relay::Value::kOff);;
/*
//DPAD Stuff
switch (evan -> GetPOV())
{
case (0):
l_shoot -> Set(-.25*evan -> GetRawAxis(3));
r_shoot -> Set(.25*evan -> GetRawAxis(3));
break;
case (90):
l_shoot -> Set(-.5*evan -> GetRawAxis(3));
r_shoot -> Set(.5*evan -> GetRawAxis(3));
break;
case (180):
l_shoot -> Set(-.75*evan -> GetRawAxis(3));
r_shoot -> Set(.75*evan -> GetRawAxis(3));
break;
case (270):
l_shoot -> Set(evan-> GetRawAxis(3));
r_shoot ->Set(-evan->GetRawAxis(3));
break;
default:
l_shoot -> Set(-evan -> GetRawAxis(3));
r_shoot -> Set(evan -> GetRawAxis(3));
break;
}*/
//Hunter controls Arm
if ((hunter->GetRawAxis(5) < 0.1 and hunter->GetRawAxis(5) > -0.1) or (a_limit->Get() and hunter->GetRawAxis(5) > 0))
{
l_arm->Set(0);
r_arm->Set(0);
}
else
{
l_arm -> Set(-hunter-> GetRawAxis(5));
r_arm -> Set(hunter-> GetRawAxis(5));
}
//let evan control arm
/*if ((evan->GetRawAxis(5) < 0.1 and evan->GetRawAxis(5) > -0.1) or (a_limit->Get() and evan->GetRawAxis(5) > 0))
{
l_arm->Set(0);
r_arm->Set(0);
}
else
{
l_arm -> Set(evan-> GetRawAxis(5));
r_arm -> Set(-evan-> GetRawAxis(5));
}*/
//FORCEFEEDBACK
evan -> SetRumble(Joystick::RumbleType::kLeftRumble, hunter -> GetRawButton(2));
evan -> SetRumble(Joystick::RumbleType::kRightRumble, hunter -> GetRawButton(2));
//Message
SmartDashboard::PutNumber("DB/String 0", evan->GetPOV() );
//std::cout << evan->GetPOV() << std::endl;
//Wait
Wait(0.001);
}
void OperatorControl(void)
{
JankyTurret turret(7,11,10);
JankyTargeting targ(&turret);
JankyShooter shoot(SHOOTER_JAGUAR_CHANNEL,SHOOTER_ENCODER_A,SHOOTER_ENCODER_B);
int rpmForShooter=0;
while (IsOperatorControl())
{
//GetImage()
//hsl.Save
// myRobot.ArcadeDrive(stick);
shoot.GetCurrentRPM();
if (button.Get()==true)
{
LEDRelay.Set(Relay::kForward);
if (button_H.Get()==true)
{
targ.SetLMHTarget(BOGEY_H);
SmartDashboard::PutString("Targeting","High Button Pressed");
}
if (button_M.Get()==true)
{
targ.SetLMHTarget(BOGEY_M);
SmartDashboard::PutString("Targeting","Medium Button Pressed");
}
if (button_L.Get()==true)
{
targ.SetLMHTarget(BOGEY_L);
SmartDashboard::PutString("Targeting","Low Button Pressed");
}
if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true)
{
if (targ.ProcessOneImage())
{
targ.ChooseBogey();
targ.MoveTurret();
rpmForShooter = targ.GetCalculatedRPM();
shoot.setTargetRPM(rpmForShooter);
targ.InteractivePIDSetup();
}
}
targ.StopPID();
shoot.setTargetRPM(0);
}
else
{
float lazysusan =stick.GetZ();
turret.Set(lazysusan);
LEDRelay.Set(Relay::kOff);
// Set shooter speed to zero.
}
// float desired=abs(stick.GetY() *1000) + 100;
// smarty->PutInt("Desired RPM1", (int)desired);
// shoot.setTargetRPM((int)desired);
Wait(0.05);
}
// After teleop runs, save preferences again.
Preferences::GetInstance()->Save();
}
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()
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 Autonomous(void)
{
#if 1
/*int autoMode = 0;
autoMode |= bcd1->Get();
autoMode <<= 1;
autoMode |= bcd2->Get();
autoMode <<= 1;
autoMode |= bcd3->Get()
;*/
//double ignoreLineStart = 0;
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(0.2);
float liftSP = 0;
PIDLift->SetTolerance(10);
PIDLift->SetContinuous(false);
PIDLift->SetOutputRange(-0.75, 1); //BUGBUG
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
PIDLift->Enable();
PIDGrip->SetSetpoint(0);
PIDGrip->Enable();
stopCount = 0;
float reduction;
int counts = 0;
leftEncoder->Start();
rightEncoder->Start();
leftEncoder->Reset();
rightEncoder->Reset();
liftEncoder->Start();
liftEncoder->Reset();
leftEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
rightEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
double avgEncoderCount;
float leftSpeed = .2, rightSpeed = .2;
short int lsL,lsM,lsR;
lineFollowDone = false;
counts = 0;
//int fancyWaiter = 0;
int popstage = 0;
goPop = false;
double backStart = 0;
double backTime = 0;
double popStart = 0;
double popTime = 0;
bool backDone = false;
miniDep->Set(miniDep->kForward);
int liftCount = 0;
bool disengageBrake = false;
float lastLiftSP = 0;
gripOpen1->Set(true);
gripOpen2->Set(false);
gripLatch1->Set(true);
gripLatch2->Set(false);
while(IsAutonomous())
{
if(!(counts % 100))printf("%2.2f \n",getDistance());
if(backStart) backTime = GetClock();
if(popStart) popTime = GetClock();
//if(!ignoreLineStart)ignoreLineStart = GetClock();
if(!compSwitch->Get()) compressor->Set(compressor->kReverse);
else compressor->Set(compressor->kOff);
if(counts%3 == 0)
{
leftValue = lsLeft->Get();
middleValue = lsMiddle->Get();
rightValue = lsRight->Get();
}
counts++;
GetWatchdog().Feed();
//avgEncoderCount = (leftEncoder->Get() + rightEncoder->Get())/2;
//myRobot.SetLeftRightMotorOutputs(.2,.2);
//All three/five autonomous programs will do the same thing up until 87 inches from the wall
if (counts % 100 == 0){//TESTING
if(lsLeft->Get()){
lsL = 1;
}else{
lsL = 0;
}
if(lsRight->Get()){
lsR = 1;
}else{
lsR = 0;
}
if(lsMiddle->Get()){
lsM = 1;
}else{
lsM = 0;
}
//printf("Encoder: %2.2f \n", (float)avgEncoderCount);
//printf("Distance: %2.2f SensorL:%1d SensorM:%1d SensorR:%1d\n",getDistance(),lsL,lsM,lsR);//TESTING
}
#if FOLLOWLINE
/*if(GetClock() - ignoreLineStart <= 0.5)
{
frontLeftMotor->Set(-.4);
rearLeftMotor->Set(-.4);
frontRightMotor->Set(.4);
rearRightMotor->Set(.4);
}
else */if (false){//(avgEncoderCount <= SECONDBREAKDISTANCE){
followLine();
}
#else
if (getDistance() > 24){
frontLeftMotor->Set(-leftSpeed);
rearLeftMotor->Set(-leftSpeed);
frontRightMotor->Set(rightSpeed);
rearRightMotor->Set(rightSpeed);
if(leftEncoder->Get() > rightEncoder->Get() && leftSpeed == .2){
rightSpeed += .03;
}else if(leftEncoder->Get() >rightEncoder->Get() && leftSpeed > .2){
leftSpeed -= .03;
}else if(leftEncoder->Get() < rightEncoder->Get() && rightSpeed == .2){
leftSpeed += .03;
}else if(leftEncoder->Get() < rightEncoder->Get() && rightSpeed > .2){
rightSpeed -= .03;
}
}
#endif
else{
if(counts % 100 == 0) {printf("DISTANCE: %2.2f\n",getDistance());}
switch(FOLLOWLINE)
{
case STRAIGHTLINEMODE: //Straight line. Scores on middle column of left or right grid.
//if(lineFollowDone && !(counts %50)) printf("Lift error: %f \n", PIDLift->GetError());
lastLiftSP = liftSP;
if(!lineFollowDone)
{
followLine();
}
else if(!PIDLift->GetSetpoint() && !popstage && !backStart)
{
//if(counts % 50 == 0)printf("Go backward\n");
frontLeftMotor->Set(.3);
rearLeftMotor->Set(.3);
frontRightMotor->Set(-.3);
rearRightMotor->Set(-.3);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2 + 0.025;
//fancyWaiter = counts;
backStart = GetClock();
printf("Setpoint set setpoint set setpoint set \n");
/*
if(leftValue && middleValue && rightValue)
{
printf("Stopped 2nd time\n");
goPop = true;
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
}
#if 1 //if we've backed up for half a second and we're not popping
else if((backTime - backStart) > 0.65 && !backDone)
{
printf("Stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!ONE\n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
backDone = true;
//Wait(.01);
//lift->brakeOff();
//fancyWaiter = counts;
//printf("Fancy waiter set Fancy waiter set Fancy waiter set");
}
/*
else if(lift->getPosition() < LIFTHIGH2)
{
//Move teh lifts
//if(counts % 100 == 0) printf("Stopping because lineFollowDone == true\n");
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
//if the lift is at the top and we're done backing up
else if(PIDLift->GetSetpoint() && fabs(liftSP - lift->getPosition()) < 0.015 && backDone)
{
if(!popStart) popStart = GetClock();
if(popstage == 0)
{
//lift->brakeOn();
//PIDLift->SetSetpoint(lift->getPosition());
popstage++;
printf("BRAKE BRAKE BRAKE BRAKE BRAKE \n");
}
else if(popstage == 1 && popTime - popStart > 0.1)
{
#if !(INNERGRIP)
gripOpen1->Set(true);
gripOpen2->Set(false);
#else
gripOpen1->Set(false);
gripOpen2->Set(true);
#endif
popstage++;
printf("OPEN OPEN OPEN OPEN OPEN \n");
}
else if(popstage == 2 && popTime - popStart > 0.35)
{
gripPop1->Set(true);
gripPop2->Set(false);
popstage++;
printf("POP POP POP POP POP POP POP \n");
}
else if(popstage == 3 && popTime - popStart > 1.35)
{
gripPop1->Set(false);
gripPop2->Set(true);
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(-.2);
rearRightMotor->Set(-.2);
popstage++;
printf("UNPOP UNPOP UNPOP UNPOP UNPOP \n");
}
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(0);
liftSP = 0;
}
/*
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(min(0.3, 0.25*(popTime - popStart - 1.85)));
rearLeftMotor->Set(min(0.3, 0.25*(popTime - popStart - 1.85)));
frontRightMotor->Set(max(-0.3, -0.25*(popTime - popStart - 1.85)));
rearRightMotor->Set(max(-0.3, -0.25*(popTime - popStart - 1.85)));
//PIDLift->SetSetpoint(0);
liftSP = 0;
popstage++;
}
else if(popstage == 5 && popTime - popStart > 4.85)
{
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
}
*/
}
//Start tele-op lift code
if(!lift->isBraking() && !disengageBrake)
{
PIDLift->SetSetpoint(liftSP);
if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1)
{
//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1);
}
else PIDLift->SetOutputRange(-0.75, 1);
}
if(lift->isBraking() && lastLiftSP != liftSP)
{
PIDLift->SetSetpoint(lift->getPosition() + 0.04);
PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
lift->brakeOff();
disengageBrake = true;
if(!liftCount) liftCount = counts;
}
//set brake (because near)
if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
{
if(liftCount == 0) liftCount = counts;
if(counts - liftCount > 1000)
{
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(counts%3000) printf("Braking/Not PID \n");
}
if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
{
disengageBrake = false;
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
liftCount = 0;
}
//End tele-op lift code
#endif
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
case NOLINEMODE: //Fork turn left. Scores on far right column of left grid.
lineFollowDone = true;
if(!lineFollowDone){}
else if(!PIDLift->GetSetpoint() && !popstage && !backStart)
{
//if(counts % 50 == 0)printf("Go backward\n");
frontLeftMotor->Set(.3);
rearLeftMotor->Set(.3);
frontRightMotor->Set(-.3);
rearRightMotor->Set(-.3);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
//fancyWaiter = counts;
backStart = GetClock();
printf("Setpoint set setpoint set setpoint set \n");
/*
if(leftValue && middleValue && rightValue)
{
printf("Stopped 2nd time\n");
goPop = true;
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
}
#if 1 //if we've backed up for half a second and we're not popping
else if((backTime - backStart) > 0.65 && !backDone)
{
printf("Stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!ONE\n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
backDone = true;
//Wait(.01);
//lift->brakeOff();
//fancyWaiter = counts;
//printf("Fancy waiter set Fancy waiter set Fancy waiter set");
}
/*
else if(lift->getPosition() < LIFTHIGH2)
{
//Move teh lifts
//if(counts % 100 == 0) printf("Stopping because lineFollowDone == true\n");
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
//if the lift is at the top and we're done backing up
else if(PIDLift->GetSetpoint() && fabs(liftSP - lift->getPosition()) < 0.015 && backDone)
{
if(!popStart) popStart = GetClock();
if(popstage == 0)
{
//lift->brakeOn();
//PIDLift->SetSetpoint(lift->getPosition());
popstage++;
printf("BRAKE BRAKE BRAKE BRAKE BRAKE \n");
}
else if(popstage == 1 && popTime - popStart > 0.1)
{
#if !(INNERGRIP)
gripOpen1->Set(true);
gripOpen2->Set(false);
#else
gripOpen1->Set(false);
gripOpen2->Set(true);
#endif
popstage++;
printf("OPEN OPEN OPEN OPEN OPEN \n");
}
else if(popstage == 2 && popTime - popStart > 0.35)
{
gripPop1->Set(true);
gripPop2->Set(false);
popstage++;
printf("POP POP POP POP POP POP POP \n");
}
else if(popstage == 3 && popTime - popStart > 1.35)
{
gripPop1->Set(false);
gripPop2->Set(true);
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(-.2);
rearRightMotor->Set(-.2);
popstage++;
printf("UNPOP UNPOP UNPOP UNPOP UNPOP \n");
}
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(0);
liftSP = 0;
}
}
//Start tele-op lift code
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(lift->getPosition() + 0.04);
PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
lift->brakeOff();
disengageBrake = true;
if(!liftCount) liftCount = counts;
}
//set brake (because near)
if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
{
if(liftCount == 0) liftCount = counts;
if(counts - liftCount > 1000)
{
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(counts%3000) printf("Braking/Not PID \n");
}
if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
{
disengageBrake = false;
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
liftCount = 0;
}
//End tele-op lift code
#endif
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
case FORKRIGHTMODE://Fork turn right. Scores on far left column of right grid.
if(leftEncoder->GetDistance() <= rightEncoder->GetDistance() + 6)
{
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
}
else if(getDistance() >= SCOREDISTANCE)
{
followLine();
}
//score here
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
}
}
}
/*frontRightMotor->Set(0);
rearRightMotor->Set(0);
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);*/
Wait(.02);
#endif
}
void Reset() { m_relay->Set(Relay::kOff); }
void TeleopPeriodic() {
displayCount++;
//tbs change button number
if ((control->gamepadButton(9) && control->gamepadButton(10)) || // start
climbState != NotInitialized) { // continue
// Do we want a manual abort here?
ClimbPeriodic();
return;
}
// drive
drive->setLeft(control->left());
// control->setRightScale(.95);
drive->setRight(control->right());
drive->setScale(control->throttle());
//drive->setLowShift(control->button(1)); // right trigger
drive->setReversed(control->toggleButton(11)); // right JS button 11
//turn light on or off
//lightRing->Set(control->gamepadToggleButton(4) ? Relay::kForward : Relay::kOff );
blowerMotor->Set(control->gamepadButton(6) ? 1.0 : 0.0 );
// For the loader, if we are rotating, wait for at least 100 counts before
// checking the switch or adjusting motor power
if (loading) {
//if (loaderSwitch->Get()) {
//loaderDisengageDetected = true;
//}
loadSwitchDelay++;
// If already rotating, and the switch trips, power down the motor
if (/*loaderDisengageDetected*/ loaderSwitch->Get() != loadSwitchOldState) {
loaderMotor->Set(Relay::kOff);
loading = false;
loadSwitchOldState = loaderSwitch->Get();
}
} else {
// If not rotating and the gamepad button is set, start rotating
if (control->gamepadButton(7)) {
loadSwitchDelay = 0;
loadCount++;
loaderMotor->Set(Relay::kForward);
loading = true;
loaderDisengageDetected = false;
}
}
if (control->gamepadToggleButton(4)){
if (control->gamepadRightVertical() > 0.05 ||
control->gamepadRightVertical() < -0.05) {
shooterMotorVolts += control->gamepadRightVertical();
if (shooterMotorVolts < 6.0)
shooterMotorVolts = 6.0;
if (shooterMotorVolts > 12.0)
shooterMotorVolts = 12.0;
}
}
// For the shooter, spin it up or down based on the toggle
//
if (control->gamepadToggleButton(8)) {
shooterMotor->Set(-shooterMotorVolts);// negative because motor is wired backwerds
log->info("shooter on");
} else {
shooterMotor->Set(0.0);
log->info("shooter off");
}
if (control->gamepadLeftVertical() > 0.05 ||
control->gamepadLeftVertical() < -0.05) {
cameraElevateAngle += control->gamepadLeftVertical()*5;
if (cameraElevateAngle < cameraElevateMotor->GetMinAngle())
cameraElevateAngle = cameraElevateMotor->GetMinAngle();
if (cameraElevateAngle > cameraElevateMotor->GetMaxAngle())
cameraElevateAngle = cameraElevateMotor->GetMaxAngle();
}
if (control->gamepadLeftHorizontal() > 0.05 ||
control->gamepadLeftHorizontal() < -0.05) {
cameraPivotAngle += control->gamepadLeftHorizontal()*5;
if (cameraPivotAngle < cameraPivotMotor->GetMinAngle())
cameraPivotAngle = cameraPivotMotor->GetMinAngle();
if (cameraPivotAngle > cameraPivotMotor->GetMaxAngle())
cameraPivotAngle = cameraPivotMotor->GetMaxAngle();
}
cameraPivotMotor->SetAngle(cameraPivotAngle);
cameraElevateMotor->SetAngle(cameraElevateAngle);
if (control->gamepadToggleButton(1)) {
// If the climber lower limit switch is set, and the distance is >0, set it to 0
if (!leftClimber->lowerLimitSwitch->Get()) {
leftClimber->encoder->Reset();
leftClimber->encoder->Start();
// Only allow forward motion
if (control->gamepadRightVertical() > 0) {
leftClimber->motor->Set(control->gamepadRightVertical());
} else {
leftClimber->motor->Set(0.0);
}
} else {
leftClimber->motor->Set(control->gamepadRightVertical());
}
}
if (control->gamepadToggleButton(3)) {
// If the climber lower limit switch is set, and the distance is >0, set it to 0
if (!rightClimber->lowerLimitSwitch->Get()) {
rightClimber->encoder->Reset();
rightClimber->encoder->Start();
// Only allow forward motion
if (control->gamepadRightVertical() > 0) {
rightClimber->motor->Set(control->gamepadRightVertical());
} else {
rightClimber->motor->Set(0.0);
}
} else {
rightClimber->motor->Set(control->gamepadRightVertical());
}
}
// assorted debug
//log->info("Shift %s", control->toggleButton(8)
// ? "low" : "high");
//log->info("ArmPot: %.0f", arm->encoderValue());
//log->info("pidf: %.2f", arm->pidFactor());
//log->info("piden: %s", arm->isPidEnabled() ? "true" : "false");
//double myTest = drive->rightPosition();
//log->info("renc: %f", myTest);
//myTest = drive->leftPosition();
//log->info("lenc: %f", myTest);
//log->info("gplh %f %f", control->gamepadLeftHorizontal(), cameraPivotAngle);
//log->info("gplv %f %f", control->gamepadLeftVertical(), cameraElevateAngle);
//log->info("gprh %f", control->gamepadRightHorizontal());
//log->info("gprv %f", control->gamepadRightVertical());
if (display()) {
//log->info("lg %d ldd %d", loading, loaderDisengageDetected);
//log->info("BCD: %d", bcd->value());
log->info("SHV: %f", shooterMotorVolts);
//log->info("LdCDS: %d %d %d", loadCount, loaderDisengageDetected, loaderSwitch->Get());
log->info("ena: %c%c%c%c",
" L"[control->gamepadToggleButton(1)],
" R"[control->gamepadToggleButton(3)],
" J"[control->gamepadToggleButton(2)],
" S"[control->gamepadToggleButton(4)]);
log->info("LRC %f %f",
leftClimber->encoder->GetDistance(),
rightClimber->encoder->GetDistance());
log->info("LRD %f %f",
leftDriveEncoder->GetDistance(),
rightDriveEncoder->GetDistance());
log->info("LRL %d %d %d %d %d %d %d",
leftClimber->lowerLimitSwitch->Get(),
leftClimber->lowerHookSwitch->Get(),
leftClimber->upperHookSwitch->Get(),
rightClimber->lowerLimitSwitch->Get(),
rightClimber->lowerHookSwitch->Get(),
rightClimber->upperHookSwitch->Get(),
loaderSwitch->Get());
log->print();
}
}
/****** AUTO FUNCTIONS END *******/
void Autonomous()
{
int counter=0;
int autonomousEngagement = 0;
DriverStationLCD *screen = DriverStationLCD::GetInstance();
compressor.Start(); //starts compressor class
rightArmSolenoid.Set(DoubleSolenoid::kReverse); //brings the arms down
leftArmSolenoid.Set(DoubleSolenoid::kReverse);
/*** ENSURES THE CATAPULT IS LOADED AND LOADS IF UNLOADED ***/
if (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1)
{
winchMotor.Set(0.1); // Gears need to be moving slowly to allow the dog gear to engage properly
dogSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the dog gear
Wait(0.2); // Giving the pistons time to engage properly
winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the ratchet
Wait(0.2); // Giving the pistons time to engage properly
}
while (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1) // If Limit Switch Buttons are not pressed
{
winchMotor.Set(1); //Now starts the winch motor to load the catapult
}
// If the Catapult Left & Limit Switches are (0,0), (0,1), (1,0)
{
winchMotor.Set(0); // Stops the Winch Motor since one or more buttons are pressed
if ((dogSolenoid.Get() == DoubleSolenoid::kReverse) && (ratchetSolenoid.Get() == DoubleSolenoid::kForward)) // If the Dog Gear is disengaged but the ratchet is engaged
{
winchMotor.Set(0.05); // Gears need to be moving slowly to allow the dog gear to engage properly. Might want to test this since the catapult's already loaded.
dogSolenoid.Set(DoubleSolenoid::kForward); // Engages the dog gear so both dog gear and ratchet are engaged before shooting for safety
Wait(0.1); // Giving the pistons time to engage properly
winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move
}
else if ((dogSolenoid.Get() == DoubleSolenoid::kForward) && (ratchetSolenoid.Get() == DoubleSolenoid::kReverse)) // If the dog gear is engaged but the ratchet is disengaged
{
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Engages the ratchet so that both dog gear and ratchet are engaged before shooting for safety
Wait(0.1); // Giving the pistons time to engage properly
}
}
/*** DONE LOADING THE CATAPULT ***/
float pLower = 5; // min height of rectangle for comparison
float pUpper = 15; // max height of rectangle for comparison
int criteriaCount = 1; // number of elements to include/exclude at a time
int rejectMatches = 1; // when set to true, particles that do not meet the criteria are discarded
int connectivity = 1; // declares connectivity value as 1; so corners are not ignored
int filterFunction; // removes small blobs
int borderSetting; // variable to store border settings, limit for rectangle
int borderSize = 1; // border for the camera frame (if you don't put this, DriverStation gets mad at you)
ParticleFilterCriteria2 particleCriteria;
ParticleFilterOptions2 particleFilterOptions;
int numParticles;
particleCriteria.parameter = IMAQ_MT_BOUNDING_RECT_HEIGHT; //The Morphological measurement we use
particleCriteria.lower = pLower; // The lower bound of the criteria range
particleCriteria.upper = pUpper; // The upper bound of the criteria range
particleCriteria.calibrated = FALSE; // We aren't calibrating to real world measurements. We don't need this.
particleCriteria.exclude = TRUE; // Remove all particles that aren't in specific pLower and pUpper range
particleFilterOptions.rejectMatches = rejectMatches; // Set to 1 above, so images that do not meet the criteria are discarded
particleFilterOptions.rejectBorder = 0; // Set to 0 over here so border images are not discarded
particleFilterOptions.connectivity8 = connectivity; // Sets the image image to 8 bit
while ((IsAutonomous()))
{
if (logitech.GetRawButton(4))
{
autonomousEngagement = 1;
}
if (autonomousEngagement == 0) // If real autonomous is not engaged start
{
if (logitech.GetRawButton(1))
{
driveForward();
}
if (logitech.GetRawButton(9))
{
dogSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm
winchMotor.Set(0.1);
Wait(0.3);
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to lower the retrieval arm
while(leftLimitSwitch.Get()==1 && rightLimitSwitch.Get()==1)
{
winchMotor.Set(1);
}
}
if (logitech.GetRawButton(2))
{
autonomousCatapultRelease();
}
if (logitech.GetRawButton(3))
{
stopDriving();
}
if (logitech.GetRawButton(5))
{
turnLeft();
}
if (logitech.GetRawButton(7))
{
turnLeftMore();
}
if (logitech.GetRawButton(6))
{
turnRight();
}
if (logitech.GetRawButton(8))
{
turnRightMore();
}
}// If real autonomous is not engaged end
HSLImage* imgpointer; // declares an image container as an HSL (hue-saturation-luminence) image
imgpointer = camera.GetImage(); //tells camera to capture image
ringLight.Set(Relay::kForward); //turns ringlight on
BinaryImage* binIMG = NULL; // declares a container to hold a binary image
binIMG = imgpointer -> ThresholdHSL(0, 255, 0, 255, 235, 255); // thresholds HSL image and places in the binary image container
delete imgpointer; // deletes the HSL image to free up memory on the cRIO
Image* modifiedImage = imaqCreateImage(IMAQ_IMAGE_U8, 0); //create a binary 8-bit format shell for the image
filterFunction = imaqParticleFilter4(modifiedImage, binIMG -> GetImaqImage(), &particleCriteria, criteriaCount, &particleFilterOptions, NULL, &numParticles); //The Particle Filter Function we use. (The ones before it are outdated)
borderSetting = imaqSetBorderSize(modifiedImage, borderSize); // Sets a border size so DriverStation is happy
delete binIMG; //Deletes the Binary image
int functionCountParticles; // stores number of particles
int particleAmount; // stores the number of particles for the measure particle function
functionCountParticles = imaqCountParticles(modifiedImage, TRUE, &particleAmount); // Counts the number of particles
int functionOne; // The first measuring particle function (specifically for particle #1)
int functionTwo; // The second measuring particle function (specifically for particle #2)
double particleOneOrientation; // TRULY ARBITRARY name of the first particle it find
double particleTwoOrientation; // TRULY ARBITRARY name of the second particle it finds
functionOne = imaqMeasureParticle(modifiedImage, 0, FALSE, IMAQ_MT_ORIENTATION, &particleOneOrientation); // Measures orientation of particle 1
functionTwo = imaqMeasureParticle(modifiedImage, 1, FALSE, IMAQ_MT_ORIENTATION, &particleTwoOrientation); // Measures orientation of particle 2
screen->PrintfLine(DriverStationLCD::kUser_Line2,"P1: %f", particleOneOrientation); // Prints particle 1's orientation
screen->PrintfLine(DriverStationLCD::kUser_Line3,"P2: %f", particleTwoOrientation); // Prints particle 2's orientation
imaqDispose(modifiedImage); // Deletes the filtered image
/**LEFT POSITION**/
if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 0)) // Left switch set on, switch set off
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Left Position:F"); // Left position and facing forward
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10))
// The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Hot!");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
//driveForward();
Wait(3);
stopDriving();
//autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
driveForward();
Wait(4);
stopDriving();
//autonomousCatapultRelease();
}
}
/**CENTER POSITION**/
else if ((leftPositionSwitch.Get() == 0) && (rightPositionSwitch.Get() == 0)) // Left switch off and right switch off
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
/** RIGHT POSITION**/
else if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
else if (((leftPositionSwitch.Get()) == 1) && ((rightPositionSwitch.Get()) == 0)) // Left switch off and switch on
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Right Position"); // position and facing forward
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || ((particleTwoOrientation > 0) && (particleTwoOrientation < 10))) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeft();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4, "Right Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
counter++;
screen -> PrintfLine(DriverStationLCD::kUser_Line5,"R: %f L: %f)", rightFront.Get(), leftFront.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Counter %d", counter);
screen->UpdateLCD();
}
compressor.Stop();
}
void Test() // DONT TOUCH THIS AREA. I KEEL YOU.
{
DriverStationLCD *screen = DriverStationLCD::GetInstance();
int counter = 0;
bool solenoidTest=0;
while (IsTest())
{
if(logitech.GetRawButton(9)) //press rightBack
{
solenoidTest=1;
compressor.Start();
}
if(logitech.GetRawButton(10)) //press Start
{
solenoidTest=0;
compressor.Stop();
}
if(solenoidTest)
{
if(logitech.GetRawButton(1)) //press X
{
rightArmSolenoid.Set(DoubleSolenoid::kForward);
leftArmSolenoid.Set(DoubleSolenoid::kForward);
}
else if(logitech.GetRawButton(2)) //press A
{
rightArmSolenoid.Set(DoubleSolenoid::kReverse);
leftArmSolenoid.Set(DoubleSolenoid::kReverse);
}
else
{
leftArmSolenoid.Set(DoubleSolenoid::kOff);
rightArmSolenoid.Set(DoubleSolenoid::kOff);
}
if(logitech.GetRawButton(3)) //PRess URTrigger
{
retrievalMotor.Set(logitech.GetRawAxis(2));
}
else
{
retrievalMotor.Set(0);
}
if(logitech.GetRawButton(4))
{
winchMotor.Set(logitech.GetRawAxis(2));
}
else
{
winchMotor.Set(0);
}
if(logitech.GetRawButton(5))
{
ratchetSolenoid.Set(DoubleSolenoid::kForward);
}
else if(logitech.GetRawButton(7))
{
ratchetSolenoid.Set(DoubleSolenoid::kReverse);
}
else
{
ratchetSolenoid.Set(DoubleSolenoid::kOff);
}
if(logitech.GetRawButton(6))
{
dogSolenoid.Set(DoubleSolenoid::kForward);
}
else if(logitech.GetRawButton(8))
{
dogSolenoid.Set(DoubleSolenoid::kReverse);
}
else
{
dogSolenoid.Set(DoubleSolenoid::kOff);
}
}
else
{
if(logitech.GetRawButton(1)) //Press X
{
rightFront.Set(logitech.GetRawAxis(2)); //Press left joystick
}
else
{
rightFront.Set(0);
}
if(logitech.GetRawButton(2)) //Press A
{
rightBack.Set(logitech.GetRawAxis(2));
}
else
{
rightBack.Set(0);
}
if(logitech.GetRawButton(3)) //Press B
{
leftFront.Set(logitech.GetRawAxis(2));
}
else
{
leftFront.Set(0);
}
if(logitech.GetRawButton(4)) //Press Y
{
leftBack.Set(logitech.GetRawAxis(2));
}
else
{
leftBack.Set(0);
}
if(logitech.GetRawButton(5)) //Press ULTrigger
{
retrievalMotor.Set(logitech.GetRawAxis(2));
}
else
{
retrievalMotor.Set(0);
}
if(logitech.GetRawButton(6)) //PRess URTrigger
{
winchMotor.Set(logitech.GetRawAxis(2));
}
else
{
winchMotor.Set(0);
}
if(logitech.GetRawButton(7)) //Press LLTrigger
{
ringLight.Set(Relay::kForward);
}
else
{
ringLight.Set(Relay::kOff);
}
if(logitech.GetRawButton(8)) //Press LRTrigger
{
compressor.Start();
}
else
{
compressor.Stop();
}
}
/****** MANUAL LOAD FUNCTION END *****/
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"LeftJoystick: %f", logitech.GetRawAxis(2));
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"RF:%f RB:%f LF:%f LB:%f", rightFront.Get(), rightBack.Get(), leftFront.Get(), leftBack.Get()); // Print WinchMotor State
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Solenoid Testing:%d", solenoidTest);
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"rightArmSolenoid:%d", rightArmSolenoid.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line5,"time:%d", counter);
counter ++;
Wait(0.005); // Waits to run the loop every 0.005 seconds so the cRIO doesn't explode
screen->UpdateLCD();
}
}
RobotDemo(void)
{
///constructs
game = false;//set to true or false before use- true for auton & tele-op, false for just 1
cd = new CustomDrive(NUM_JAGS);
closed = new Solenoid(8, 1);//true if closed
open = new Solenoid(8, 2);
stick = new Joystick(2);// arm controll
controller = new Joystick(1);// base
manEnc = new Encoder(4,5);
reset = false;//if button
manJag = new Jaguar(6);//cons manip Jag
ShoulderJag = new Jaguar(5);
minibot = new DoubleSolenoid(3, 4);cmp = new Compressor(DOC7_RELAY_PORT, DOC7_COMPRESSOR_PORT);
track = new Tracking(DOC7_LEFT_LINE_PORT, DOC7_CENTER_LINE_PORT, DOC7_RIGHT_LINE_PORT);
red_white = new Relay(DOC7_RED_WHITE_SPIKE);
red_white->SetDirection(Relay::kBothDirections);
red_white->Set(Relay::kOff);
blue = new Relay(DOC7_BLUE_SPIKE);
blue->SetDirection(Relay::kBothDirections);
blue->Set(Relay::kOff);
GetWatchdog().Kill();
};
