RobotSystem(void):
robotInted(false)
,stick(1) // as they are declared above.
,stick2(2)
,line1(10)
,line2(11)
,line3(12)
//,camera(AxisCamera::GetInstance())
,updateCAN("CANUpdate",(FUNCPTR)UpdateCAN)
,cameraTask("CAMERA", (FUNCPTR)CameraTask)
,compressor(14,1)
,EncArm(2,3)
,EncClaw(5,6)
,PIDArm(.04,0,0) // .002, .033
,PIDClaw(.014,.0000014,0)
,LowArm(.1)
/*
,MiniBot1(4)
,MiniBot2(2)
,ClawGrip(3)
*/
,MiniBot1a(8,1)
,MiniBot1b(8,2)
,MiniBot2a(8,3)
,MiniBot2b(8,4)
,ClawOpen(8, 8)
,ClawClose(8,7)
,LimitClaw(7)
,LimitArm(13)
{
// myRobot.SetExpiration(0.1);
GetWatchdog().SetEnabled(false);
GetWatchdog().SetExpiration(1);
compressor.Start();
debug("Waiting to init CAN");
Wait(2);
Dlf = new CANJaguar(6,CANJaguar::kSpeed);
Dlb = new CANJaguar(3,CANJaguar::kSpeed);
Drf = new CANJaguar(7,CANJaguar::kSpeed);
Drb = new CANJaguar(2,CANJaguar::kSpeed);
arm1 = new CANJaguar(5);
arm1_sec = new CANJaguar(8);
arm2 = new CANJaguar(4);
EncArm.SetDistancePerPulse(.00025);
EncClaw.SetDistancePerPulse(.00025);
EncClaw.SetReverseDirection(false);
EncArm.SetReverseDirection(true);
EncArm.Reset();
EncClaw.Reset();
updateCAN.Start((int)this);
//cameraTask.Start((int)this);
EncArm.Start();
EncClaw.Start();
debug("done initing");
}
/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
* If using the SendableChooser make sure to add them to the chooser code above as well.
*/
void AutonomousInit()
{
autoSelected = *((std::string*)chooser->GetSelected());
//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
std::cout << "Auto selected: " << autoSelected << std::endl;
rotation = 0.0;
//*((double*)posChooser->GetSelected());
//goal = *((std::string*)goalChooser->GetSelected());
shoot = "No";
//*((std::string*)shootChooser->GetSelected());
defenseCrossed = false;
done = false;
std::cout << "Here" << std::endl;
drive->SetMaxOutput(1.0);
std::cout << "there" << std::endl;
//Make sure to reset the encoder!
leftEnc->Reset();
rightEnc->Reset();
gyro->Reset();
autoCounter = 0;
timer->Reset();
}
void RawControl::resetEncoders()
{
wheelEncoderFR->Reset();
wheelEncoderFL->Reset();
wheelEncoderBR->Reset();
wheelEncoderBL->Reset();
}
// Starts at the beginning of the autonomous period
void Robot::AutonomousInit() {
autoLoopCounter = 0;
encoder1.Reset();
encoder2.Reset();
lifterEncoder.Reset();
ballManipulatorEncoder.Reset();
}
void TeleopInit()
{
leftEnc->Reset();
rightEnc->Reset();
gyro->Reset();
powerCounter = 0;
}
void RobotInit ()
{
lw = LiveWindow::GetInstance();
CameraServer::GetInstance()->SetQuality(50);
//the camera name (ex "cam0") can be found through the roborio web interface
CameraServer::GetInstance()->StartAutomaticCapture("cam1");
AutonState = 0;
ballarm.Reset();
ballarm.SetMaxPeriod(.01);
ballarm.SetMinRate(.02);
ballarm.SetDistancePerPulse(.9);
gyroOne.Calibrate();
UpdateActuatorCmnds(0,0,false,false,false,false,false,false,false,0,0,0,0,0);
UpdateSmartDashboad(false,
false,
false,
false,
false,
0,
0,
0,
0,
0,
0,
0,
0,
0);
}
void Drive (float speed, int dist)
{
leftDriveEncoder.Reset();
leftDriveEncoder.Start();
int reading = 0;
dist = abs(dist);
// The encoder.Reset() method seems not to set Get() values back to zero,
// so we use a variable to capture the initial value.
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "initial=%d\n", leftDriveEncoder.Get());
dsLCD->UpdateLCD();
// Start moving the robot
robotDrive.Drive(speed, 0.0);
while ((IsAutonomous()) && (reading <= dist))
{
reading = abs(leftDriveEncoder.Get());
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "reading=%d\n", reading);
dsLCD->UpdateLCD();
}
robotDrive.Drive(0.0, 0.0);
leftDriveEncoder.Stop();
}
// Runs during test mode
// Test
// *
void Test()
{
shifters.Set(DoubleSolenoid::kForward);
leftDriveEncoder.Start();
leftDriveEncoder.Reset();
int start = leftDriveEncoder.Get();
while (IsTest()) {
if (rightStick.GetRawButton(7)) {
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
else {
robotDrive.ArcadeDrive(rightStick.GetY()/2, -rightStick.GetX()/2);
}
if (gamepad.GetEvent(4) == kEventClosed) {
start = leftDriveEncoder.Get();
}
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "lde: %d", leftDriveEncoder.Get() - start);
dsLCD->UpdateLCD();
gamepad.Update();
}
}
// WHen robot is enabled
void init() {
log->info("initializing");
log->print();
climber = NULL;
leftDriveEncoder->Reset();
leftDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
leftDriveEncoder->Start();
rightDriveEncoder->Reset();
rightDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
rightDriveEncoder->Start();
//leftClimber->motorController->Disable();
leftClimber->encoder->Reset();
leftClimber->encoder->Start();
//rightClimber->motorController->Disable();
rightClimber->encoder->Reset();
rightClimber->encoder->Start();
bcdValue = bcd->value();
loadSwitchOldState = loaderSwitch->Get();
#if 0
bool leftDone = false;
bool rightDone = false;
// Only do this for some BCD values?
while (!leftDone || !rightDone){
if (!leftDone)
leftDone = leftClimber->UpdateState(-1.0, -30);
if (!rightDone)
rightDone = rightClimber->UpdateState(-1.0, -30);
log->info("Wait: Ll Rl: %d %d",
leftClimber->lowerLimitSwitch->Get(),
rightClimber->lowerLimitSwitch->Get());
log->print();
}
#endif
climbState = NotInitialized;
cameraElevateAngle =
(cameraElevateMotor->GetMaxAngle()-cameraElevateMotor->GetMinAngle()) * 2/3;
cameraPivotAngle = 0;
cameraPivotMotor->SetAngle(cameraPivotAngle);
cameraElevateMotor->SetAngle(cameraElevateAngle);
loading = false;
loaderDisengageDetected = false;
//This is a rough guess of motor power it should be based on voltage
shooterMotorVolts = 8.0; // volts as a fraction of 12V
loadCount = 0;
}
void initRobot () {
cerr << "running init\n";
Dlf->EnableControl(0);
Dlb->EnableControl(0);
Drf->EnableControl(0);
Drb->EnableControl(0);
arm1->EnableControl();
arm1_sec->EnableControl();
arm2->EnableControl();
Dlf->ConfigEncoderCodesPerRev(250);
Dlf->SetPID(1,0,0);
Dlb->ConfigEncoderCodesPerRev(250);
Dlb->SetPID(1,0,0);
Drf->ConfigEncoderCodesPerRev(250);
Drf->SetPID(1,0,0);
Drb->ConfigEncoderCodesPerRev(250);
Drb->SetPID(1,0,0);
Wait(.1);
if(robotInted==false) {
int count=220;
arm2->Set(-.3);
while(count-->0 && LimitClaw.Get() == 1) Wait(.005);
arm2->Set(.15);
while(count-->0 && LimitClaw.Get() == 0) Wait(.005);
arm2->Set(0);
if(count>0)
EncClaw.Reset();
arm1->Set(-.3);
arm1_sec->Set(-.3);
while(count-->0 && LimitArm.Get() == 1) Wait(.005);
arm1->Set(.5);
arm1_sec->Set(.5);
while(count-->0 && LimitArm.Get() == 0) Wait(.005);
if(count>0)
EncArm.Reset();
arm1->Set(0);
arm1_sec->Set(0);
robotInted = true;
}
}
void Test() {
robotDrive.SetSafetyEnabled(false);
uint8_t toSend[10];//array of bytes to send over I2C
uint8_t toReceive[10];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 7; //send 0 to arduino
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
bool isSettingUp = true;
pickup.setGrabber(-1);
pickup.setLifter(1);
while (isSettingUp) {
isSettingUp = false;
if (grabOuterLimit.Get() == false) {
pickup.setGrabber(0);
}
else {
isSettingUp = true;
}
if (liftLowerLimit.Get()) {
pickup.setLifter(0);
}
else {
isSettingUp = true;
}
}
gyro.Reset();
liftEncoder.Reset();
grabEncoder.Reset();
toSend[0] = 8;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
while(IsTest() && IsEnabled());
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
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;
}
}
/**
* Runs during test mode ```````
*/
void Test() {
TestMode tester(m_ds);
driveDistanceRight.Reset();
driveDistanceRight.Start();
ballGrabber.resetSetPoint();
shooter.motorShutOff();
while (IsTest() && IsEnabled()){
lcd->Clear();
tester.PerformTesting(&gamePad, &driveDistanceRight, lcd, &rightJoyStick, &leftJoyStick,
&testSwitch, &testTalons, &frontUltrasonic, &backUltrasonic,
&ballGrabber.ballDetector, &analogTestSwitch,
&shooter, &ballGrabber
);
lcd->UpdateLCD();
Wait(0.1);
}
driveDistanceRight.Stop();
}
void DoAutonomousMoveStep(const step_speed *speeds, char * message)
{
leftDriveEncoder.Reset();
double dist = speeds[0].distance;
double reading;
// Start moving the robot
leftDriveMotor.Set(speeds->speed_left);
rightDriveMotor.Set(speeds->speed_right);
reading = absolute(leftDriveEncoder.GetDistance());
while (dist > reading)
{
Wait(0.02);
reading = absolute(leftDriveEncoder.GetDistance());
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "D: %5.0f R : %5.0f", dist, reading);
dsLCD->UpdateLCD();
}
leftDriveMotor.Set(0.0);
rightDriveMotor.Set(0.0);
}
void Autonomous()
{
Timer timer;
float power = 0;
bool isLifting = false;
bool isGrabbing = false;
double liftHeight = Constants::liftBoxHeight-Constants::liftBoxLip;
double grabPower = Constants::grabAutoCurrent;
bool backOut;
uint8_t toSend[1];//array of bytes to send over I2C
uint8_t toReceive[0];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 2;//set the byte to send to 1
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);//send over I2C
bool isSettingUp = true;
//pickup.setGrabber(-1); //open grabber all the way
pickup.setLifter(0.8);
while (isSettingUp && IsEnabled() && IsAutonomous()) {
isSettingUp = false;
/*if (grabOuterLimit.Get() == false) {
pickup.setGrabber(0); //open until limit
}
else {
isSettingUp = true;
}*/
if (liftLowerLimit.Get()) {
pickup.setLifter(0); //down till bottom
}
else {
isSettingUp = true;
}
}
gyro.Reset();
liftEncoder.Reset();
grabEncoder.Reset();
if (grabStick.GetZ() > .8) {
timer.Reset();
timer.Start();
while (timer.Get() < 1) {
robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back
if(power>-.4){
power-=0.005;
Wait(.005);
}
}
robotDrive.MecanumDrive_Cartesian(0, 0, 0, gyro.GetAngle()); // STOP!!!
timer.Stop();
timer.Reset();
Wait(1);
}
power = 0;
while (isLifting && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
backOut = Constants::autoBackOut;
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);
Wait(.005);
while (isGrabbing && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
liftHeight = 3*Constants::liftBoxHeight;
Wait(.005);
pickup.lifterPosition(liftHeight, isLifting, grabStick);
Wait(.005);
while (isLifting && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12 < 2 && IsEnabled() && IsAutonomous()); // while the nearest object is closer than 2 feet
timer.Start();
while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches < Constants::autoBackupDistance && timer.Get() < Constants::autoMaxDriveTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet
if (power < .45) { //ramp up the power slowly
power += .00375;
}
robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back
float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor
SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash
SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle());
SmartDashboard::PutNumber("Distance (in)", prox.GetVoltage() * Constants:: ultrasonicVoltageToInches);
Wait(.005);
}
timer.Reset();
while(timer.Get() < Constants::autoBrakeTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet
robotDrive.MecanumDrive_Cartesian(0,Constants::autoBrakePower,0); ///Brake
}
float turn = 0;
while (fabs(turn) < 85 && IsEnabled() && IsAutonomous()) { //turn 90(ish) degrees
robotDrive.MecanumDrive_Cartesian(0,0,.1);
turn = gyro.GetAngle();
if (turn > 180) {
turn -= 360;
}
}
robotDrive.MecanumDrive_Cartesian(0,0,0); ///STOP!!!
timer.Stop();
toSend[0] = 8;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
while(IsAutonomous() && IsEnabled());
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
/******************************************************************************
* Function: TeleopPeriodic
*
* Description: This is the function that is called during the periodic period
* for teleop.
******************************************************************************/
void TeleopPeriodic()
{
float DrvRY, DrvLY;
float BallRollerSpd;
float FishTapeSpd;
float WenchSpd;
float LgtY, LgtZ;
double LgtPOV;
int LgtP1;
bool LgtT0;
bool LgtB1,LgtB3,LgtB4,LgtB5,LgtB6,LgtB7,LgtB8,LgtB9,LgtB11,LgtB10,LgtB12;
bool XbxB4, XbxB5, XbxB2, XbxB1;
bool BallSw;
int XbxPOV;
double ArmP;
double ArmError;
double desiredPos;
double LoArmCurve;
while (IsOperatorControl() && IsEnabled())
{
/* Read sensor values here: */
ArmP = ballarm.GetDistance();
BallSw = BlSw.Get();
/* Read Xbox controller commands here: */
DrvLY = -xboxDrive.GetRawAxis(1);
DrvRY = -xboxDrive.GetRawAxis(5);
XbxB1 = xboxDrive.GetRawButton(1);
XbxB2 = xboxDrive.GetRawButton(2);
XbxB4 = xboxDrive.GetRawButton(4);
XbxB5 = xboxDrive.GetRawButton(5);
XbxPOV = xboxDrive.GetPOV(0);
/* Read Logictec joystick commands here: */
LgtY = Logitech.GetRawAxis(1);
LgtZ = Logitech.GetRawAxis(2);
LgtP1 = Logitech.GetPOV(0);
LgtT0 = Logitech.GetRawButton(1);
LgtB1 = Logitech.GetRawButton(2);
LgtB3 = Logitech.GetRawButton(3);
LgtB4 = Logitech.GetRawButton(4);
LgtB5 = Logitech.GetRawButton(5);
LgtB6 = Logitech.GetRawButton(6);
LgtB7 = Logitech.GetRawButton(7);
LgtB8 = Logitech.GetRawButton(8);
LgtB9 = Logitech.GetRawButton(9);
LgtB10 = Logitech.GetRawButton(10);
LgtB11 = Logitech.GetRawButton(11);
LgtB12 = Logitech.GetRawButton(12);
LgtPOV = (double)LgtP1;
// DriverStation::
//double GetMatchTime();
// SmartDashboard::PutNumber("MatchTime", GetMatchTime());
if(LgtB12 && LgtB10)
{
ballarm.Reset();
}
/* Set the desired position of the ball arm. */
if (LgtB7)
{
/* This is the middle position of the arm: */
desiredPos = 120;
}
else if (LgtB8)
{
/* This is the upper position of the arm: */
desiredPos = 210;
}
else if((LgtB3) || (LgtB5) ||(LgtB6) || (LgtB9) || (LgtB11))
{
/* Default */
desiredPos = 0;
}
/* Set the ball roller state: */
if (LgtT0 == true &&
BallSw == true)
{
BallRollerSpd = 1;
}
else if (LgtB1 == true)
{
BallRollerSpd = -1;
}
else
{
BallRollerSpd = 0.0;
}
/* Set the output to the fish tape: */
if (LgtP1 == 180)
{
FishTapeSpd = -1.0;
}
else if(LgtP1 == 0)
{
FishTapeSpd = 1.0;
}
else
{
FishTapeSpd = 0.0;
}
/* Set the output to the lower arm: */
LoArmCurve = LgtY*LowArmGx;
// SmartDashboard::
/* Determine the wench state */
if (LgtB4 == true)
{
WenchSpd = 1;
}
else if (XbxB4)
{
WenchSpd = -.5;
}
else
{
WenchSpd = 0.0;
}
/* if(GetMatchTime() < 30)
{
Light1.Set(0);
Light2.Set(0);
}*/
/* Output data to the smart dashboard: */
ReadAutonSwitch();
UpdateSmartDashboad(Sw1.Get(),
Sw2.Get(),
Sw3.Get(),
Sw4.Get(),
BlSw.Get(),
AutonState,
0,
ballarm.GetDistance(),
gyroOne.GetAngle(),
accel.GetX(),
accel.GetY(),
accel.GetZ(),
(double)DrvLY,
(double)DrvRY);
UpdateActuatorCmnds(BallRollerSpd,
desiredPos,
LgtB3,
LgtB5,
LgtB6,
LgtB7,
LgtB8,
LgtB9,
LgtB11,
DrvLY,
DrvRY,
FishTapeSpd,
LoArmCurve,
WenchSpd);
/* Force the program to wait a period of time in order to conserve power: */
Wait(kUpdatePeriod); // Wait 5ms for the next update.
Scheduler::GetInstance()->Run();
}
}
void TeleopPeriodic()
{
char myString [STAT_STR_LEN];
if (running)
{
enterHoldCommand = joystick->GetRawButton(BUT_JS_ENT_POS_HOLD);
exitHoldCommand = joystick->GetRawButton(BUT_JS_EXIT_POS_HOLD);
switch (liftState)
{
case raising:
if (GetLiftLimitSwitchMax())
{
SetLiftMotor(MOTOR_SPEED_STOP);
if(!liftEncFullRanged)
{
maxLiftEncDist = liftEncoder->GetDistance();
liftEncFullRanged = true;
}
motorSpeed = -MOTOR_SPEED_DOWN;
liftState = lowering;
SetLiftMotor(motorSpeed);
}
if (enterHoldCommand && liftEncZeroed && liftEncFullRanged)
{
liftState = holding;
}
break;
case lowering:
if (GetLiftLimitSwitchMin())
{
SetLiftMotor(MOTOR_SPEED_STOP);
if(!liftEncZeroed)
{
liftEncoder->Reset();
liftEncZeroed = true;
}
motorSpeed=MOTOR_SPEED_UP;
liftState = raising;
SetLiftMotor(motorSpeed);
}
if (enterHoldCommand && liftEncZeroed && liftEncFullRanged)
{
liftState = holding;
}
break;
case holding:
if(!(controlLift->IsEnabled()))
{
pidPosSetPoint = SP_RANGE_FRACTION*maxLiftEncDist; //go to the midpoint of the range
controlLift->SetSetpoint(pidPosSetPoint);
#if BUILD_VERSION == COMPETITION
controlLift2->SetSetpoint(pidPosSetPoint);
#endif
controlLift->Enable();
#if BUILD_VERSION == COMPETITION
controlLift2->Enable();
#endif
}
if(exitHoldCommand)
{
controlLift->Disable();
#if BUILD_VERSION == COMPETITION
controlLift2->Disable();
#endif
motorSpeed = -MOTOR_SPEED_DOWN;
liftState = lowering;
SetLiftMotor(motorSpeed);
}
break;
}
}
//status
sprintf(myString, "running: %d\n", running);
SmartDashboard::PutString("DB/String 0", myString);
sprintf(myString, "State: %d\n", liftState);
SmartDashboard::PutString("DB/String 1", myString);
sprintf(myString, "motorSpeed: %f\n", motorSpeed);
SmartDashboard::PutString("DB/String 2", myString);
sprintf(myString, "lift encoder zeroed: %d\n", liftEncZeroed);
SmartDashboard::PutString("DB/String 3", myString);
sprintf(myString, "max enc set: %d\n", liftEncFullRanged);
SmartDashboard::PutString("DB/String 4", myString);
sprintf(myString, "maxLiftEncDist: %f\n", maxLiftEncDist);
SmartDashboard::PutString("DB/String 5", myString);
sprintf(myString, "enc dist: %f\n", liftEncoder->GetDistance());
SmartDashboard::PutString("DB/String 6", myString);
sprintf(myString, "pid: %d\n", controlLift->IsEnabled());
SmartDashboard::PutString("DB/String 7", myString);
sprintf(myString, "dist to sp : %f\n", DistToSetpoint());
SmartDashboard::PutString("DB/String 8", myString);
sprintf(myString, "at sp : %d\n", AtSetpoint());
SmartDashboard::PutString("DB/String 9", myString);
}
void Reset() {
m_speedController->Set(0.0f);
m_encoder->Reset();
}
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
}
}
// Starts at the beginning of the autonomous period
void Robot::AutonomousInit() {
autoLoopCounter = 0;
encoder1.Reset();
encoder2.Reset();
}
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
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous()
{
init();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Entered Autonomous");
driveTrain.SetSafetyEnabled(false);
bool checkBox1 = SmartDashboard::GetBoolean("Checkbox 1");
m_FromAutonomous = true;
//float rightDriveSpeed = -1.0;
//float leftDriveSpeed = -1.0;
//int rangeToWallClose = 60;
//int rangeToWallFar = 120;
//Initialize encoder.
//int distanceToShoot = 133;
//int shootDelay = 0;
//ballGrabber.elevatorController.SetSetpoint(PHOENIX2014_INITIAL_AUTONOMOUS_ELEVATOR_ANGLE);
//TODO Remove encoders from code??
driveDistanceRight.Reset();
driveDistanceLeft.Reset();
driveDistanceRight.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_RIGHT);
driveDistanceLeft.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_LEFT);
driveDistanceRight.Start();
driveDistanceLeft.Start();
//int printDelay = 0;
float minDistance = 52.0; // Closer to the wall than this is too close
float maxDistance = 12.0*11.0; // Once at least this close, within shooting range
//float closeDistance = maxDistance + 24.0;
float autoDriveSpeed = 0.55;
//float autoDriveSlowSpeed = 0.38;
int time = 0;
int maxDriveLoop = 4*200; // 4 seconds @200 times/sec
bool shootingBall = false;
bool wantFirstShot = true;
if(checkBox1 == false){
int printDelay = 0;
//Ultrasonic Autonomous
//bool isInRange = false;
while(IsAutonomous() && IsEnabled())
{
float currentDistance = frontUltrasonic.GetAverageDistance();
// Transition isInRange from false to true once
if((minDistance < currentDistance) && (currentDistance < maxDistance)){
//isInRange = true;
}
time++;
bool motorDriveTimeExceeded = time > maxDriveLoop;
bool motorMinMet = time > m_MinDriveLoop;
if(/*isInRange ||*/ motorMinMet){
driveTrain.TankDrive(0.0,0.0);
if((ballGrabber.elevatorAngleSensor.GetVoltage() > PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE - 0.1) &&
(ballGrabber.elevatorAngleSensor.GetVoltage() < PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE + 0.1)){
//Enough to cover break release and start winding again.
shootingBall = shooter.OperateShooter(wantFirstShot);
}
if(shootingBall == true){
wantFirstShot = false;
}
}
else if(motorDriveTimeExceeded){
driveTrain.TankDrive(0.0,0.0);
}
else{
//if((currentDistance < closeDistance) && motorMinMet){
// autoDriveSpeed = autoDriveSlowSpeed;
//}
driveTrain.TankDrive(-0.97 * autoDriveSpeed, -1.0 * autoDriveSpeed);//the DriveTrain is BACKWARD
}
ballGrabber.RunElevatorAutonomous(PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE);
printDelay = printDelay++;
if(printDelay >= 200){
lcd->Clear();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "In Autonomous");
shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line2, lcd);
shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd);
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "Ulra=%f", frontUltrasonic.GetAverageDistance());
//lcd->PrintfLine(DriverStationLCD::kUser_Line5, "CEV=%f", ballGrabber.elevatorAngleSensor.GetVoltage());
//lcd->PrintfLine(DriverStationLCD::kUser_Line6, "DEV=%f", ballGrabber.m_desiredElevatorVoltage);
lcd->UpdateLCD();
printDelay = 0;
}
Wait(0.005);
}
lcd->Clear();
lcd->UpdateLCD();
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "Exiting Autonomous");
} else {
//Timer Autonomous
driveTrain.TankDrive(-0.5,-0.5);
ballGrabber.DriveElevatorTestMode(-1.0);
lcd->Clear();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Skip Auto");
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "CheckBox Checked");
lcd->UpdateLCD();
Wait(2.0);
bool shooting = true;
driveTrain.TankDrive(0.0,0.0);
int counter = 0;
while(counter < 600){
shooter.OperateShooter(shooting);
Wait(0.005);
}
}
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()//teleop code
{
robotDrive.SetSafetyEnabled(false);
gyro.Reset();
grabEncoder.Reset();
timer.Start();
timer.Reset();
double liftHeight = 0; //variable for lifting thread
int liftHeightBoxes = 0; //another variable for lifting thread
int liftStep = 0; //height of step in inches
int liftRamp = 0; //height of ramp in inches
double grabPower;
bool backOut;
uint8_t toSend[10];//array of bytes to send over I2C
uint8_t toReceive[10];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 1;//set the byte to send to 1
i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C
bool isGrabbing = false;//whether or not grabbing thread is running
bool isLifting = false;//whether or not lifting thread is running
bool isBraking = false;//whether or not braking thread is running
float driveX = 0;
float driveY = 0;
float driveZ = 0;
float driveGyro = 0;
bool liftLastState = false;
bool liftState = false; //button pressed
double liftLastTime = 0;
double liftTime = 0;
bool liftRan = true;
Timer switchTimer;
Timer grabTimer;
switchTimer.Start();
grabTimer.Start();
while (IsOperatorControl() && IsEnabled())
{
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
// This sample does not use field-oriented drive, so the gyro input is set to zero.
toSend[0] = 1;
numToSend = 1;
driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code
driveY = driveStick.GetRawAxis(Constants::driveYAxis);
driveZ = driveStick.GetRawAxis(Constants::driveZAxis);
driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset;
if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X
toSend[0] = 6;
numToSend = 1;
if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) {
driveY = 0;
driveZ = 0;
}
else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y
driveX = 0;
driveZ = 0;
}
else {//if Z is greater than X and Y, then it will only go in the direction of Z
driveX = 0;
driveY = 0;
}
}
if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function
toSend[0] = 7;
driveZ = 0;//Stops Z while Z lock is pressed
}
if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field
driveGyro = 0;//gyro stops while field lock is enabled
}
driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree);
driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree);
driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree);
robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive
if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) {
pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber
if(grabTimer.Get() < 1) {
toSend[0] = 6;
}
}
else {
pickup.setGrabber(0);
grabTimer.Reset();
toSend[0] = 6;
}
if (Constants::grabLiftInverted) {
pickup.setLifter(-Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
}
else {
pickup.setLifter(Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
}
SmartDashboard::PutNumber("Lift Power", Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree));
SmartDashboard::PutBoolean("Is Lifting", isLifting);
if (Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree) != 0 || isLifting) { //if the robot is lifting
isBraking = false; //stop braking thread
SmartDashboard::PutBoolean("Braking", false);
}
else if(!isBraking) {
isBraking = true; //run braking thread
pickup.lifterBrake(isBraking);//brake the pickup
}
if (grabStick.GetRawButton(Constants::liftFloorButton)) {
liftHeight = 0;
pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
liftRan = true;
}
liftTime = timer.Get();
liftState = grabStick.GetRawButton(Constants::liftButton);
if (liftState) { //if button is pressed
if (!liftLastState) {
if (liftTime - liftLastTime < Constants::liftMaxTime) {
if (liftHeightBoxes < Constants::liftMaxHeightBoxes) {
liftHeightBoxes++; //adds 1 to liftHeightBoxes
}
}
else {
liftHeightBoxes = 1;
liftRamp = 0;
liftStep = 0;
}
}
liftLastTime = liftTime;
liftLastState = true;
liftRan = false;
}
else if (grabStick.GetRawButton(Constants::liftRampButton)) {
if (liftTime - liftLastTime > Constants::liftMaxTime) {
liftHeight = 0;
liftStep = 0;
}
liftRamp = 1; //prepares to go up ramp
liftLastTime = liftTime;
liftRan = false;
}
else if (grabStick.GetRawButton(Constants::liftStepButton)) {
if (liftTime - liftLastTime > Constants::liftMaxTime) {
liftHeight = 0;
liftRamp = 0;
}
liftStep = 1; //prepares robot for step
liftLastTime = liftTime;
liftRan = false;
}
else {
if (liftTime - liftLastTime > Constants::liftMaxTime && !liftRan) {
liftHeight = liftHeightBoxes * Constants::liftBoxHeight + liftRamp * Constants::liftRampHeight + liftStep * Constants::liftStepHeight; //sets liftHeight
if (liftHeightBoxes > 0) {
liftHeight -= Constants::liftBoxLip;
}
pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
liftRan = true;
}
liftLastState = false;
}
if (grabStick.GetRawButton(Constants::grabToteButton)) {//if grab button is pressed
grabPower = Constants::grabToteCurrent;
backOut = true;
if (!isGrabbing) {
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
}
}
else if (grabStick.GetRawButton(Constants::grabBinButton)) {//if grab button is pressed
grabPower = Constants::grabBinCurrent;
backOut = false;
if (!isGrabbing) {
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
}
}
else if (grabStick.GetRawButton(Constants::grabChuteButton)) {//if grab button is presset
SmartDashboard::PutBoolean("Breakpoint -2", false);
SmartDashboard::PutBoolean("Breakpoint -1", false);
SmartDashboard::PutBoolean("Breakpoint 0", false);
SmartDashboard::PutBoolean("Breakpoint 1", false);
SmartDashboard::PutBoolean("Breakpoint 2", false);
SmartDashboard::PutBoolean("Breakpoint 3", false);
SmartDashboard::PutBoolean("Breakpoint 4", false);
//Wait(.5);
if (!isGrabbing) {
//pickup.grabberChute(isGrabbing, grabStick);//start grabber thread
}
}
//determines what the LED's look like based on what the Robot is doing
if (isGrabbing) {
toSend[0] = 5;
numToSend = 1;
}
if (isLifting) {//if the grabbing thread is running
if (Constants::encoderToDistance(liftEncoder.Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < liftHeight) {
toSend[0] = 3;
}
else {
toSend[0] = 4;
}
numToSend = 1;//sends 1 byte to I2C
}
if(!grabOuterLimit.Get()) { //tells if outer limit is hit with lights
if(switchTimer.Get() < 1) {
toSend[0] = 6;
}
}
else {
switchTimer.Reset();
}
if (driveStick.GetRawButton(Constants::sneakyMoveButton)) {
toSend[0] = 0;
numToSend = 1;
}
float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor
float rotations = (float) liftEncoder.Get(); // rotations on encoder
SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash
SmartDashboard::PutNumber("Current", pdp.GetCurrent(Constants::grabPdpChannel));
SmartDashboard::PutNumber("LED Current", pdp.GetCurrent(Constants::ledPdpChannel));
SmartDashboard::PutNumber("Lift Encoder", rotations);
SmartDashboard::PutNumber("Lift Height", liftHeight);
SmartDashboard::PutNumber("Grab Encoder", grabEncoder.Get());
SmartDashboard::PutBoolean("Grab Inner", grabInnerLimit.Get());
SmartDashboard::PutBoolean("Grab Outer", grabOuterLimit.Get());
SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
SmartDashboard::PutNumber("Throttle", grabStick.GetZ());
i2c.Transaction(toSend, 1, toReceive, 0);//send and receive information from arduino over I2C
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
} //end of teleop
isBraking = false;
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
void OperatorControl()//TODO remember that this is the beginning of operator controll
{
printf(
"****************************VERSION .00017****************************\n");
#if CAMERA
AxisCamera &camera = AxisCamera::GetInstance("10.26.43.11");
#endif
GetWatchdog().SetEnabled(true);
shooter_reset ->Start();
#if TIMER_RESET
pid_code_timer->Reset();
#endif
front_shooter_encoder->Reset();
back_shooter_encoder->Reset();
/*float prev_error_front = 0;
float prev_error_back = 0;
int test_back = 5;
int test_front = 6;*/
integral_back = 0.0;
integral_front = 0.0;
desired_RPS_control = 0.0;
//RETRACTS PISTON WHEN TELEOP STARTS
override_timer->Reset();
stabilizing_timer ->Reset();
retraction_timer ->Reset();
shooter_fire_piston_A ->Set(true);
shooter_fire_piston_B ->Set(false);
shooter_stop_timer->Start();
while (IsOperatorControl() && IsEnabled())
{
GetWatchdog().Feed();
//---------------------Display Output---------------------------
dsLCD = DriverStationLCD::GetInstance();
DriverLCD();
//printf("PRINTFS\n");
printfs();
//-------------------------Climber-----------------------------
//climber_code();
//dumb_climber_code();
climber_state();
//---------------------------Drive-----------------------------
arcade_tank_code();
constant_RPS_code();
//dumb_drive_code(); // In case our smart code doesnt work
//--------------------------PID-------------------------------
integral_reset();
//-------------------------Shooter----------------------------
pneumatic_shooter_angler_code();
pneumatic_feeder_code();
//dump_code();
//intelligent_shooter();
//-------------------------Test Code--------------------------
//camera_test();
//cin_code_get();
//prev_error_back = RPS_control_code(shooter_motor_back, back_shooter_encoder, prev_error_back, desired_RPS_control);
//interpolated_test_code();
//pointer_test(shooter_motor_front);
//PIDController(first_pterm, iterm, dterm, front_shooter_encoder, shooter_motor_front);
//------------cRIO Housekeeping Timing-MUST HAVE--------------
Wait(0.005);
}//while operator
}//operator ctrl
void Autonomous()
{
GetWatchdog().SetEnabled(true);
autonomous_timer->Reset();
#if TIMER_RESET
pid_code_timer->Reset();
#endif
front_shooter_encoder->Reset();
back_shooter_encoder->Reset();
shooter_angle_1 ->Set(true);
shooter_angle_2 ->Set(false);
retraction_timer->Reset();
stabilizing_timer->Reset();
override_timer->Reset();
//shooter_motor_front->Set(0.8);
//shooter_motor_front->Set(0.8);
integral_back = 0.0;
integral_front = 0.0;
error_back = 0.0;
error_front = 0.0;
desired_RPS_control = back_position_RPS_1;
smart_autonomous_state = unstable;//default is unstable
while (IsAutonomous() && IsEnabled())
{
printfs();
GetWatchdog().Feed();
RPS_control_code(desired_RPS_control);
Wait(.001);
if (autonomous_timer->Get() >= first_fire && autonomous_timer->Get() <(first_fire + 1))//fire
{
shooter_fire_piston_A ->Set(false);
shooter_fire_piston_B ->Set(true);
}
if (autonomous_timer->Get() >= first_retract && autonomous_timer->Get() < (first_retract + 2))//retract
{
shooter_fire_piston_A ->Set(true);
shooter_fire_piston_B ->Set(false);
}
if (autonomous_timer->Get() >= second_fire && autonomous_timer->Get() < (second_fire + 1))//fire
{
shooter_fire_piston_A ->Set(false);
shooter_fire_piston_B ->Set(true);
}
if (autonomous_timer->Get() >= second_retract && autonomous_timer->Get() < (second_retract +2))//retract
{
shooter_fire_piston_A ->Set(true);
shooter_fire_piston_B ->Set(false);
}
if (autonomous_timer->Get() >= third_fire && autonomous_timer->Get() < (third_fire + 1))//fire
{
shooter_fire_piston_A ->Set(false);
shooter_fire_piston_B ->Set(true);
}
if (autonomous_timer->Get() >= third_retract)//final retract
{
shooter_fire_piston_A ->Set(true);
shooter_fire_piston_B ->Set(false);
}
}
autonomous_timer->Reset();
}
void Autonomous()
{
GetWatchdog().SetEnabled(false);
Timer* hotGoalTimer = new Timer();
Timer* reloadTimer = new Timer();
Timer* intakeTimer = new Timer();
Timer* intakeDropTimer = new Timer();
bool goalFound = false;
//bool rightSideHot = false;
hotGoalTimer->Reset();
hotGoalTimer->Start();
gyro->Reset();
leftEnco->Reset();
rightEnco->Reset();
LEDLight->Set(Relay::kForward);
//Find out the type of autonomous we are using
int autonType = (int)SmartDashboard::GetNumber(NUM_BALL_AUTO);
if(autonType == 2)//Set the auton mode to two if we are doing a two ball auto
{
autonMode = TWO_BALL_AUTON;
autonStep = AUTON_TWO_WAIT_FOR_INTAKE;
}
else//Set the auton to one if the value on SD is set to 1 or another random value
{
autonMode = ONE_BALL_AUTON;
autonStep = AUTON_ONE_FIND_HOT;
}
//Bring the intake down
intake->DropIntake();
intakeDropTimer->Reset();
intakeDropTimer->Start();
while(IsAutonomous() && !IsDisabled())
{
rpi->Read();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "L: %f", leftEnco->GetDistance());
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "R: %f", rightEnco->GetDistance());
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "T: %f", hotGoalTimer->Get());
lcd->Printf(DriverStationLCD::kUser_Line4, 1, "i: %i", rpi->GetMissingPacketcount());
lcd->Printf(DriverStationLCD::kUser_Line5, 1, "%i", rpi->GetXPos());
lcd->Printf(DriverStationLCD::kUser_Line6, 1, "%i", rpi->GetYPos());
lcd->UpdateLCD();
LEDLight->Set(Relay::kForward);
if(autonMode == ONE_BALL_AUTON)
{
switch(autonStep)
{
case AUTON_ONE_FIND_HOT:
//Reload the catapult and find the hot goal
rpi->Read();
if(!goalFound)
{
//This is put into an if statement to protect against the
//rpi finding the hot goal and then losing it
goalFound = ((rpi->GetXPos() != RPI_ERROR_VALUE) ||
(rpi->GetYPos() != RPI_ERROR_VALUE));
}
//Wait for the goal to be hot and the intake to move down
if(((goalFound) || (hotGoalTimer->Get() >= 6.75)) && intakeDropTimer->Get() >= INTAKE_DROP_WAIT)
{
autonStep = AUTON_ONE_SHOOT;
catapult->StartRelease();
}
break;
case AUTON_ONE_SHOOT:
//Shoot the catapult
if(!catapult->ReleaseHold())
{
//Move on to the next step when the catapult is released
autonStep = AUTON_ONE_WAIT;
//Start the wait timer
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_ONE_WAIT:
if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
{
autonStep = AUTON_ONE_DRIVE_FORWARDS;
reloadTimer->Stop();
//Start reloading the catapult
catapult->StartLoad();
gyro->Reset();
}
break;
case AUTON_ONE_DRIVE_FORWARDS:
//Drive forwards into the alliance zone and reload the catapult
if((!GyroDrive(0, 0.75, 36)) && (!(bool)catapult->Load()))
{
autonStep = AUTON_END;
}
break;
case AUTON_END:
break;
}
}
else if(autonMode == TWO_BALL_AUTON)
{
switch(autonStep)
{
/*case AUTON_TWO_RELOAD:
//Determine if the hot goal is on the right
if((rpi->GetXPos() != RPI_ERROR_VALUE) &&
(rpi->GetYPos() != RPI_ERROR_VALUE))
{
rightSideHot = true;
}
//Reload the catapult
if(!((bool)catapult->Load()))
{
autonStep = AUTON_TWO_FIRST_SHOOT;
catapult->StartShoot();
}
if((goalFound))
{
//If the goal is found, the right side is hot and we can go to the next step
rightSideHot = true;
autonStep = AUTON_TWO_FIRST_SHOOT;
}
else if(hotGoalTimer->Get() >= 0.5)
{
//If the timer runs of, the right side is not hot and we can go to the next step
rightSideHot = false;
autonStep = AUTON_TWO_FIRST_TURN;
}
break;*/
case AUTON_TWO_WAIT_FOR_INTAKE:
//wait for the intake to drop down
if(intakeDropTimer->Get() >= INTAKE_DROP_WAIT)
{
autonStep = AUTON_TWO_FIRST_SHOOT;
catapult->StartShoot();
}
break;
case AUTON_TWO_FIRST_SHOOT:
if(catapult->GetLoadingState() == LOAD_RELEASE_TENSION)
{
intake->RollIn();
}
if(!((bool)catapult->Shoot()))
{
autonStep = AUTON_TWO_INTAKE;
intakeTimer->Reset();
intakeTimer->Start();
}
break;
case AUTON_TWO_INTAKE:
intake->RollIn();
if(intakeTimer->Get() >= 1.5)
{
intake->Stop();
intakeTimer->Stop();
autonStep = AUTON_TWO_SECOND_SHOOT;
catapult->StartRelease();
}
break;
case AUTON_TWO_SECOND_SHOOT:
if(!catapult->ReleaseHold())
{
autonStep = AUTON_TWO_WAIT;
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_TWO_WAIT:
if(reloadTimer->Get() >= 0.5)
{
reloadTimer->Stop();
leftEnco->Reset();
rightEnco->Reset();
catapult->StartLoad();
gyro->Reset();
autonStep = AUTON_TWO_DRIVE_FORWARDS;
}
break;
case AUTON_TWO_DRIVE_FORWARDS:
if((!GyroDrive(0, 0.9, 36)) && (!((bool)catapult->Load())))
{
autonStep = AUTON_END;
}
break;
/*case AUTON_TWO_FIRST_TURN:
//Turn to the left 5* if the right goal is not hot
if(!rightSideHot)
{
if(!GyroTurn(-5, 0.5))
{
autonStep = AUTON_TWO_FIRST_SHOOT;
}
}
else
{
autonStep = AUTON_TWO_FIRST_SHOOT;
}
break;
case AUTON_TWO_FIRST_SHOOT:
//Release the catapult to shoot
if(!catapult->ReleaseHold())
{
autonStep = AUTON_TWO_FIRST_WAIT;
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_TWO_FIRST_WAIT:
if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
{
autonStep = AUTON_TWO_SECOND_TURN;
catapult->StartLoad();
reloadTimer->Stop();
}
break;
case AUTON_TWO_SECOND_TURN:
//Turn the robot so it's facing forwards and reload the catapult
if((!GyroTurn(0, 0.5)) && (!(bool)catapult->Load()))
{
autonStep = AUTON_TWO_GET_SECOND_BALL;
}
break;
case AUTON_TWO_GET_SECOND_BALL:
//Start up the intake and drive back to pick up the second ball
intake->RollIn();
if(!GyroDrive(0, -0.5, -12))
{
autonStep = AUTON_TWO_THIRD_TURN;
leftEnco->Reset();
rightEnco->Reset();
}
break;
case AUTON_TWO_THIRD_TURN:
//If the right goal was originally hot, turn left
if(rightSideHot)
{
if(!GyroTurn(-5, 0.5))
{
autonStep = AUTON_TWO_SECOND_SHOOT;
}
}
else
{
autonStep = AUTON_TWO_SECOND_SHOOT;
}
break;
case AUTON_TWO_SECOND_SHOOT:
intake->Stop();
if(!catapult->ReleaseHold())
{
autonStep = AUTON_TWO_SECOND_WAIT;
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_TWO_SECOND_WAIT:
if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
{
autonStep = AUTON_TWO_DRIVE_FORWARDS;
catapult->StartLoad();
reloadTimer->Stop();
}
break;
case AUTON_TWO_DRIVE_FORWARDS:
if(!GyroDrive(0, 0.75, 60) && (!(bool)catapult->Load()))
{
autonStep = AUTON_END;
}
break;*/
case AUTON_END:
break;
}
}
}
}
/**
* Reset the count for the encoder object.
* Resets the count to zero.
*
* @param aSlot The digital module slot for the A Channel on the encoder
* @param aChannel The channel on the digital module for the A Channel of the encoder
* @param bSlot The digital module slot for the B Channel on the encoder
* @param bChannel The channel on the digital module for the B Channel of the encoder
*/
void ResetEncoder(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel)
{
Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel);
if (encoder != NULL)
encoder->Reset();
}
// During every loop intervel of the teleop period
void Robot::TeleopPeriodic() {
// Choose the teleop drive option
DriverControl(driveOption);
// A simple thing to test a motor
if (gamePad.GetRawButton(GAMEPAD_BUTTON_A) == true)
ballManipulator.GoUp();
else
ballManipulator.StopAll();
// Edits the gyro rate to account for drift
/*
editedGyroRate = gyro.GetRate();
if (gyro.GetRate() > GYRO_DRIFT_VALUE_MIN && gyro.GetRate() < GYRO_DRIFT_VALUE_MAX) {
editedGyroRate = 0;
} else {
editedGyroRate += GYRO_DRIFT_VALUE_AVERAGE;
}
*/
// Edits the gyro angle to account for drift
if (fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE)
editedGyroAngle = gyro.GetAngle();
else {
gyro.Reset();
editedGyroAngle = 0;
}
// Reset the buttons
for(int c=0; c<7; c++)
buttonDone[c] = false;
// To reset encoder data for the wheels
if (gamePad.GetRawButton(GAMEPAD_BUTTON_Y) == true) {
encoder1.Reset();
encoder2.Reset();
}
// Print gyro data
if (showGyro == true) {
SmartDashboard::PutNumber("Gyro Angle (Raw)", gyro.GetAngle()*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Angle (Edited)", editedGyroAngle*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR);
}
// Print out the encoder data
// Raw encoder data
// if (showEncoderRaw == true) {
SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());
// }
// The delta encoder change
if (showEncoderRate == true) {
SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate());
SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate());
}
// Print the encoder index
if (showEncoderIndex == true) {
SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex());
SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex());
}
}
/**
* Runs the motor from the output of a Joystick.
*/
void OperatorControl() {
LifterEncoder.StartLiveWindowMode();
LifterEncoder.Reset();
while (IsOperatorControl() && IsEnabled()) {
// Set the motor controller's output.
// This takes a number from -1 (100% speed in reverse) to +1 (100% speed forwards).
// lifterA_motor.Set(joy.GetY());
// lifterB_motor.Set(joy.GetY());
if ( stick.GetRawButton(1))
{
}
else if (stick.GetRawButton(2))
{
}
else if (stick.GetRawButton(3))
{
// Elevator down
if (BottomLimitSwitch.Get() == 0) {
lifterA_motor.Set(0.5);
lifterB_motor.Set(0.5);
} else {
lifterA_motor.Set(0);
lifterB_motor.Set(0);
}
}
else if (stick.GetRawButton(4))
{
}
else if (stick.GetRawButton(5))
{
// Elevator up
if (TopLimitSwitch.Get() == 0) {
lifterA_motor.Set(-0.5);
lifterB_motor.Set(-0.5);
} else {
lifterA_motor.Set(0);
lifterB_motor.Set(0);
}
}
else if (stick.GetRawButton(6))
{
}
else if (stick.GetRawButton(7))
{
}
else if (stick.GetRawButton(8))
{
}
else if (stick.GetRawButton(9))
{
}
else if (stick.GetRawButton(10))
{
}
else if (stick.GetRawButton(11))
{
}
else if (stick.GetRawButton(12))
{
}
else
{
lifterA_motor.Set(0.0);
lifterB_motor.Set(0.0);
}
if(BottomLimitSwitch.Get()==true)
{
LifterEncoder.Reset();
}
// Send some stuff to the dashboard
SmartDashboard::PutBoolean("Top Limit Switch", TopLimitSwitch.Get());
SmartDashboard::PutBoolean("Bottom Limit Switch", BottomLimitSwitch.Get());
SmartDashboard::PutNumber("Encoder Position",LifterEncoder.GetRaw());
Wait(kUpdatePeriod); // Wait 5ms for the next update.
}
}
