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();
}
}
float RawControl::averageEncoders()
{
float traveled;
traveled=(((abs(wheelEncoderFR->Get()))+(abs(wheelEncoderFL->Get()))+(abs(wheelEncoderBR->Get()))+(abs(wheelEncoderBL->Get())))/4);
traveled/=250;
traveled=traveled * 8 * 3.14;
traveled=fabs(traveled);
return traveled;
}
//void StartAutomaticCapture(std::shared_ptr<USBCamera>cam0);
void TeleopPeriodic() {
// ui.GetData(&wui);
// m_tank.Drive(wui.LeftSpeed, wui.RightSpeed);
//
// m_shooter.Rotate(wui.RotateSpeed*3); //70 degrees per second at full value
// m_shooter.Lift(wui.LiftSpeed*1.193); //4 seconds for 180 degree revolution
// if(wui.SpinUp) {
// m_shooter.Spinup(1);
// }
// if(wui.Shoot) {
// m_shooter.Shoot();
// }
// if(wui.Pickup) {
// m_shooter.Pickup();
// }
//
// m_suspension.SetFrontLeft(wui.DropFL);
// m_suspension.SetBackLeft(wui.DropBL);
// m_suspension.SetFrontRight(wui.DropFR);
// m_suspension.SetBackRight(wui.DropBR);
// m_leddar.GetDetections();
// m_shooter.Update();
//float RTrigger = m_lStick->GetRawAxis(3);
//float LTrigger = m_lStick->GetRawAxis(2);
//if (m_PWMTalonLeftFrontTop == .5)
//if (abs(RTrigger) < 0.2)
//RTrigger = 0;
//if (abs(LTrigger) < 0.2)
//LTrigger = 0;
float leftSpeed = m_lStick->GetRawAxis(1);
float rightSpeed = m_lStick->GetRawAxis(5);
if (abs(leftSpeed) < 0.2)
leftSpeed = 0;
if (abs(rightSpeed) < 0.2)
rightSpeed = 0;
//float LTrigger = m_lStick->GetRawAxis(3);
//float RTrigger = m_lStick->GetRawAxis(2);
SmartDashboard::PutNumber("Left Stick", leftSpeed);
SmartDashboard::PutNumber("Right Stick", rightSpeed);
//SmartDashboard::PutNumber("L Trigger", LTrigger);
//SmartDashboard::PutNumber("R Trigger", RTrigger);
SmartDashboard::PutNumber("Left Encoder", leftEncoder->Get());
SmartDashboard::PutNumber("Right Encoder", rightEncoder->Get());
drive->TankDrive(leftSpeed, rightSpeed, true);
//drive->TankDrive(RTrigger, LTrigger, true);
LEFTDRIVE1->Set(leftSpeed);
LEFTDRIVE2->Set(leftSpeed);
RIGHTDRIVE1->Set(rightSpeed);
RIGHTDRIVE2->Set(rightSpeed);
//m_PWMTalonLeftFrontTop->Set(RTrigger);
//m_PWMTalonRightFrontTop->Set(RTrigger);
//m_PWMTalonRightRearTop->Set(LTrigger);
//m_PWMTalonLeftRearTop->Set(LTrigger);
}
// Called repeatedly when this Command is scheduled to run
void RPMHoldingCommand::Execute() {
double countsPerRevolution = 360;
double currentTime = timer->Get();
double timeDifference = currentTime - lastTime;
lastTime = currentTime;
Encoder* motorEncoder = prototypingsubsystem->GetMotorEncoder();
int currentCount = motorEncoder->Get();
int countDifference = currentCount - lastCount;
double rawRPM = countDifference * (60.0 / countsPerRevolution) / timeDifference;
double rpm = GetRPM(rawRPM);
printf("rawRPM %f rpm %f\n", rawRPM, rpm);
rpmSource->inputRPM(rpm);
//rpmSource->inputRPM(rpm);
printf("setpoint %f RPM %f result %f error %f \n", controller->GetSetpoint(), rpm, controller->Get(), controller->GetError());
SmartDashboard *sd = oi->getSmartDashboard();
oi->DisplayEncoder(motorEncoder);
oi->DisplayPIDController(controller);
sd->PutDouble("Count Difference", countDifference);
sd->PutDouble("Current count", currentCount);
sd->PutDouble("Time difference", timeDifference);
sd->PutDouble("Calculated RPM", rpm);
sd->PutDouble("Motor speed", prototypingsubsystem->GetMotor()->Get());
sd->PutDouble("Counts per revolution", countsPerRevolution);
lastCount = currentCount;
}
/**
* Gets the current count.
* Returns the current count on the Encoder.
* This method compensates for the decoding type.
*
* @deprecated Use GetEncoderDistance() in favor of this method. This returns unscaled pulses and GetDistance() scales using value from SetEncoderDistancePerPulse().
*
* @return Current count from the Encoder.
*
* @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
*/
INT32 GetEncoder(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel)
{
Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel);
if (encoder != NULL)
return encoder->Get();
else
return 0;
}
void TeleopPeriodic()
{
drive_mode_t new_mode = drive_mode_chooser.GetSelected();
SmartDashboard::PutString("current mode", new_mode == TANK_DRIVE ? "Tank" : "Arcade");
if (new_mode != drive_mode)
SetDriveMode(new_mode);
if (drive_mode == TANK_DRIVE) {
left_speed = accel(left_speed, pilot->LeftY(), TICKS_TO_ACCEL);
right_speed = accel(right_speed, pilot->RightY(), TICKS_TO_ACCEL);
drive->TankDrive(left_speed, right_speed);
}
else {
rot_speed = accel(rot_speed, pilot->RightX(), TICKS_TO_ACCEL);
SmartDashboard::PutNumber("rotation speed", rot_speed);
rot_speed = pilot->RightX();
move_speed = accel(move_speed, pilot->LeftY(), TICKS_TO_ACCEL);
drive->ArcadeDrive(move_speed * MOVE_SPEED_LIMIT, -rot_speed * MOVE_SPEED_LIMIT, false);
}
SmartDashboard::PutBoolean("clamp open", clamp->isOpen());
SmartDashboard::PutBoolean("sword in", clamp->isSwordIn());
SmartDashboard::PutData("gyro", gyro);
// for (uint8 i = 0; i <= 15; ++i)
// SmartDashboard::PutNumber(std::string("current #") + std::to_string(i), pdp->GetCurrent(i));
SmartDashboard::PutNumber("Current", pdp->GetTotalCurrent());
if (pilot->ButtonState(GamepadF310::BUTTON_A)) {
clamp->open();
}
else if (pilot->ButtonState(GamepadF310::BUTTON_B)){
clamp->close();
}
clamp->update();
SmartDashboard::PutNumber("accelerometer Z", acceler->GetZ());
SmartDashboard::PutNumber("Encoder", encoder->Get());
flywheel->Set(pilot->RightTrigger());
if (pilot->LeftTrigger() != 0)
flywheel->Set(-pilot->LeftTrigger());
SmartDashboard::PutNumber("Left Trigger:", pilot->LeftTrigger());
if (pilot->ButtonState(GamepadF310::BUTTON_X)) {
cameraFeeds-> changeCam(cameraFeeds->kBtCamFront);
}
if (pilot->ButtonState(GamepadF310::BUTTON_Y)){
cameraFeeds-> changeCam(cameraFeeds->kBtCamBack);
}
cameraFeeds->run();
}
void Claw(double joy) {
if(joy < 10) joy = 10;
if(joy > 230) joy = 230;
int location = EncClaw.Get();
double speed = PIDClaw(joy, location);
//if(location < 15) speed *= .2;
if(speed > .32) speed = .32;
if(speed < -.32) speed = -.32;
if(speed < .1 && speed > -.1) speed = 0;
arm2->Set(speed,2);
}
inline void lifterPositionTaskFunc(uint32_t joystickPtr, uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t pdpPtr, uint32_t heightPtr, uint32_t isLiftingPtr ...) {//uint is a pointer and not an integer
double *height = (double *) heightPtr;//initializes double
Joystick *joystick = (Joystick *) joystickPtr;
Talon *liftTalon = (Talon *) liftTalonPtr;
Encoder *liftEncoder = (Encoder *) liftEncoderPtr;
Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr;
Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr;
PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
bool *isLifting = (bool *) isLiftingPtr;
*isLifting = true;//tells robot.cpp that thread is running
if (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height) {//checks to see if encoder is higher than it's supposed to be
if (liftLowerLimit->Get() == false) {//starts to spin motor to pass startup current
liftTalon->Set(1);//move down
Wait(Constants::liftDelay);
}
while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftLowerLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too high and hasn't hit a limit switch or been cancelled
SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder in SmartDashboard
liftTalon->Set(.7);//move down
}
}
else {
if (liftUpperLimit->Get() == false) {//starts to spin motor to pass startup current
liftTalon->Set(-1);//move up
Wait(Constants::liftDelay);
}
while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftUpperLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too low and hasn't hit a limit switch or been cancelled
SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder on SmartDashboard
liftTalon->Set(-1);//move up
}
}
liftTalon->Set(0);//stop
*isLifting = false;//tells robot.cpp that thread is finished
}
inline void lifterBrakeTaskFunc(uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t isBrakingPtr ...) {//uint is a pointer and not an integer
Talon *liftTalon = (Talon *) liftTalonPtr;
Encoder *liftEncoder = (Encoder *) liftEncoderPtr;
Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr;
Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr;
bool *isBraking = (bool *) isBrakingPtr;
PIDController pid(Constants::liftBrakeP, Constants::liftBrakeI, Constants::liftBrakeD, liftEncoder, liftTalon);
Timer timer;
//TODO enable and test out brake
if (!Constants::liftBrakeIsEnabled) {
return;
}
timer.Start();
while (timer.Get() < Constants::liftBrakeUpTime) {
liftTalon->Set(Constants::liftBrakeUpPower);
}
liftTalon->Set(0);
pid.Enable();
pid.SetSetpoint(liftEncoder->Get());
while (*isBraking) {
if ((pid.Get() > 0 && liftLowerLimit->Get()) || (pid.Get() < 0 && liftUpperLimit->Get())){
liftTalon->Set(0);
}
else {
liftTalon->Set(pid.Get());
}
SmartDashboard::PutBoolean("Braking", true);
}
pid.Disable();
SmartDashboard::PutBoolean("Braking", false);
liftTalon->Set(0);
}
void Arm(double joy) {
int location = EncArm.Get();
/*
if(location < 10 && joy < 0) joy = 0;
if(location > 110 && joy > 0) joy = 0;
arm1->Set(joy);
arm1_sec->Set(joy);
return;
*/
if(joy < -10) joy = -10;
if(joy > 110) joy = 110;
double speed = PIDArm(joy, location);
if(speed > .5) speed = .5;
if(speed < -.3) speed = -.3;
if(speed < 0 && location < 10) speed = 0;
if(speed > 0 && location > 110) speed = 0;
speed = LowArm(speed);
if(speed < .01 && speed > -.01) speed = 0;
arm1->Set(speed,3);
arm1_sec->Set(speed,3);
}
void RobotDemo::RPS_control_code(float desired_RPS)
// THE 360 COUNT PER REVOLUTION ENCODERS ARE GOOD FOR 10K RPM, BUT THE CIM motor MAXIMUM SPEED IS APPROXIMATELY 5K.
{
#if 1
/* If the difference between the old and new RPS (typ ~35) by greater than 1 either way,
* reset the ingegral terms to prevent stale values from affecting our PID calculations
*/
if (desired_RPS != prev_desired_RPS)
{
if (fabs(prev_desired_RPS - desired_RPS) >= 1.0)
{
integral_back = 0;
integral_front = 0;
}
cout << desired_RPS << " " << prev_desired_RPS << endl;
prev_desired_RPS = desired_RPS;
}
#endif
//motor = new Victor(motor_pwm);
//test_encoder = new Encoder(encoder_pwm1, encoder_pwm2, true);
if ((pid_code_timer->Get()) >= pidtime)
{
float actual_time = pid_code_timer ->Get();
prev_error_back = error_back;
prev_error_front = error_front;
RPS_back = (back_shooter_encoder->Get() / 360.0) / actual_time;
RPS_front = (front_shooter_encoder->Get() / 360.0) / actual_time;
error_back = desired_RPS - RPS_back;
error_front = desired_RPS - RPS_front;
integral_back = integral_back + (error_back * actual_time);
integral_front = integral_front + (error_front * actual_time);
derivitive_back = (error_back - prev_error_back) / actual_time;
derivitive_front = (error_front - prev_error_front) / actual_time;
//take error and set it to motors
{
RPS_speed_back = (error_back * first_pterm) + (integral_back
* iterm) + (derivitive_back * dterm);
RPS_speed_front = (error_front * first_pterm) + (integral_front
* iterm) + (derivitive_front * dterm);
}
if (RPS_speed_back > 1)
{
RPS_speed_back = 1;
}
if (RPS_speed_front > 1)
{
RPS_speed_front = 1;
}
if (RPS_speed_back < 0)
{
RPS_speed_back = 0;
}
if (RPS_speed_front < 0)
{
RPS_speed_front = 0;
}
//cout << RPS_speed << endl;
//cout << autonomous_timer->Get() << " RPS_back: " << RPS_back
//<< " RPS_front: " << RPS_front << " Desired RPS:"
//<< autonomous_desired_RPS << endl;
shooter_motor_back ->Set(RPS_speed_back);
shooter_motor_front ->Set(RPS_speed_front);
counter++;
#if 0
if (counter == 5)
{
counter = 1;
printf(
" %i)%f %f %f %f\n",
number, RPS, RPS_speed, error,
shooter_motor_back->Get());
number++;
}
else
{
printf("%f %f %f %f\n", RPS, RPS_speed, error,
shooter_motor_back->Get());
}
#endif
pid_code_timer->Reset();
back_shooter_encoder ->Reset();
front_shooter_encoder ->Reset();
}
}
void TeleopPeriodic()
{
//camera->GetImage(frame);
//imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f);
//CameraServer::GetInstance()->SetImage(frame);
printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle());
drive->ArcadeDrive(driveStick);
drive->SetMaxOutput((1-driveStick->GetThrottle())/2);
//printf("%f\n", (1-stick->GetThrottle())/2);
//leftMotor->Set(0.1);
//rightMotor->Set(0.1);
if (shootStick->GetRawAxis(3) > 0.5) {
launch1->Set(1.0);
launch2->Set(1.0);
} else if (shootStick->GetRawAxis(2) > 0.5) {
printf("Power Counter: %i\n", powerCounter);
if (powerCounter < POWER_MAX) {
powerCounter++;
launch1->Set(-0.8);
launch2->Set(-0.8);
} else {
launch1->Set(-0.6);
launch2->Set(-0.6);
}
} else {
launch1->Set(0.0);
launch2->Set(0.0);
powerCounter = 0.0;
}
//use this button to spin only one winch, to lift up.
if (shootStick->GetRawButton(7)) {
otherWinch->Set(0.5);
} else if (shootStick->GetRawButton(8)) {
otherWinch->Set(-0.5);
} else {
otherWinch->Set(0.0);
}
if (shootStick->GetRawButton(5)) {
winch->Set(-0.7);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
// otherWinch->Set(-0.5);
}
} else if (shootStick->GetRawButton(6)) {
winch->Set(0.7);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
// otherWinch->Set(0.5);
}
} else {
winch->Set(0.0);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//, otherWinch->Set(0.0);
}
}
if (shootStick->GetRawButton(1)) {
launchPiston->Set(1);
} else {
launchPiston->Set(0);
}
if (shootStick->GetRawButton(3)) {
Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer);
}
if (shootStick->GetRawButton(3) && debounce == false) {
debounce = true;
if (defenseUp) {
defensePiston->Set(DoubleSolenoid::Value::kReverse);
defenseUp = false;
} else {
defenseUp =true;
defensePiston->Set(DoubleSolenoid::Value::kForward);
}
} else if (!shootStick->GetRawButton(3)){
debounce = false;
}
}
void OperatorControl(void)
{
AxisCamera &camera = AxisCamera::GetInstance();
miniBotTime.Start();
initRobot();
debug("in telop");
compressor.Start();
GetWatchdog().SetEnabled(true);
/*int l1, l2, l3;
while (IsOperatorControl()) {
GetWatchdog().Feed();
//char val = (line1.Get() & 0x01) | (line2.Get() & 0x02) | (line3.Get() & 0x04);
//if(l1 != line1.Get() || l2 != line2.Get() || l3 != line3.Get()) {
// cerr << "change " << (l1 = line1.Get()) << "\t" << (l2 = line2.Get()) << "\t" << (l3 = line3.Get()) << endl;
//}
cerr << "Change "<< line1.Get() <<"\t" << line2.Get() << "\t" << line3.Get() << endl;
Wait(0.2);
}*/
char count=0, pneumaticCount=0;
// was .125 when loop at .025
lowPass lowSpeed(.04), lowStrafe(.04), lowTurn(.04), lowClaw(.04), lowArm(.04), lowArmLoc(.05);
double ClawLocation=0, ArmLocation=0, OldArmLocation=0;
while (IsOperatorControl() && !IsDisabled())
{
GetWatchdog().Feed();
float speed = -1*stick.GetRawAxis(2);
float strafe = stick.GetRawAxis(1);
float turn = stick.GetRawAxis(3);
if(!stick.GetRawButton(7)) {
speed /= 2;
strafe /= 2;
turn /= 2;
}
if(stick.GetRawButton(8)) {
speed /= 2;
strafe /= 2;
turn /= 2;
}
if(stick.GetRawButton(2)) {
speed = 0;
turn = 0;
}
Drive(lowSpeed(speed), lowTurn(turn), lowStrafe(strafe));
#ifndef NDEBUG
if(stick2.GetRawButton(10)) {
robotInted = false;
initRobot();
}
#endif
if(stick2.GetRawButton(7) && (miniBotTime.Get() >= 110 || (stick2.GetRawButton(9) && stick2.GetRawButton(10)))) { // launcher
// the quick launcher
MiniBot1a.Set(true);
MiniBot1b.Set(false);
}
if(!stick2.GetRawButton(10) && stick2.GetRawButton(9)) { // deploy in
MiniBot2a.Set(false);
MiniBot2b.Set(true);
//MiniBot2a.Set(false);
//MiniBot2b.Set(true);
}
if(stick2.GetRawButton(5)) { // top deploy out
MiniBot2a.Set(true);
MiniBot2b.Set(false);
}
if(stick2.GetRawButton(6)) { // open
ClawOpen.Set(true);
ClawClose.Set(false);
}
if(stick2.GetRawButton(8)) { // closed
ClawOpen.Set(false);
ClawClose.Set(true);
ClawLocation += 2;
}
/*156 straight
* 56 90 angle
* 10 back
*/
if(stick2.GetRawButton(1)) { // top peg
ClawLocation = 156;
ArmLocation = 105;
}
if(stick2.GetRawButton(2)) {
ClawLocation = 111; // the 90angle / middle peg
ArmLocation = 50;
}
if(stick2.GetRawButton(3)) { // off ground
ClawLocation = 176;
ArmLocation = 5;
}
if(stick2.GetRawButton(4)) {
ClawLocation = 0; // back
ArmLocation = 0;
}
double tmpClaw = .7*lowClaw(stick2.GetRawAxis(4));
if(tmpClaw < .2 && tmpClaw > -.2) tmpClaw = 0;
double tmpArm = .4*lowArm(-1*stick2.GetRawAxis(2));
if(tmpArm < .2 && tmpArm > -.2) tmpArm = 0;
if(tmpArm > .5) tmpArm = .5;
if(tmpArm < -.5) tmpArm = -.5;
ClawLocation += tmpClaw + tmpArm; // the right joy stick y
if(ClawLocation < 10) ClawLocation = 10;
if(ClawLocation > 230) ClawLocation = 230;
Claw(ClawLocation);
ArmLocation += tmpArm;
if(ArmLocation > 110) ArmLocation = 110;
if(ArmLocation < -10) ArmLocation = -10;
Arm(lowArmLoc(ArmLocation));
OldArmLocation = ArmLocation;
#ifndef NDEBUG
if(count++%20==0){
cerr << EncClaw.Get() << '\t' << arm1->GetOutputCurrent() << '\t' << arm1_sec->GetOutputCurrent() << '\t' << ArmLocation << '\t' << EncArm.Get() << endl;
// cerr << arm1->GetOutputCurrent() << '\t' << arm1_sec->GetOutputCurrent() << '\t' << arm2->GetOutputCurrent() << '\t'
// << EncArm.Get () << '\t' << LimitArm.Get() << '\t' << EncClaw.Get() << '\t' << LimitClaw.Get() << '\t' << ClawLocation << endl;
//cerr << '\t' << EncArm.Get() <<'\t' << arm1->Get() << endl;
// cerr << Dlf->Get() << '\t' << Dlf->GetSpeed() << '\t' << Dlb->GetSpeed() <<'\t' << Drf->GetSpeed() <<'\t' << Drb->GetSpeed() <<endl;//'\t' << line1.Get() << "\t" << line2.Get() << "\t" << line3.Get() << endl;
// cerr << '\t' << Dlb->GetSpeed() << '\t';
}
#endif
if(pneumaticCount++==0) {
ClawOpen.Set(false);
ClawClose.Set(false);
MiniBot1a.Set(false);
MiniBot1b.Set(false);
MiniBot2a.Set(false);
MiniBot2b.Set(false);
}
Wait(0.01); // wait for a motor update time
}
}
/**
* 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 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)
{
// feed watchdog --- TheVessel.SetSafetyEnabled(true);
GetWatchdog().Kill();
if (!game){
AlmostZeroPi = manEnc->GetDistance() -1;//Zeros manipulator encoder-- only here for tele op practice w/out auto
cmp->Start();
}
cd->MecanumDrive(0, 0, 0);
minibot->Set(DoubleSolenoid::kForward);
while(IsOperatorControl() && IsEnabled())
{
GetWatchdog().Kill();
if(IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kReverse);
else if(!IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kForward);
cd->MecanumDrive(controller);//passes joystick controller's input to cd mechdrv
if(stick->GetRawButton(1))
{
}
else if(stick->GetRawButton(2))
{
}
else if(stick->GetRawButton(3))
{
reset = true;// ENABLES NO RESTRICTION MANIPULATOR MOVEMENT
}
else if(stick->GetRawButton(4))
{
cout << "Manipulator Encoder Value: " << manEnc->Get() << "/n";//prints manip encoder to console if open;
Wait(.1f);//pause so not above a billion times
}
else if(stick->GetRawButton(6))
{
open->Set(true);//self explanatory, eote pnuematics
closed->Set(false);
}
else if(stick->GetRawButton(8))
{
closed->Set(true);
open->Set(false);
}
ShoulderJag->Set(stick->GetY());//shoulder moves with stick->GetY()
if (stick->GetTwist() < 0 || manEnc->GetDistance() < AlmostZeroPi || reset)//man extending, enc not at 0, or reset true
{
manJag->Set(stick->GetTwist()*.3);//shoulder above but for wrist, also, only 3/10ths power
}
}
cmp->Stop();//stops compressor
while (manEnc->GetDistance() < AlmostZeroPi)//brings manipulator back to 0 so not need reset
{
manJag->Set(.1);
}
}