RobotDemo()
{
//Initialize the objects for the drive train
myRobot = new RobotDrive(1, 2);
leftEnco = new Encoder(LEFT_ENCO_PORT_1, LEFT_ENCO_PORT_2);
rightEnco = new Encoder(RIGHT_ENCO_PORT_1, RIGHT_ENCO_PORT_2);
leftEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
leftEnco->Start();
rightEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
rightEnco->Start();
leftStick = new Joystick(1);
rightStick = new Joystick(2);
//Initialize the gyro
gyro = new Gyro(GYRO_PORT);
gyro->Reset();
//Initialize the manipulators
intake = new Intake(INTAKE_ROLLER_PORT, BALL_SENSOR_PORT, LEFT_SERVO_PORT, RIGHT_SERVO_PORT);
catapult = new Catapult(LOADING_MOTOR_PORT, HOLDING_MOTOR_PORT, LOADED_LIMIT_1_PORT,
LOADED_LIMIT_2_PORT, HOLDING_LIMIT_PORT);
//Initialize the objects needed for camera tracking
rpi = new RaspberryPi("17140");
LEDLight = new Relay(1);
LEDLight->Set(Relay::kForward);
//Set the autonomous modes
autonMode = ONE_BALL_AUTON;
autonStep = AUTON_ONE_SHOOT;
//Initialize the lcd
lcd = DriverStationLCD::GetInstance();
}
BuiltinDefaultCode() {
m_robotDrive = new RobotDrive (m_lDrive, m_rDrive);
m_driver = new Joystick (1);
m_operator = new Joystick (2);
m_rDrive = new Victor (1);
m_lDrive = new Victor (2);
m_climber = new Victor (3);
m_plate1 = new Victor (8);
m_plate2 = new Victor (10);
m_launcher1 = new Victor (4);
m_launcher2 = new Victor (5);
m_launcher3 = new Victor (6);
m_feeder = new Victor (7);
m_ratchet = new Relay (1);
m_left = new Encoder (1,2,true);
m_left->SetDistancePerPulse(1);
m_left->SetMaxPeriod(1.0);
m_left->Start();
m_right = new Encoder (3,4,false);
m_right->SetDistancePerPulse(1);
m_right->SetMaxPeriod(1.0);
m_right->Start();
m_plateh = new AnalogChannel (1);
m_climberp = new AnalogChannel (2);
m_dsLCD = DriverStationLCD::GetInstance();
}
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");
}
void TeleopInit(void) {
printf("Robot teleop initializing...\n");
GetWatchdog().Feed();
compressor->Start();
kicker->SafeReset();
leftDrivetrainEncoder->Start();
rightDrivetrainEncoder->Start();
printf("Robot teleop initialization complete.\n");
}
// 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();
}
}
void OperatorControl(void)
{
NetTest();
return;
myRobot.SetSafetyEnabled(true);
digEncoder.Start();
const double ppsTOrpm = 60.0/250.0; //Convert from Pos per Second to Rotations per Minute by multiplication
// (See the second number on the back of the encoder to replace 250 for different encoders)
const float VoltsToIn = 41.0; // Convert from volts to cm by multiplication (volts from ultrasonic).
// This value worked for distances between 1' and 10'.
while (IsOperatorControl())
{
if (stick.GetRawButton(4)) {
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1);
}
else if (stick.GetRawButton(5))
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1);
}
else
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
}
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm));
SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch);
SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage());
Wait(0.1);
}
digEncoder.Stop();
}
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();
}
/**
* Start the encoder counting.
*
* @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 StartEncoder(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel)
{
Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel);
if (encoder != NULL)
{
encoder->Start();
}
}
// 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;
}
PnumaticArmTest (void):
stick(1)
,up(7,7)
,down(7,8)
,enc(1,2)
,pid(.045,.00000,.16)
,low(.05)
{
enc.SetReverseDirection(true);
enc.Start();
}
void OperatorControl(void)
{
digEncoder.Start();
const double ppsTOrpm = 60.0/250.0; //This converts from Pos per Second to Rotations per Minute (See second number on back of encoder to replace 250 if you need it)
while (IsOperatorControl())
{
SmartDashboard::PutNumber("Digital Encoder RPM", digEncoder.GetRate()*ppsTOrpm);
Wait(0.1);
}
digEncoder.Stop();
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
encoder.Start();
while (IsEnabled())
{
float controlv = control.GetVoltage();
motor.Set(controlv/5); //get's voltage from control and divides it by 5
double rate = encoder.GetRate();
double distance = encoder.GetDistance();
printf ("%f %f \n", rate, distance);
}
}
BuiltinDefaultCode() {
//Initialze drive controllers
m_rDrive1 = new Talon (1);
m_rDrive2 = new Talon (2);
m_lDrive1 = new Talon (3);
m_lDrive2 = new Talon (4);
//Initialize ramrod motor
m_ramMotor = new Talon (5);
//Initialize Arm
m_arm = new ArmWrapper (7, 6, 5, 6, 10);
m_arm->StartPID(0.0, 0.0, 0.0);
//initialize bGrabber motor
m_roller = new Talon (8);
//Initialize ramrod servo
m_ramServo = new Servo (9);
//Initialize drive wrappers
m_rDrive = new DriveWrapper (m_rDrive1, m_rDrive2);
m_lDrive = new DriveWrapper (m_lDrive1, m_lDrive2);
//Initialize robot drive
m_robotDrive = new RobotDrive (m_lDrive, m_rDrive);
//Initialize ramrod encoder
m_ramEncoder = new Encoder (7,8,false);
m_ramEncoder->SetDistancePerPulse(1);
m_ramEncoder->SetMaxPeriod(1.0);
m_ramEncoder->Start();
//Initialize Compressor
m_compressor = new Compressor(9, 1);
//shifters
m_shifters = new Solenoid(1);
//Initialize bGrabber Solenoids
m_bArm = new Solenoid (2);
m_catch = new Solenoid (3);
//Initialize joysticks
m_driver = new JoystickWrapper (1);
m_operator = new JoystickWrapper (2);
//Grab driver station object
m_dsLCD = DriverStationLCD::GetInstance();
}
RobotDemo(void):
leftDriveMotor(LEFT_DRIVE_PWM),
rightDriveMotor(RIGHT_DRIVE_PWM),
myRobot(&leftDriveMotor, &rightDriveMotor), // these must be initialized in the same order
stick(1), // as they are declared above.
stick2(2),
gamepad(3),
collectorMotor(PICKUP_PWM),
indexerMotor(INDEX_PWM),
shooterMotor(SHOOTER_PWM),
armMotor (ARM_PWM),
leftDriveEncoder(LEFT_DRIVE_ENC_A, LEFT_DRIVE_ENC_B),
shifter(SHIFTER_A,SHIFTER_B),
greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
potentiometer(ARM_ROTATION_POT),
indexSwitch(INDEXER_SW),
greenClawLockSwitch(CLAW_1_LOCK_SENSOR),
yellowClawLockSwitch(CLAW_2_LOCK_SENSOR),
compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE),
jogTimer(),
shooterTimer()
{
m_collectorMotorRunning = false;
m_shooterMotorRunning = false;
m_jogTimerRunning = false;
m_shiftCount = MAX_SHIFTS;
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);
dsLCD->UpdateLCD();
myRobot.SetExpiration(0.1);
shifter.Set(DoubleSolenoid::kReverse);
leftDriveEncoder.SetDistancePerPulse(DRIVE_ENCODER_DISTANCE_PER_PULSE);
leftDriveEncoder.SetMaxPeriod(1.0);
leftDriveEncoder.SetReverseDirection(true); // change to true if necessary
leftDriveEncoder.Start();
}
/**
* 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 OperatorControl(void)
{
//Open the file
//setup watchdog
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(0.3);
theEncoder->Start();
//theLift->Enable();
#if RATE
#endif
float maxHeight = 0;
float maxRate = 0;
while (IsOperatorControl())
{
printf("Distance %f rate %f maxHeight %f maxRate %f\n",
theEncoder->GetDistance(),
theEncoder->GetRate(),
maxHeight,
maxRate);
if(writing)
{
write();
}else if (!closed){
printf("CLOSED!");
myfile.close();
closed = true;
}
Wait(.01);
if(theEncoder->GetDistance() > maxHeight) maxHeight = theEncoder->GetDistance();
if(theEncoder->GetRate() > maxRate) maxRate = theEncoder->GetRate();
//check whether we should switch modes
/*if (decelToStop(theEncoder->GetRate(),SETPOINT_METERS - theEncoder->GetDistance()) <= MAXDECEL && !decel)
{
decelRate = decelToStop(theEncoder->GetRate(),SETPOINT_METERS - theEncoder->GetDistance());
cout<<"DecelRate: "<<decelRate<<endl;
decel = true;
printf("BEGINNING DECELERATION\n");
initTime = GetTime();
}*/
/*if(theEncoder->GetDistance() > 1.0)
{
theJag->Set(0.0);
writing = false;
//theLift->SetPID(.3,0,0);
//theLift->SetSetpoint(0.0);
done = true;
}else if(!done){
theJag->Set(1.0);
//theLift->SetSetpoint(1.5);
}
*/
if(theEncoder->GetDistance() >= SETPOINT_METERS)
{
done = true;
theJag->Set(0.0);
writing = false;
}else if (!done){
theJag->Set(1.0);
}
rateSetpoint = vFinal + (decelRate * (GetTime() / pow(10,9)));
#if 0
if(theEncoder->GetDistance() >= SETPOINT_METERS)
{
theJag->Set(0.0);
writing = false;
}else if(decel){
#if RATE
//theLift->SetSetpoint(rateSetpoint);
theJag->Set(0.0);
#else
theLift->SetSetpoint(SETPOINT_METERS);
#endif
}else{
theJag->Set(1.0);
}
#if RATE
#else
theLift->SetSetpoint(SETPOINT_METERS);
#endif
#endif
GetWatchdog().Feed();
}
}
void Autonomous(void)
{
#if 1
/*int autoMode = 0;
autoMode |= bcd1->Get();
autoMode <<= 1;
autoMode |= bcd2->Get();
autoMode <<= 1;
autoMode |= bcd3->Get()
;*/
//double ignoreLineStart = 0;
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(0.2);
float liftSP = 0;
PIDLift->SetTolerance(10);
PIDLift->SetContinuous(false);
PIDLift->SetOutputRange(-0.75, 1); //BUGBUG
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
PIDLift->Enable();
PIDGrip->SetSetpoint(0);
PIDGrip->Enable();
stopCount = 0;
float reduction;
int counts = 0;
leftEncoder->Start();
rightEncoder->Start();
leftEncoder->Reset();
rightEncoder->Reset();
liftEncoder->Start();
liftEncoder->Reset();
leftEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
rightEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
double avgEncoderCount;
float leftSpeed = .2, rightSpeed = .2;
short int lsL,lsM,lsR;
lineFollowDone = false;
counts = 0;
//int fancyWaiter = 0;
int popstage = 0;
goPop = false;
double backStart = 0;
double backTime = 0;
double popStart = 0;
double popTime = 0;
bool backDone = false;
miniDep->Set(miniDep->kForward);
int liftCount = 0;
bool disengageBrake = false;
float lastLiftSP = 0;
gripOpen1->Set(true);
gripOpen2->Set(false);
gripLatch1->Set(true);
gripLatch2->Set(false);
while(IsAutonomous())
{
if(!(counts % 100))printf("%2.2f \n",getDistance());
if(backStart) backTime = GetClock();
if(popStart) popTime = GetClock();
//if(!ignoreLineStart)ignoreLineStart = GetClock();
if(!compSwitch->Get()) compressor->Set(compressor->kReverse);
else compressor->Set(compressor->kOff);
if(counts%3 == 0)
{
leftValue = lsLeft->Get();
middleValue = lsMiddle->Get();
rightValue = lsRight->Get();
}
counts++;
GetWatchdog().Feed();
//avgEncoderCount = (leftEncoder->Get() + rightEncoder->Get())/2;
//myRobot.SetLeftRightMotorOutputs(.2,.2);
//All three/five autonomous programs will do the same thing up until 87 inches from the wall
if (counts % 100 == 0){//TESTING
if(lsLeft->Get()){
lsL = 1;
}else{
lsL = 0;
}
if(lsRight->Get()){
lsR = 1;
}else{
lsR = 0;
}
if(lsMiddle->Get()){
lsM = 1;
}else{
lsM = 0;
}
//printf("Encoder: %2.2f \n", (float)avgEncoderCount);
//printf("Distance: %2.2f SensorL:%1d SensorM:%1d SensorR:%1d\n",getDistance(),lsL,lsM,lsR);//TESTING
}
#if FOLLOWLINE
/*if(GetClock() - ignoreLineStart <= 0.5)
{
frontLeftMotor->Set(-.4);
rearLeftMotor->Set(-.4);
frontRightMotor->Set(.4);
rearRightMotor->Set(.4);
}
else */if (false){//(avgEncoderCount <= SECONDBREAKDISTANCE){
followLine();
}
#else
if (getDistance() > 24){
frontLeftMotor->Set(-leftSpeed);
rearLeftMotor->Set(-leftSpeed);
frontRightMotor->Set(rightSpeed);
rearRightMotor->Set(rightSpeed);
if(leftEncoder->Get() > rightEncoder->Get() && leftSpeed == .2){
rightSpeed += .03;
}else if(leftEncoder->Get() >rightEncoder->Get() && leftSpeed > .2){
leftSpeed -= .03;
}else if(leftEncoder->Get() < rightEncoder->Get() && rightSpeed == .2){
leftSpeed += .03;
}else if(leftEncoder->Get() < rightEncoder->Get() && rightSpeed > .2){
rightSpeed -= .03;
}
}
#endif
else{
if(counts % 100 == 0) {printf("DISTANCE: %2.2f\n",getDistance());}
switch(FOLLOWLINE)
{
case STRAIGHTLINEMODE: //Straight line. Scores on middle column of left or right grid.
//if(lineFollowDone && !(counts %50)) printf("Lift error: %f \n", PIDLift->GetError());
lastLiftSP = liftSP;
if(!lineFollowDone)
{
followLine();
}
else if(!PIDLift->GetSetpoint() && !popstage && !backStart)
{
//if(counts % 50 == 0)printf("Go backward\n");
frontLeftMotor->Set(.3);
rearLeftMotor->Set(.3);
frontRightMotor->Set(-.3);
rearRightMotor->Set(-.3);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2 + 0.025;
//fancyWaiter = counts;
backStart = GetClock();
printf("Setpoint set setpoint set setpoint set \n");
/*
if(leftValue && middleValue && rightValue)
{
printf("Stopped 2nd time\n");
goPop = true;
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
}
#if 1 //if we've backed up for half a second and we're not popping
else if((backTime - backStart) > 0.65 && !backDone)
{
printf("Stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!ONE\n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
backDone = true;
//Wait(.01);
//lift->brakeOff();
//fancyWaiter = counts;
//printf("Fancy waiter set Fancy waiter set Fancy waiter set");
}
/*
else if(lift->getPosition() < LIFTHIGH2)
{
//Move teh lifts
//if(counts % 100 == 0) printf("Stopping because lineFollowDone == true\n");
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
//if the lift is at the top and we're done backing up
else if(PIDLift->GetSetpoint() && fabs(liftSP - lift->getPosition()) < 0.015 && backDone)
{
if(!popStart) popStart = GetClock();
if(popstage == 0)
{
//lift->brakeOn();
//PIDLift->SetSetpoint(lift->getPosition());
popstage++;
printf("BRAKE BRAKE BRAKE BRAKE BRAKE \n");
}
else if(popstage == 1 && popTime - popStart > 0.1)
{
#if !(INNERGRIP)
gripOpen1->Set(true);
gripOpen2->Set(false);
#else
gripOpen1->Set(false);
gripOpen2->Set(true);
#endif
popstage++;
printf("OPEN OPEN OPEN OPEN OPEN \n");
}
else if(popstage == 2 && popTime - popStart > 0.35)
{
gripPop1->Set(true);
gripPop2->Set(false);
popstage++;
printf("POP POP POP POP POP POP POP \n");
}
else if(popstage == 3 && popTime - popStart > 1.35)
{
gripPop1->Set(false);
gripPop2->Set(true);
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(-.2);
rearRightMotor->Set(-.2);
popstage++;
printf("UNPOP UNPOP UNPOP UNPOP UNPOP \n");
}
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(0);
liftSP = 0;
}
/*
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(min(0.3, 0.25*(popTime - popStart - 1.85)));
rearLeftMotor->Set(min(0.3, 0.25*(popTime - popStart - 1.85)));
frontRightMotor->Set(max(-0.3, -0.25*(popTime - popStart - 1.85)));
rearRightMotor->Set(max(-0.3, -0.25*(popTime - popStart - 1.85)));
//PIDLift->SetSetpoint(0);
liftSP = 0;
popstage++;
}
else if(popstage == 5 && popTime - popStart > 4.85)
{
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
}
*/
}
//Start tele-op lift code
if(!lift->isBraking() && !disengageBrake)
{
PIDLift->SetSetpoint(liftSP);
if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1)
{
//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1);
}
else PIDLift->SetOutputRange(-0.75, 1);
}
if(lift->isBraking() && lastLiftSP != liftSP)
{
PIDLift->SetSetpoint(lift->getPosition() + 0.04);
PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
lift->brakeOff();
disengageBrake = true;
if(!liftCount) liftCount = counts;
}
//set brake (because near)
if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
{
if(liftCount == 0) liftCount = counts;
if(counts - liftCount > 1000)
{
PIDLift->Reset();
liftMotor1->Set(LIFTNEUTRALPOWER);
liftMotor2->Set(LIFTNEUTRALPOWER);
Wait(0.02);
lift->brakeOn();
Wait(0.02);
liftMotor1->Set(0.0);
liftMotor2->Set(0.0);
PIDLift->Enable();
//PIDLift->SetSetpoint(lift->getPosition());
liftCount = 0;
}
if(counts%3000) printf("Braking/Not PID \n");
}
if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
{
disengageBrake = false;
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
liftCount = 0;
}
//End tele-op lift code
#endif
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
case NOLINEMODE: //Fork turn left. Scores on far right column of left grid.
lineFollowDone = true;
if(!lineFollowDone){}
else if(!PIDLift->GetSetpoint() && !popstage && !backStart)
{
//if(counts % 50 == 0)printf("Go backward\n");
frontLeftMotor->Set(.3);
rearLeftMotor->Set(.3);
frontRightMotor->Set(-.3);
rearRightMotor->Set(-.3);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
//fancyWaiter = counts;
backStart = GetClock();
printf("Setpoint set setpoint set setpoint set \n");
/*
if(leftValue && middleValue && rightValue)
{
printf("Stopped 2nd time\n");
goPop = true;
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
}
#if 1 //if we've backed up for half a second and we're not popping
else if((backTime - backStart) > 0.65 && !backDone)
{
printf("Stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!ONE\n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
backDone = true;
//Wait(.01);
//lift->brakeOff();
//fancyWaiter = counts;
//printf("Fancy waiter set Fancy waiter set Fancy waiter set");
}
/*
else if(lift->getPosition() < LIFTHIGH2)
{
//Move teh lifts
//if(counts % 100 == 0) printf("Stopping because lineFollowDone == true\n");
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
//if the lift is at the top and we're done backing up
else if(PIDLift->GetSetpoint() && fabs(liftSP - lift->getPosition()) < 0.015 && backDone)
{
if(!popStart) popStart = GetClock();
if(popstage == 0)
{
//lift->brakeOn();
//PIDLift->SetSetpoint(lift->getPosition());
popstage++;
printf("BRAKE BRAKE BRAKE BRAKE BRAKE \n");
}
else if(popstage == 1 && popTime - popStart > 0.1)
{
#if !(INNERGRIP)
gripOpen1->Set(true);
gripOpen2->Set(false);
#else
gripOpen1->Set(false);
gripOpen2->Set(true);
#endif
popstage++;
printf("OPEN OPEN OPEN OPEN OPEN \n");
}
else if(popstage == 2 && popTime - popStart > 0.35)
{
gripPop1->Set(true);
gripPop2->Set(false);
popstage++;
printf("POP POP POP POP POP POP POP \n");
}
else if(popstage == 3 && popTime - popStart > 1.35)
{
gripPop1->Set(false);
gripPop2->Set(true);
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(-.2);
rearRightMotor->Set(-.2);
popstage++;
printf("UNPOP UNPOP UNPOP UNPOP UNPOP \n");
}
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(0);
liftSP = 0;
}
}
//Start tele-op lift code
if(!lift->isBraking() && !disengageBrake)
{
PIDLift->SetSetpoint(liftSP);
if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1) //BUGBUG
{
//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1.); //BUGBUG
}
else PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
}
if(lift->isBraking() && lastLiftSP != liftSP)
{
PIDLift->SetSetpoint(lift->getPosition() + 0.04);
PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
lift->brakeOff();
disengageBrake = true;
if(!liftCount) liftCount = counts;
}
//set brake (because near)
if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
{
if(liftCount == 0) liftCount = counts;
if(counts - liftCount > 1000)
{
PIDLift->Reset();
liftMotor1->Set(LIFTNEUTRALPOWER);
liftMotor2->Set(LIFTNEUTRALPOWER);
Wait(0.02);
lift->brakeOn();
Wait(0.02);
liftMotor1->Set(0.0);
liftMotor2->Set(0.0);
PIDLift->Enable();
//PIDLift->SetSetpoint(lift->getPosition());
liftCount = 0;
}
if(counts%3000) printf("Braking/Not PID \n");
}
if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
{
disengageBrake = false;
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
liftCount = 0;
}
//End tele-op lift code
#endif
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
case FORKRIGHTMODE://Fork turn right. Scores on far left column of right grid.
if(leftEncoder->GetDistance() <= rightEncoder->GetDistance() + 6)
{
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
}
else if(getDistance() >= SCOREDISTANCE)
{
followLine();
}
//score here
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
}
}
}
/*frontRightMotor->Set(0);
rearRightMotor->Set(0);
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);*/
Wait(.02);
#endif
}
void 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
}
}
RobotDemo()
{
#if YEAR_2013
drive = new RobotDrive(left_drive_motor_A_PWM, left_drive_motor_B_PWM,
right_drive_motor_A_PWM, right_drive_motor_B_PWM);
drive->SetExpiration(15);
drive->SetSafetyEnabled(false);
#endif
drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
//Joystick
//ds = new DriverStation();
drive_stick_sec = new Joystick(right_stick);
drive_stick_prim = new Joystick(left_stick);
operator_stick = new Joystick(operator_joystick);
//Motors
#if JAGUAR_SWITCH
shooter_motor_front = new Jaguar(shooter_front_motor);
shooter_motor_back = new Jaguar(shooter_back_motor);
#else
shooter_motor_front = new Talon(shooter_front_motor);
shooter_motor_back = new Talon(shooter_back_motor);
#endif
#if DUMB_DRIVE_CODE
left_drive_motor_A = new Victor(left_drive_motor_A_PWM);
left_drive_motor_B = new Victor(left_drive_motor_B_PWM);
right_drive_motor_A = new Victor(right_drive_motor_A_PWM);
right_drive_motor_B = new Victor(right_drive_motor_B_PWM);
#endif
#if YEAR_2013
climbing_motor = new Victor(climbing_motor_PWM);
#endif
//limit switches
top_claw_limit_switch = new DigitalInput(top_claw_limit_switch_port);
bottom_claw_limit_switch = new DigitalInput(
bottom_claw_limit_switch_port);
//Encoders
front_shooter_encoder = new Encoder(shooter_motor_front_encoder_A_port,
shooter_motor_front_encoder_B_port, false);
back_shooter_encoder = new Encoder(shooter_motor_back_encoder_A_port,
shooter_motor_back_encoder_B_port, false);
//solenoids
shooter_angle_1 = new Solenoid(SHOOTER_ANGLE_SOLENOID_1);
shooter_angle_2 = new Solenoid(SHOOTER_ANGLE_SOLENOID_2);
shooter_fire_piston_A = new Solenoid(shooter_fire_piston_solenoid_A);
shooter_fire_piston_B = new Solenoid(shooter_fire_piston_solenoid_B);
//Timers
shooter_piston_timer = new Timer();
VC_timer = new Timer();
loop_time_measure_timer = new Timer();
shooter_reset = new Timer();
pid_code_timer = new Timer();
autonomous_timer = new Timer();
error_timer = new Timer();
retraction_timer = new Timer();
override_timer = new Timer();
stabilizing_timer = new Timer();
shooter_stop_timer = new Timer();
//Compressor
compressor1 = new Compressor(PRESSURE_SWITCH, compressor_enable);
#if CAMERA
//Camera
camera = &(AxisCamera::GetInstance(AxisCameraIpAddr));
camera->WriteResolution(AxisCamera::kResolution_640x480);
AxisCamera::GetInstance();
#endif
//Function starter
compressor1 ->Start();
//float RPS;
//PIDController pid1 (0.1 , 0.001 ,0.0 , &RPS , test_motor );
loop_time_measure_timer ->Start();
VC_timer ->Start();
//Variable Initialization
arcadedrive = true;
test_encoder_value = 0;
total_test_encoder_value = 0;
old_RPS = 0;
new_RPS = 0;
second_count = 0;
set_speed = 0.5;
cycle_counter = 0;
//test_motor ->Set(set_speed);
// float RPS;
//int old_encoder_value;
//double old_timer;
speed2 = 0.0;
test_speed = 0.3;
button = false;
pickup_on = false;
switch_on = false;
kicker_piston_on = false;
kicker_button_on = false;
average_counter = 0;
counter = 1;
number = 1;
additive_error = 0;
prev_error_front = 0;
prev_desired_RPS = 0;
ROC = 0;
claw_ = false;
claw_go_down = false;
constant_ = false;
constant_desired_RPS = false;
divided = 0;
//RPS_encoder_1 = new Encoder(9, 10);// for 2012 robot
shooter_motor_back_RPS = shooter_motor_back->Get();
//RPS_encoder_1 -> SetDistancePerPulse(1 / 250);//TODO figure out how this works
first_press = true;
test_RPS = false;
desired_RPS_control = 0.0;
slow_control = 0;
//RPS_encoder_1 ->Start();
pid_code_timer ->Start();
front_shooter_encoder->Start();
back_shooter_encoder->Start();
autonomous_timer ->Start();
error_timer ->Start();
retraction_timer->Start();
stabilizing_timer->Start();
override_timer->Start();
}
/**
* 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);
}
}
}
