void TestBGrabber()
{
//ROLLERS
if (m_operator->GetRawAxis(TRIGGERS) > 0.4) {
m_roller->Set(0.5);
}
else if (m_operator->GetRawAxis(TRIGGERS) < -0.4)
m_roller->Set(0.5);
else {
m_roller->Set(0.0);
}
//BALL CATCH (#Sweg)
if (m_operator->GetRawButton(BUTTON_A)) {
m_catch->Set(true);
}
else if (m_operator->GetRawButton(BUTTON_B)) {
m_catch->Set(false);
}
//bArm OPEN / CLOSE
if (m_operator->GetRawButton(BUTTON_X)) {
m_bArm->Set(true);
}
else if (m_operator->GetRawButton(BUTTON_Y)) {
m_bArm->Set(false);
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
GetWatchdog().SetEnabled(true);
compressor->Start();
GetWatchdog().SetExpiration(0.5);
bool valve_state = false;
while (IsOperatorControl())
{
motor->Set(stick->GetY());
if (stick->GetRawButton(1) && !valve_state)
{
valve->Set(true);
valve_state = true;
}
if (!stick->GetRawButton(1) && valve_state)
{
valve->Set(false);
valve_state = false;
}
// Update driver station
//dds->sendIOPortData(valve);
GetWatchdog().Feed();
}
}
/**
* Read the current value of the solenoid.
*
* @param channel The channel in the Solenoid module
* @return The current value of the solenoid.
*/
bool GetSolenoid(UINT32 channel)
{
Solenoid *s = allocateSolenoid(channel);
if (s == NULL)
return false;
return s->Get();
}
void ShiftLow(void)
{
// shiftRight->Get() : false is low gear, true is high gear
if(shiftRight->Get()) {
shiftRight->Set(false);
shiftLeft->Set(false);
}
}
void RobotInit(void) {
// Actions which would be performed once (and only once) upon initialization of the
// robot would be put here.
shiftLeft->Set(false); //low gear
shiftRight->Set(false); //low gear
passingPiston->Set(false); //retracted
printf("RobotInit() completed.\n");
}
void RawControl::deployMini()
{
if(chkSwitch())
{
deploy->Set(true);
}
else
deploy->Set(false);
}
void ShiftHigh(void)
{
// shiftRight->Get() : false is low gear, true is high gear
if(!(shiftRight->Get()))
{
shiftRight->Set(true);
shiftLeft->Set(true);
}
}
void RobotDemo::DriverLCD()
{
if (cycle_counter >= 50)
{
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "RPS Back:%f ",
RPS_back);
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "RPS Front:%f ",
RPS_front);
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "RPS DRPS:%f ",
desired_RPS_control);
#if 0
if (shooter_fire_piston_A->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"Fire ");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"Retracting... ");
}
#endif
//dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"TopLS:%i BotLS:%i ", top_claw_limit_switch->Get(),
// bottom_claw_limit_switch ->Get());
if (top_claw_limit_switch->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "!TOP");
}
else if (!bottom_claw_limit_switch->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "!BOTTOM");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Neither");
}
if (shooter_angle_1->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Up ");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Down ");
}
if (arcadedrive)
{
dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Arcade ");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Tank ");
}
dsLCD->UpdateLCD();
//cycle_counter = 0;
}
//cycle_counter++;
}
void OperatorControl()
{
compressor.Start();
while (IsOperatorControl())
{
if(stick.GetRawButton(6)) //press the upper right trigger
{
piston.Set(true);
}
else if(stick.GetRawButton(8)) //press the lower right trigger
{
piston.Set(false);
}
}
compressor.Stop();
}
void TeleopInit() override {
drive->SetExpiration(200000);
drive->SetSafetyEnabled(false);
liftdown->Set(false);
intake_hold = false;
lastLiftPos = 0;
manual = true;
}
/**
* 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 TestInit()
{
printf(">>> TestInit\n");
#ifdef HAVE_COMPRESSOR
compressor->Start();
#endif
#ifdef HAVE_ARM
arm->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_INJECTOR
injectorL->Set(DoubleSolenoid::kOff);
injectorR->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_EJECTOR
ejector->Set(false);
#endif
#ifdef HAVE_LEGS
legs->Set(false);
#endif
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
jagVbus(topWheel1, 0.0);
#endif
#ifdef HAVE_TOP_PWM1
topWheel1->Set(0.0);
#endif
#ifdef HAVE_TOP_CAN2
jagVbus(topWheel2, 0.0);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
jagVbus(bottomWheel1, 0.0);
#endif
#ifdef HAVE_BOTTOM_PWM1
bottomWheel1->Set(0.0);
#endif
#ifdef HAVE_BOTTOM_CAN2
jagVbus(bottomWheel2, 0.0);
#endif
#endif
printf("<<< TestInit\n");
}
// Start auto mode
void AutonomousInit() override {
autoSelected = *((int*) chooser.GetSelected()); // autonomous mode chosen in dashboard
currentState = 1;
ahrs->ZeroYaw();
ahrs->GetFusedHeading();
autoLength = SmartDashboard::GetNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
autoSpeed = SmartDashboard::GetNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
autoIntakeSpeed = SmartDashboard::GetNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
liftdown->Set(false);
}
void Shoot (bool teleop)
{
// code to differentiate teleop from autonomous
/*if (teleop == true)
shootyesorno = controller.GetRawButton(BTN_SHOOT);
else if (teleop == false)
shootyesorno = true;*/
if (AcceptableToFire() == true && firefrisbee.Get() == true)
{
firefrisbee.Set(false); // SHOOT THE DARN FRISBEE
shottime.Stop();
shottime.Reset();
shottime.Start();
}
if (shootyesorno == true && firefrisbee.Get() == false && AcceptableToFire() == true && ShootMode == eShoot)
{
firefrisbee.Set(true); // Primes that there frisbee
shottime.Stop();
shottime.Reset();
shottime.Start();
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
log->Info("TELEOP START");
// myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
double distance = dist.GetVoltage() / (5.0 / 512.0);
SmartDashboard::Log(distance, "Rangefinder distance");
SmartDashboard::Log(dist.GetVoltage(), "Rangefinder voltage");
Sol1.Set(stick1.GetRawButton(1));
Sol2.Set(stick1.GetRawButton(2));
Sol3.Set(stick1.GetRawButton(3));
Sol4.Set(stick1.GetRawButton(4));
Sol5.Set(stick1.GetRawButton(5));
if (stick1.GetRawButton(6)) {
if (!lastButton) log->Shot(stick1.GetZ(), stick2.GetZ());
lastButton = true;
} else {
lastButton = false;
}
SmartDashboard::Log(((stick1.GetZ() + 1) / 2) * 2500, "Distance");
SmartDashboard::Log(((stick2.GetZ() + 1) / 2) * 5.0, "Compression");
SmartDashboard::Log(LookUp(stick1.GetZ() * 2500, /* stick2.GetZ() * 5.0 */ dist.GetVoltage()), "Shooter Speed");
// myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
// lJagA.Set(stick1.GetY());
// lJagB.Set(stick1.GetY());
// rJagA.Set(stick2.GetY());
// rJagB.Set(stick2.GetY());
Wait(0.010); // wait for a motor update time
}
}
void TeleopPeriodic(void)
{
bool flag = true; //flag object initial declaration to ensure passing Piston toggle works properly
float leftStick = gamePad->GetRawAxis(4);
float rightStick = gamePad->GetRawAxis(2);
bool rightBumper = gamePad->GetRawButton(6);
bool leftBumper = gamePad->GetRawButton(5);
bool buttonA = gamePad->GetRawButton(2);
if(fabs(leftStick) >= 0.05 || fabs(rightStick >= 0.05))
{
m_robotDrive->TankDrive(leftStick, rightStick);
}
else if(rightBumper || leftBumper)
{
if(rightBumper && !leftBumper)
{
ShiftHigh();
}
else if(leftBumper && !rightBumper)
{
ShiftLow();
}
}
else if(buttonA && flag)
{
flag = false;
passingPiston->Set(true);
}
else
{
if(!buttonA)
{
flag = false;
MotorControlLeft(0.0);
MotorControlRight(0.0);
}
else
{
MotorControlLeft(0.0);
MotorControlRight(0.0);
}
}
}
void RobotInit() override {
lw = LiveWindow::GetInstance();
drive->SetExpiration(20000);
drive->SetSafetyEnabled(false);
//Gyroscope stuff
try {
/* Communicate w/navX-MXP via the MXP SPI Bus. */
/* Alternatively: I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
ahrs = new AHRS(SPI::Port::kMXP);
} catch (std::exception ex) {
std::string err_string = "Error instantiating navX-MXP: ";
err_string += ex.what();
//DriverStation::ReportError(err_string.c_str());
}
if (ahrs) {
LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
ahrs->ZeroYaw();
// Kp Ki Kd Kf PIDSource PIDoutput
turnController = new PIDController(0.015f, 0.003f, 0.100f, 0.00f,
ahrs, this);
turnController->SetInputRange(-180.0f, 180.0f);
turnController->SetOutputRange(-1.0, 1.0);
turnController->SetAbsoluteTolerance(2); //tolerance in degrees
turnController->SetContinuous(true);
}
chooser.AddDefault("No Auto", new int(0));
chooser.AddObject("GyroTest Auto", new int(1));
chooser.AddObject("Spy Auto", new int(2));
chooser.AddObject("Low Bar Auto", new int(3));
chooser.AddObject("Straight Spy Auto", new int(4));
chooser.AddObject("Adjustable Straight Auto", new int(5));
SmartDashboard::PutNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
SmartDashboard::PutNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
SmartDashboard::PutNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
SmartDashboard::PutData("Auto Modes", &chooser);
liftdown->Set(false);
comp->Start();
}
/**
* Initialization code for teleop mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters teleop mode.
*/
void TeleopInit()
{
printf(">>> TeleopInit\n");
#ifdef HAVE_COMPRESSOR
compressor->Start();
#endif
#ifdef HAVE_ARM
arm->Set(DoubleSolenoid::kForward);
#endif
#ifdef HAVE_INJECTOR
injectorL->Set(DoubleSolenoid::kReverse);
injectorR->Set(DoubleSolenoid::kReverse);
#endif
#ifdef HAVE_EJECTOR
ejector->Set(false);
#endif
#ifdef HAVE_LEGS
// legs->Set(true);
#endif
// StartWheels();
printf("<<< TeleopInit\n");
}
void RobotDemo::printfs()
{
//printf("OUT LOOP\n");
if (cycle_counter >= 70)
{
printf("RPS Back:%f RPS Front:%f %f ", RPS_back, RPS_front,
desired_RPS_control);
printf("LS:%i %i ", top_claw_limit_switch->Get(),
bottom_claw_limit_switch->Get());
//printf("Climb:%f " , climbing_motor->Get());
//printf("Shooters:%f %f ", shooter_motor_front ->Get(),
//shooter_motor_back ->Get());
if (shooter_angle_1->Get())
{
printf("Up ");
}
else
{
printf("Down ");
}
if (arcadedrive)
{
printf("Arcade ");
}
else
{
printf("Tank ");
}
printf("\n");
cycle_counter = 0;
}
cycle_counter++;
}
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
}
static nr::diag::handler_t
solenoid_diag_handler( const Solenoid& solenoid )
{ return solenoid.Get() ? "True" : "False"; }
void OperatorControl(void)
{
AxisCamera &camera = AxisCamera::GetInstance();
miniBotTime.Start();
initRobot();
debug("in telop");
compressor.Start();
GetWatchdog().SetEnabled(true);
/*int l1, l2, l3;
while (IsOperatorControl()) {
GetWatchdog().Feed();
//char val = (line1.Get() & 0x01) | (line2.Get() & 0x02) | (line3.Get() & 0x04);
//if(l1 != line1.Get() || l2 != line2.Get() || l3 != line3.Get()) {
// cerr << "change " << (l1 = line1.Get()) << "\t" << (l2 = line2.Get()) << "\t" << (l3 = line3.Get()) << endl;
//}
cerr << "Change "<< line1.Get() <<"\t" << line2.Get() << "\t" << line3.Get() << endl;
Wait(0.2);
}*/
char count=0, pneumaticCount=0;
// was .125 when loop at .025
lowPass lowSpeed(.04), lowStrafe(.04), lowTurn(.04), lowClaw(.04), lowArm(.04), lowArmLoc(.05);
double ClawLocation=0, ArmLocation=0, OldArmLocation=0;
while (IsOperatorControl() && !IsDisabled())
{
GetWatchdog().Feed();
float speed = -1*stick.GetRawAxis(2);
float strafe = stick.GetRawAxis(1);
float turn = stick.GetRawAxis(3);
if(!stick.GetRawButton(7)) {
speed /= 2;
strafe /= 2;
turn /= 2;
}
if(stick.GetRawButton(8)) {
speed /= 2;
strafe /= 2;
turn /= 2;
}
if(stick.GetRawButton(2)) {
speed = 0;
turn = 0;
}
Drive(lowSpeed(speed), lowTurn(turn), lowStrafe(strafe));
#ifndef NDEBUG
if(stick2.GetRawButton(10)) {
robotInted = false;
initRobot();
}
#endif
if(stick2.GetRawButton(7) && (miniBotTime.Get() >= 110 || (stick2.GetRawButton(9) && stick2.GetRawButton(10)))) { // launcher
// the quick launcher
MiniBot1a.Set(true);
MiniBot1b.Set(false);
}
if(!stick2.GetRawButton(10) && stick2.GetRawButton(9)) { // deploy in
MiniBot2a.Set(false);
MiniBot2b.Set(true);
//MiniBot2a.Set(false);
//MiniBot2b.Set(true);
}
if(stick2.GetRawButton(5)) { // top deploy out
MiniBot2a.Set(true);
MiniBot2b.Set(false);
}
if(stick2.GetRawButton(6)) { // open
ClawOpen.Set(true);
ClawClose.Set(false);
}
if(stick2.GetRawButton(8)) { // closed
ClawOpen.Set(false);
ClawClose.Set(true);
ClawLocation += 2;
}
/*156 straight
* 56 90 angle
* 10 back
*/
if(stick2.GetRawButton(1)) { // top peg
ClawLocation = 156;
ArmLocation = 105;
}
if(stick2.GetRawButton(2)) {
ClawLocation = 111; // the 90angle / middle peg
ArmLocation = 50;
}
if(stick2.GetRawButton(3)) { // off ground
ClawLocation = 176;
ArmLocation = 5;
}
if(stick2.GetRawButton(4)) {
ClawLocation = 0; // back
ArmLocation = 0;
}
double tmpClaw = .7*lowClaw(stick2.GetRawAxis(4));
if(tmpClaw < .2 && tmpClaw > -.2) tmpClaw = 0;
double tmpArm = .4*lowArm(-1*stick2.GetRawAxis(2));
if(tmpArm < .2 && tmpArm > -.2) tmpArm = 0;
if(tmpArm > .5) tmpArm = .5;
if(tmpArm < -.5) tmpArm = -.5;
ClawLocation += tmpClaw + tmpArm; // the right joy stick y
if(ClawLocation < 10) ClawLocation = 10;
if(ClawLocation > 230) ClawLocation = 230;
Claw(ClawLocation);
ArmLocation += tmpArm;
if(ArmLocation > 110) ArmLocation = 110;
if(ArmLocation < -10) ArmLocation = -10;
Arm(lowArmLoc(ArmLocation));
OldArmLocation = ArmLocation;
#ifndef NDEBUG
if(count++%20==0){
cerr << EncClaw.Get() << '\t' << arm1->GetOutputCurrent() << '\t' << arm1_sec->GetOutputCurrent() << '\t' << ArmLocation << '\t' << EncArm.Get() << endl;
// cerr << arm1->GetOutputCurrent() << '\t' << arm1_sec->GetOutputCurrent() << '\t' << arm2->GetOutputCurrent() << '\t'
// << EncArm.Get () << '\t' << LimitArm.Get() << '\t' << EncClaw.Get() << '\t' << LimitClaw.Get() << '\t' << ClawLocation << endl;
//cerr << '\t' << EncArm.Get() <<'\t' << arm1->Get() << endl;
// cerr << Dlf->Get() << '\t' << Dlf->GetSpeed() << '\t' << Dlb->GetSpeed() <<'\t' << Drf->GetSpeed() <<'\t' << Drb->GetSpeed() <<endl;//'\t' << line1.Get() << "\t" << line2.Get() << "\t" << line3.Get() << endl;
// cerr << '\t' << Dlb->GetSpeed() << '\t';
}
#endif
if(pneumaticCount++==0) {
ClawOpen.Set(false);
ClawClose.Set(false);
MiniBot1a.Set(false);
MiniBot1b.Set(false);
MiniBot2a.Set(false);
MiniBot2b.Set(false);
}
Wait(0.01); // wait for a motor update time
}
}
void RawControl::claw(bool open) {
gripper->Set(m_Cypress->GetClaw());
}
void OperatorControl(void) {
char count=0;
double target = 0, speed = 0;
while(!IsDisabled()) {
double tmpStick = -1*stick.GetRawAxis(2);
if(tmpStick < .2 && tmpStick > -.2) tmpStick=0;
target += tmpStick*1.5;
int location = enc.GetRaw();
if(stick.GetRawButton(5)) {
up.Set(true);
down.Set(false);
}else if(stick.GetRawButton(7) && location > 0) {
down.Set(true);
up.Set(false);
}else if(stick.GetRawButton(8)) {
down.Set(true);
up.Set(false);
}else if(stick.GetRawButton(9)){
speed = pid(target, location);
if(speed > 1) {
up.Set(true);
down.Set(false);
}else if(speed < -1) {
up.Set(false);
down.Set(true);
}else{
up.Set(false);
down.Set(false);
}
}else if(stick.GetRawButton(10)) {
enc.Reset();
}else{
up.Set(false);
down.Set(false);
}
if(stick.GetRawButton(1))
target = 2;
if(stick.GetRawButton(4))
target = 400;
if(stick.GetRawButton(3))
target = 200;
if(stick.GetRawButton(2))
target = 70;
Wait(.02);
while(count++%30==0) cerr << location << '\t' << target << '\t' << speed << endl;
}
}
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 tick_itf_pneumatic() {
Concurrent::IPCMutex *mtx = Memory::shared_mutex()->pcm;
for (int i = 0; i < 2; i++) {
MTX_LOCK(mtx, i);
Memory::Shared::IO::Pneumatics *pcm = Memory::shared()->pneumatics(i);
if (pcm->get_init()) {
Solenoid **sol = sols[i];
int modid = pcm->get_pcm_can_id();
if (!pcm->get_bootstrap()) {
cmps[i] = new Compressor(modid);
pcm->set_closed_loop(cmps[i]->GetClosedLoopControl());
for (int j = 0; j < 8; j++) {
sol[j] = new Solenoid(modid, j);
}
pcm->set_bootstrap(true);
}
Compressor *cmp = cmps[i];
pcm->set_fault_comp_too_high(cmp->GetCompressorCurrentTooHighFault());
pcm->set_fault_comp_not_conn_sticky(cmp->GetCompressorCurrentTooHighStickyFault());
pcm->set_fault_comp_shorted(cmp->GetCompressorShortedFault());
pcm->set_fault_comp_shorted_sticky(cmp->GetCompressorShortedStickyFault());
pcm->set_fault_comp_not_conn(cmp->GetCompressorNotConnectedFault());
pcm->set_fault_comp_not_conn_sticky(cmp->GetCompressorNotConnectedStickyFault());
if (pcm->get_comp_sticky_clear_pending()) {
cmp->ClearAllPCMStickyFaults();
pcm->set_comp_sticky_clear_pending(false);
}
pcm->set_pressure_switch(cmp->GetPressureSwitchValue());
if (pcm->get_closed_loop_mode_pending()) {
cmp->SetClosedLoopControl(pcm->get_closed_loop());
pcm->set_closed_loop_mode_pending(true);
}
pcm->set_is_enabled(cmp->Enabled());
if (pcm->get_start_pending()) {
cmp->Start();
pcm->set_start_pending(false);
}
if (pcm->get_stop_pending()) {
cmp->Stop();
pcm->set_stop_pending(false);
}
for (int j = 0; j < 8; j++) {
Solenoid *s = sol[j];
s->Set(pcm->get_solenoid(j));
pcm->set_solenoid_black(j, s->IsBlackListed());
}
Solenoid *s = sol[0];
pcm->set_fault_sol_volt(s->GetPCMSolenoidVoltageFault(modid));
pcm->set_fault_sol_volt_sticky(s->GetPCMSolenoidVoltageStickyFault(modid));
if (pcm->get_sol_sticky_clear_pending()) {
s->ClearAllPCMStickyFaults(modid);
pcm->set_sol_sticky_clear_pending(true);
}
pcm->set_comp_current(cmp->GetCompressorCurrent());
}
MTX_UNLOCK(mtx, i);
}
}
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 driveTrainValues(void) {
//Testing the encoder...
//Get the encoder value
//encoder_value = right_encoder->Get();
//Print encoder value
//printf("right encoder value: %f ", encoder_value);
//Shifting gears...
//Low gear
if (gamepad->GetRawButton(5))
{
printf("The left button is being pressed.\n");
drivetrain_pressure->Set(false);
drivetrain_vent->Set(true);
}
//Six is high gear
if (gamepad->GetRawButton(6))
{
printf("The right button is being pressed.\n");
drivetrain_pressure->Set(true);
drivetrain_vent->Set(false);
}
//assign right joystick value to 'right'
right=(gamepad->GetTwist());
//find the change in joystick values
rightchange=fabs(oldright-right);
// if the change in joystick values is less than 0.2 then use the old right value vs the new one
//Stops driver from accelerating the robot too quickly
if(rightchange<=threshold) {
useright=right;
}
//else... if the new right is greater than the old one use the old value plus the threshold (faster)
//if the new right is less than the old right use the old value minue the threshold (slower)
//so that really large movements of the joystick don't translate immediately into a really big acceleration
else {
if(oldright<right) {
useright=oldright+threshold;
}
if(oldright>right) {
useright=oldright-threshold;
}
}
//Same as right, but on the left side
left=(gamepad->GetY());
leftchange=fabs(oldleft-left);
if(leftchange<=threshold) {
useleft=left;
}
else {
if(oldleft<left) {
useleft=oldleft+threshold;
}
if(oldleft>left) {
useleft=oldleft-threshold;
}
}
//printf("hello world");
//make useright and useleft (the values sent to the robot) the old values for the next loop
oldright=useright;
oldleft=useleft;
}
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
}
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
valve->Set(true);
Wait(5.0);
valve->Set(false);
}
/**
* Set the value of a solenoid.
*
* @param channel The channel on the Solenoid module
* @param on Turn the solenoid output off or on.
*/
void SetSolenoid(UINT32 channel, bool on)
{
Solenoid *s = allocateSolenoid(channel);
if (s != NULL)
s->Set(on);
}
