bool getOrange()
{
return m_joy->GetRawButton(5);
}
bool getBack()
{
return m_joy->GetRawButton(7);
}
void OperatorControl(void)
{
// Teleoperated Code.
/*double WaitDash = 0.0;
double FireDash = 0.0;
double intPause = 0.0;
SmartDashboard::PutNumber("P", 3.0);
SmartDashboard::PutNumber("W", 0.0);
SmartDashboard::PutNumber("A", 0.0);
WaitDash = SmartDashboard::GetNumber("P");
FireDash = SmartDashboard::GetNumber("W");
intPause = SmartDashboard::GetNumber("A");
SmartDashboard::PutNumber("P", WaitDash);
SmartDashboard::PutNumber("W", FireDash);
SmartDashboard::PutNumber("A", WaitDash);
*/
// Enable and start the compressor.
//compressor->Enabled();
compressor->Start();
// Enable drive motor safety timeout.
myRobot.SetSafetyEnabled(true);
// Enable watchdog and initial feed.
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(1);
GetWatchdog().Feed();
// Set robot in low gear by default. Not active.
//s[0]->Set(false);
GetWatchdog().Feed();
//bool blnShoot = false;
bool blnLowHang = false;
bool blnShift = false;
GetWatchdog().Feed();
bool blnShooterSpd = false;
bool blnReverse = false;
float fltShoot;
float fltSpeed = 1;
int intFail = 0;
timerLowHang.Reset();
timerShift.Reset();
timerFire.Reset();
timerShooter.Reset();
timerDriveCtrl.Reset();
timerCamera.Reset();
timerReverse.Reset();
timerLowHang.Start();
timerShift.Start();
timerFire.Start();
timerShooter.Start();
timerDriveCtrl.Start();
timerCamera.Start();
timerReverse.Start();
GetWatchdog().Feed();
//sd->sendIOPortData();
// Local variables.
//float fltStick1X, fltStick1Y;
while (IsOperatorControl())
{
if(timerReverse.Get() > 0.5)
{
if(stick1->GetRawButton(8) && blnReverse == false )
{
blnReverse = true;
GetWatchdog().Feed();
timerReverse.Reset();
timerReverse.Start();
}
else if(stick1->GetRawButton(9) && blnReverse == true)
{
blnReverse = false;
GetWatchdog().Feed();
timerReverse.Reset();
timerReverse.Start();
}
}
if(blnReverse == false)
{
fltStick1Y = stick1->GetY();
fltStick1X = stick1->GetX();
SmartDashboard::PutBoolean("Reverse",false);
GetWatchdog().Feed();
}
else if(blnReverse == true)
{
fltStick1Y = ((stick1->GetY())*(-1));
fltStick1X = ((stick1->GetX())*(1));
SmartDashboard::PutBoolean("Reverse",true);
GetWatchdog().Feed();
}
myRobot.ArcadeDrive(fltStick1Y,fltStick1X);
//myRobot.ArcadeDrive(stick1);
GetWatchdog().Feed(); // Feed hungary demonic Watchdog.
SmartDashboard::PutBoolean("Touching Tower?",LimitSwitch->Get());
SmartDashboard::PutNumber("Throttle (%)",stick1->GetY()*(-100));
SmartDashboard::PutNumber("Steering (%)",stick1->GetX()*(100));
GetWatchdog().Feed();
//End Stick1 arcade drive code.
GetWatchdog().Feed();
fltShoot = (((-(stick2->GetRawAxis(3)))+1)/2);
GetWatchdog().Feed();
SmartDashboard::PutNumber("Shooter Power (%)", fltShoot);
SmartDashboard::PutNumber("Shooter Set Speed (%)", (fltSpeed*100));
//float fltPressureSwitch = m_pressureSwitch;
//float fltRelay = m_relay;
//SmartDashboard::PutNumber("Demo",3);
GetWatchdog().Feed();
if(timerShift.Get() > 0.2)
{
if(stick1->GetRawButton(7) || stick1->GetTrigger())
{
if(blnShift == false)
{
GetWatchdog().Feed();
s[0]->Set(false);
s[1]->Set(true);
SmartDashboard::PutString("Gear","Low");
blnShift = true;
timerShift.Stop();
timerShift.Reset();
timerShift.Start();
GetWatchdog().Feed();
}
else if (blnShift == true)
{
GetWatchdog().Feed();
s[0]->Set(true);
s[1]->Set(false);
SmartDashboard::PutString("Gear","High");
blnShift = false;
timerShift.Stop();
timerShift.Reset();
timerShift.Start();
GetWatchdog().Feed();
}
}
}
if(stick1->GetRawButton(2) && blnLowHang == false && timerLowHang.Get() > 0.5)
{
blnLowTime = true;
blnLowHang = true;
timerLowHang.Stop();
timerLowHang.Reset();
timerLowHang.Start();
GetWatchdog().Feed();
}
else if(stick1->GetRawButton(2) && blnLowHang == true && timerLowHang.Get() > 0.5)
{
blnLowTime = false;
blnLowHang = false;
timerLowHang.Stop();
timerLowHang.Reset();
timerLowHang.Start();
GetWatchdog().Feed();
}
if(blnLowTime == true)
{
s[3]->Set(true);
GetWatchdog().Feed();
}
else if (blnLowTime == false)
{
s[3]->Set(false);
GetWatchdog().Feed();
}
if(stick1->GetRawButton(3))
{
mtdCameraCode();
GetWatchdog().Feed();
}
if(stick2->GetTrigger() && intFail == 0 && timerFire.Get() > 0.8)
{
s[2]->Set(true);
SmartDashboard::PutString("Shooter Piston","In");
intFail = 1;
GetWatchdog().Feed();
timerFire.Stop();
timerFire.Reset();
timerFire.Start();
GetWatchdog().Feed();
}
else if(stick2->GetTrigger() && intFail == 1 && timerFire.Get() > 0.8)
{
s[2]->Set(false);
intFail = 0;
SmartDashboard::PutString("Shooter Piston","Out");
GetWatchdog().Feed();
timerFire.Stop();
timerFire.Reset();
timerFire.Start();
GetWatchdog().Feed();
}
if(stick2->GetRawButton(2) && blnShooterSpd == false && timerShooter.Get() > 0.5)
{
GetWatchdog().Feed();
myShooter1.Set(-fltSpeed);
myShooter2.Set(-fltSpeed);
SmartDashboard::PutString("Shooter","On");
SmartDashboard::PutNumber("Shooter Speed (%)",(fltSpeed)*(100));
blnShooterSpd = true;
GetWatchdog().Feed();
timerShooter.Stop();
timerShooter.Reset();
timerShooter.Start();
GetWatchdog().Feed();
}
else if(stick2->GetRawButton(2) && blnShooterSpd == true && timerShooter.Get() > 0.5)
{
GetWatchdog().Feed();
myShooter1.Set(0);
myShooter2.Set(0);
SmartDashboard::PutString("Shooter","Off");
SmartDashboard::PutNumber("Shooter Speed (%)",0);
blnShooterSpd = false;
GetWatchdog().Feed();
//Wait(0.2);
timerShooter.Stop();
timerShooter.Reset();
timerShooter.Start();
GetWatchdog().Feed();
}
if(stick2->GetRawButton(10))
{
fltSpeed = 0.6;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(9))
{
fltSpeed = 0.7;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(8))
{
fltSpeed = 0.8;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(7))
{
fltSpeed = 0.9;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(6))
{
fltSpeed = 1;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(11))
{
fltSpeed = fltShoot;
GetWatchdog().Feed();
}
GetWatchdog().Feed();
}
}
bool getYellow()
{
return m_joy->GetRawButton(4);
}
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 OperatorControl()
{
compressor.Start();
DriverStationLCD *screen = DriverStationLCD::GetInstance();
while (IsOperatorControl())
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"LeftTopM1_%f", logitech.GetRawAxis(2));
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"LeftBackM2_%f", logitech.GetRawAxis(2));
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"RightTopM1_%f", logitech.GetRawAxis(4));
//screen -> PrintfLine(DriverStationLCD::kUser_Line4,"RightTopM2_%f", logitech.GetRawAxis(4));
//screen -> PrintfLine(DriverStationLCD::kUser_Line5,"Button_%d", buttonOne.Get());
//screen -> PrintfLine(DriverStationLCD::kUser_Line6,"toggle_%d", togglebuttonOne.Get());
/** CATAPULT **/
loadCatapult();
shootCatapult(); //Press A to shoot
/** RETRIEVAL **/
if(logitech.GetRawButton(6)) //Press Upper Right Trigger to go down
{
armSolenoidOne.Set(DoubleSolenoid::kForward);
armSolenoidTwo.Set(DoubleSolenoid::kForward);
}
else if(logitech.GetRawButton(8)) //Press Lower Right Trigger to go up
{
armSolenoidOne.Set(DoubleSolenoid::kReverse);
armSolenoidTwo.Set(DoubleSolenoid::kReverse);
}
else
{
armSolenoidOne.Set(DoubleSolenoid::kOff);
armSolenoidTwo.Set(DoubleSolenoid::kOff);
}
/**DRIVING**/
if (fabs(logitech.GetRawAxis(2)) > driveStickBuffer)
{
leftFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1));
leftBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1));
}
else
{
leftFront.Set(0);
leftBack.Set(0);
}
if (fabs(logitech.GetRawAxis(4)) > driveStickBuffer)
{
rightFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4));
rightBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4));
}
else
{
rightFront.Set(0);
rightBack.Set(0);
}
// END TANK DRIVE
/***********************/
if ((buttonOne.Get()) == 1)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Not Pressed!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Pressed!");
}
if ((buttonTwo.Get()) == 1)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Not Pressed!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Pressed!");
}
if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 0))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Right Position");
}
//both on or off
else if ((((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 1))||((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 0))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Middle Position");
}
//Left button on && right off
else if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Position");
}
/***********************/
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Pressure:%f", compressor.GetPressureSwitchValue());
screen -> PrintfLine(DriverStationLCD::kUser_Line5,"Left_%f Right_&f", leftFront.Get(), rightFront.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"But6_%d But8_%d", logitech.GetRawButton(6), logitech.GetRawButton(8));
Wait(0.005);
screen -> UpdateLCD();
}
compressor.Stop();
}
void OperatorControl(void)
{
AxisCamera &camera = AxisCamera::GetInstance();
miniBotTime.Start();
initRobot();
debug("in telop");
compressor.Start();
GetWatchdog().SetEnabled(true);
/*int l1, l2, l3;
while (IsOperatorControl()) {
GetWatchdog().Feed();
//char val = (line1.Get() & 0x01) | (line2.Get() & 0x02) | (line3.Get() & 0x04);
//if(l1 != line1.Get() || l2 != line2.Get() || l3 != line3.Get()) {
// cerr << "change " << (l1 = line1.Get()) << "\t" << (l2 = line2.Get()) << "\t" << (l3 = line3.Get()) << endl;
//}
cerr << "Change "<< line1.Get() <<"\t" << line2.Get() << "\t" << line3.Get() << endl;
Wait(0.2);
}*/
char count=0, pneumaticCount=0;
// was .125 when loop at .025
lowPass lowSpeed(.04), lowStrafe(.04), lowTurn(.04), lowClaw(.04), lowArm(.04), lowArmLoc(.05);
double ClawLocation=0, ArmLocation=0, OldArmLocation=0;
while (IsOperatorControl() && !IsDisabled())
{
GetWatchdog().Feed();
float speed = -1*stick.GetRawAxis(2);
float strafe = stick.GetRawAxis(1);
float turn = stick.GetRawAxis(3);
if(!stick.GetRawButton(7)) {
speed /= 2;
strafe /= 2;
turn /= 2;
}
if(stick.GetRawButton(8)) {
speed /= 2;
strafe /= 2;
turn /= 2;
}
if(stick.GetRawButton(2)) {
speed = 0;
turn = 0;
}
Drive(lowSpeed(speed), lowTurn(turn), lowStrafe(strafe));
#ifndef NDEBUG
if(stick2.GetRawButton(10)) {
robotInted = false;
initRobot();
}
#endif
if(stick2.GetRawButton(7) && (miniBotTime.Get() >= 110 || (stick2.GetRawButton(9) && stick2.GetRawButton(10)))) { // launcher
// the quick launcher
MiniBot1a.Set(true);
MiniBot1b.Set(false);
}
if(!stick2.GetRawButton(10) && stick2.GetRawButton(9)) { // deploy in
MiniBot2a.Set(false);
MiniBot2b.Set(true);
//MiniBot2a.Set(false);
//MiniBot2b.Set(true);
}
if(stick2.GetRawButton(5)) { // top deploy out
MiniBot2a.Set(true);
MiniBot2b.Set(false);
}
if(stick2.GetRawButton(6)) { // open
ClawOpen.Set(true);
ClawClose.Set(false);
}
if(stick2.GetRawButton(8)) { // closed
ClawOpen.Set(false);
ClawClose.Set(true);
ClawLocation += 2;
}
/*156 straight
* 56 90 angle
* 10 back
*/
if(stick2.GetRawButton(1)) { // top peg
ClawLocation = 156;
ArmLocation = 105;
}
if(stick2.GetRawButton(2)) {
ClawLocation = 111; // the 90angle / middle peg
ArmLocation = 50;
}
if(stick2.GetRawButton(3)) { // off ground
ClawLocation = 176;
ArmLocation = 5;
}
if(stick2.GetRawButton(4)) {
ClawLocation = 0; // back
ArmLocation = 0;
}
double tmpClaw = .7*lowClaw(stick2.GetRawAxis(4));
if(tmpClaw < .2 && tmpClaw > -.2) tmpClaw = 0;
double tmpArm = .4*lowArm(-1*stick2.GetRawAxis(2));
if(tmpArm < .2 && tmpArm > -.2) tmpArm = 0;
if(tmpArm > .5) tmpArm = .5;
if(tmpArm < -.5) tmpArm = -.5;
ClawLocation += tmpClaw + tmpArm; // the right joy stick y
if(ClawLocation < 10) ClawLocation = 10;
if(ClawLocation > 230) ClawLocation = 230;
Claw(ClawLocation);
ArmLocation += tmpArm;
if(ArmLocation > 110) ArmLocation = 110;
if(ArmLocation < -10) ArmLocation = -10;
Arm(lowArmLoc(ArmLocation));
OldArmLocation = ArmLocation;
#ifndef NDEBUG
if(count++%20==0){
cerr << EncClaw.Get() << '\t' << arm1->GetOutputCurrent() << '\t' << arm1_sec->GetOutputCurrent() << '\t' << ArmLocation << '\t' << EncArm.Get() << endl;
// cerr << arm1->GetOutputCurrent() << '\t' << arm1_sec->GetOutputCurrent() << '\t' << arm2->GetOutputCurrent() << '\t'
// << EncArm.Get () << '\t' << LimitArm.Get() << '\t' << EncClaw.Get() << '\t' << LimitClaw.Get() << '\t' << ClawLocation << endl;
//cerr << '\t' << EncArm.Get() <<'\t' << arm1->Get() << endl;
// cerr << Dlf->Get() << '\t' << Dlf->GetSpeed() << '\t' << Dlb->GetSpeed() <<'\t' << Drf->GetSpeed() <<'\t' << Drb->GetSpeed() <<endl;//'\t' << line1.Get() << "\t" << line2.Get() << "\t" << line3.Get() << endl;
// cerr << '\t' << Dlb->GetSpeed() << '\t';
}
#endif
if(pneumaticCount++==0) {
ClawOpen.Set(false);
ClawClose.Set(false);
MiniBot1a.Set(false);
MiniBot1b.Set(false);
MiniBot2a.Set(false);
MiniBot2b.Set(false);
}
Wait(0.01); // wait for a motor update time
}
}
void TeleopPeriodic(void)
{
myarm->prepareSignal();
// Call the drive routine to drive the robot.
if(rightStick->GetRawButton(1)|| leftStick->GetRawButton(1))
drive->MecanumDrive_Cartesian(rightStick->GetX()/2,leftStick->GetY()/2,leftStick->GetX()/2,0.00);
else
drive->MecanumDrive_Cartesian(rightStick->GetX(),leftStick->GetY(),leftStick->GetX(),0.00);
GetStateForArm();
//Wait(.1);
if(modeArm==DDCArm::kManualOveride)
{
myarm->OperateArm(0,0,0,modeArm);
//Shoulder movement
if(leftStick->GetRawButton(3))
myarm->MoveShoulder(1);
else if(leftStick->GetRawButton(2))
myarm->MoveShoulder(-1);
else
myarm->MoveShoulder(0);
//Elbow Movement
if(rightStick->GetRawButton(3))
myarm->MoveElbow(1);
else if (rightStick->GetRawButton(2))
myarm->MoveElbow(-1);
else
myarm->MoveElbow(0);
//Wrist Movement
if(rightStick->GetRawButton(4))
myarm->MoveWrist(-1);
else if(rightStick->GetRawButton(5))
myarm->MoveWrist(1);
else
myarm->MoveWrist(0);
}
else
myarm->OperateArm(0.0,0.0,peg,modeArm);
if(leftStick->GetRawButton(4))
myarm->MoveClaw(-1);
else if(leftStick->GetRawButton(5))
myarm->MoveClaw(1);
else
myarm->MoveClaw(0);
if(rightStick->GetRawButton(10))
{
printf("S: %f \n E: %f \n W: %f \n \n",myarm->GetShoulderVoltage(),myarm->GetElbowVoltage(), myarm->GetWristVoltage());
}
deploy->OperateDeployment(fire,pull);
// Send Data to the Driver Station for Monitoring (w/in .
//sendIOPortData();
//Wait(.1);
}
/**
* Periodic code for teleop mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in teleop mode.
*/
void RobotDemo::TeleopPeriodic() {
/*ALTERNATE OPTION: use constants at the top of the code to pass in buttons associated
* with various actions.
*
*FOR ALTERNATE LAYOUTS:
*have one set of layouts commented out?
*take input from Driver Station I/O tab to determine which set is used?
* */
//driver joystick: Extreme 3D
bool compressor = stickDrive->GetRawButton(10);
//drive-related
bool stopDrive = stickDrive->GetRawButton(1);
bool slowSpeed = stickDrive->GetRawButton(2);
//arm controls--solenoids involved
bool extendAll_driver = stickDrive->GetRawButton(9);
bool retractAll_driver = stickDrive->GetRawButton(11);
//spinner controls
bool leftDrop_driver = stickDrive->GetRawButton(3);
bool leftPick_driver = stickDrive->GetRawButton(5);
bool rightDrop_driver = stickDrive->GetRawButton(4);
bool rightPick_driver = stickDrive->GetRawButton(6);
//other joystick: Attack 3
//arm controls--solenoids involved
bool extendAll_other = stickOther->GetRawButton(3);
bool retractAll_other = stickOther->GetRawButton(2);
bool leftArm_other = (stickOther->GetRawButton(6) || stickOther->GetRawButton(10));
bool rightArm_other = (stickOther->GetRawButton(7) || stickOther->GetRawButton(11));
//spinner controls
bool leftDrop_other = (stickOther->GetRawButton(4) && !stickOther->GetRawButton(1));
bool leftPick_other = (stickOther->GetRawButton(4) && stickOther->GetRawButton(1));
bool rightDrop_other = (stickOther->GetRawButton(5) && !stickOther->GetRawButton(1));
bool rightPick_other = (stickOther->GetRawButton(5) && stickOther->GetRawButton(1));
SmartDashboard::PutBoolean("Left-Spinner", spinnerStatusLeft);
SmartDashboard::PutBoolean("Right-Spinner", spinnerStatusLeft);
SmartDashboard::PutBoolean("S1-Status", s1Status);
SmartDashboard::PutBoolean("S2-Status", s2Status);
SmartDashboard::PutBoolean("S3-Status", s3Status);
SmartDashboard::PutNumber("Compressor-Pressure", this->compressor->GetPressureSwitchValue());
if(compressor) {
toggleCompressor();
}
++periodicCounter;
float x = stickDrive->GetX();
float y = stickDrive->GetY();
float z = (stickDrive->GetRawAxis(3)) / 1.5; //actually twist--capped at 2/3 for speed control
if(abs(x) < .125) x =0;
if(abs(y) < .125) y =0;
if(abs(z) < .125) z =0;
if(!slowSpeed) {
//myRobot.MecanumDrive_Cartesian(x, y, z, 0.0);
MecanumDrive(x, y, z, 0.0);
}
else if(slowSpeed) {
if (x < 0) x = x/3;
else x = x/3;
if (y < 0) y = y/3;
else y = y/3;
if (z < 0) z = z/3;
else z = z/3;
//myRobot.MecanumDrive_Cartesian(x, y, z, 0.0);
MecanumDrive(x, y, z, 0.0);
}
//stop robot
if (stopDrive){
stop();
return;
}
//extend all arms
if(extendAll_driver){
extend(s1,s1Status); //spinner arms go out first
extend(s3,s3Status);
Wait(.2);
extend(s2,s2Status);
return;
}
//retract all arms
if(retractAll_driver){
retract(s2,s2Status); //catcher arms come in first
Wait(.2);
retract(s3,s3Status);
retract(s1,s1Status);
return;
}
//toggle left set of spinners
if(leftDrop_driver){
toggleLeftDrop(SPINNER_SPEED);
return;
}
if(leftPick_driver){
toggleLeftPick(SPINNER_SPEED);
return;
}
//toggle right set of spinners
if(rightDrop_driver){
toggleRightDrop(SPINNER_SPEED);
return;
}
if(rightPick_driver){
toggleRightPick(SPINNER_SPEED);
return;
}
//attack3 joystick code below
//extend all arms
if(extendAll_other){
extend(s1,s1Status); //spinner arms go out first
extend(s3,s3Status);
Wait(.2);
extend(s2,s2Status);
Wait(0.5);
return;
}
//retract all arms
if(retractAll_other){
retract(s2,s2Status); //catcher arms come in first
Wait(.2);
retract(s3,s3Status);
retract(s1,s1Status);
Wait(0.5);
return;
}
if(leftDrop_other){
spinnerL1->Set(SPINNER_SPEED); //may need to
spinnerL2->Set(SPINNER_SPEED); //reverse these
return;
}
if(leftPick_other){
spinnerL1->Set(-SPINNER_SPEED); //may need to
spinnerL2->Set(-SPINNER_SPEED); //reverse these
return;
}
//toggle right set of spinners
if(rightDrop_other){
spinnerR1->Set(SPINNER_SPEED);
spinnerR2->Set(SPINNER_SPEED);
return;
}
if(rightPick_other){
spinnerR1->Set(-SPINNER_SPEED);
spinnerR2->Set(-SPINNER_SPEED);
return;
}
if (!(leftDrop_driver || leftPick_driver || leftDrop_other || leftPick_other))
{
stopSpinnersLeft();
}
if (!(rightDrop_driver || rightPick_driver || rightDrop_other || rightPick_other))
{
stopSpinnersRight();
}
//toggle left spinner arm
if(leftArm_other) {
toggle(s1,s1Status);
return;
}
//toggle right spinner arm
if(rightArm_other) {
toggle(s3,s3Status);
return;
}
}
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
}
void TeleopPeriodic()
{
/*if(!m_firstSample)
{
double v0X = m_curVX;
double p0X = m_curX;
double dT = (imu->GetLastSampleTime()-m_preT);
m_curVX = imu->GetAccelX()*9.8*dT+v0X;
m_curX = .5*imu->GetAccelX()*9.8*dT*dT+v0X*dT+p0X;
}
m_preT = imu->GetLastSampleTime();
m_firstSample = false;*/
ostringstream os;
//os << "Value " << imu->GetAngle() << endl;
//os << "Pitch " << imu->GetPitch() << endl;
//os << "Roll " << imu->GetRoll() << endl;
//os << "Yaw " << imu->GetYaw() << endl;
os << "AccelX " << imu->GetAccelX()*9.8 << endl;
os << "AccelY " << imu->GetAccelY()*9.8 << endl;
os << "AccelZ " << imu->GetAccelZ()*9.8 << endl;
os << "AngleX " << imu->GetAngleX() << endl;
os << "AngleY " << imu->GetAngleY() << endl;
os << "AngleZ " << imu->GetAngleZ() << endl;
os << "LST " << imu->GetLastSampleTime() << endl;
os << "D/s " << imu->GetRateZ() << endl;
// DriverStation::ReportError(os.str());
//myRobot.TankDrive(-stick.GetRawAxis(1),-stick.GetRawAxis(5),false);
float s1 = -stick.GetRawAxis(1);
float s2 = -stick.GetRawAxis(5);
float m1 = s1;
float m2 = s2;
m_rightBumper = stick.GetRawButton(6);
m_leftBumper = stick.GetRawButton(5);
if(m_rightBumper && !m_prevRightBumper)
{
m_correcting1 = !m_correcting1;
m_correcting2 = false;
}
if(m_leftBumper && !m_prevLeftBumper)
{
m_correcting1 = false;
m_correcting2 = !m_correcting2;
}
if(m_correcting1){
os<<"CORRECTED: 1";
m1 -= imu->GetRateZ()*0.9;
m2 += imu->GetRateZ()*0.9;
}else if(m_correcting2){
os<<"CORRECTED: 2";
m1 -= imu->GetRateZ()*0.7;
m2 += imu->GetRateZ()*0.7;
}
m_prevRightBumper = m_rightBumper;
myRobot.TankDrive(m1,m2,false);
os<<endl<<endl;
cerr << os.str();
//SmartDashboard::PutData("IMU", imu);
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot->SetSafetyEnabled(true);
while (IsOperatorControl())
{
bool setLimit;
double cimValue1= -scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1, also invert the cim, since we want it to rotate coutnerclockwise/clockwise
double cimValue2= scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1
//For shooter
/*if (stick.GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
myRobot.ArcadeDrive((stick.GetY()),
(stick.GetX()), false); // inverted drive control
} else {
//front of the robot is moved forward by pushing forward on the joystick
myRobot.ArcadeDrive(-(stick.GetY()),
(stick.GetX()), false); // normal drive control
}
*/
//For manual button speed control, this sets the speed
if (stick->GetRawButton(4) == true) {
cim1->Set(cimValue1); //use the value from the throttle to set cim speed
cim2->Set(cimValue2);//Get speed from throttle, and then scale it
setLimit = true;
//Open Ball Stopper
}
else {
cim1->Set(0.0);
cim2->Set(0.0);
setLimit = false;
//Close Ball Stopper
}
//For precisebelt pickup
if (stick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
if (setLimit == true) {
JagBelt->ArcadeDrive(0.1,
(stick->GetX()), false); // inverted drive control
} else {
JagBelt->ArcadeDrive((stick->GetY()),
(stick->GetX()), false); // inverted drive control
}
} else {
//front of the robot is moved forward by pushing forward on the joystick
if (setLimit == true) {
JagBelt->ArcadeDrive(-0.1,
(stick->GetX()), false); // inverted drive control
} else {
JagBelt->ArcadeDrive(-(stick->GetY()),
(stick->GetX()), false); // inverted drive control
}
}
//For normal belt pickup
/*if (stick->GetRawButton(6) == true) {
JagBelt->Drive(1.0, 0); //opens gripper
} else {
JagBelt->Drive(0.0, 0); //closes gripper
}
*/
//For drive
if (rightstick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
if (rightstick->GetRawButton(10) == true) {
precisionMode= true;
}
else {
precisionMode= false;
}
myRobot->ArcadeDrive((rightstick->GetY()), (rightstick->GetX()), precisionMode); // inverted drive control
} else
{
if (rightstick->GetRawButton(10) == true) {
precisionMode= true;
}
else {
precisionMode= false;
}
//front of the robot is moved forward by pushing forward on the joystick
myRobot->ArcadeDrive(-(rightstick->GetY()), (rightstick->GetX()), precisionMode); // normal drive control
}
/*if (stick->GetRawButton(8) == true) {
cim1->Set(-0.37); //run cim 1 at 50% speed counterclockwise??
cim2->Set(0.37); // run cim at 50% speed clockwise
check = true; //indicate if motors are running
} else if (check == true){ // if motors are running a
Wait(2.0);
belt->Set(1); // run the belt
Wait(2.0); // one sec delay
belt->Set(0.0); // turn belt off
check = false; // put new check
}
else if (check == false){ //if false
// 2 sec delay to wait for the first ball to shoot
cim1->Set(0.0); //stop cims
cim2->Set(0.0);
}
else { // Stop everything
cim1->Set(0.0); //stop cims
cim2->Set(0.0);
belt->Set(0.0);
}
*/
//Code for using window motor
if (rightstick->GetRawButton(4)) {
window->Set(Relay::kOn);
window->Set(Relay::kForward); // tell window motor to go forward
} else if (rightstick->GetRawButton(5) == true){
window->Set(Relay::kOn);
window->Set(Relay::kReverse); //tell window motor to go backward
}
else {
//Wait(1.0);
window->Set(Relay::kOff); //turn it off, if the relays aint being used
}
//Code for Banebot Motor for stopping ballz
if (stick->GetRawButton(2) == true) { // press button TWO to close
pickStop->Set(-0.5); //closes ball stopper
Wait(1);
pickStop->Set(0.0);
} else if (stick->GetRawButton(3) == true){ //press button three to open
pickStop->Set(0.5); //opens ball stopper
Wait(1.2); // too slow, so needs more time
pickStop->Set(0.0);
}
else if (stick->GetRawButton(5)== true){ //press 5 to stop imediately, useful for adjusting angles...
//Wait(1.0);
pickStop->Set(0.0);
}
//Code for ... servoooo
if (stick->GetRawButton(10) == true) { //press 10 on the left stick...
bar->SetAngle(60); // set the angles to 60...clockwise?
bar2->SetAngle(60);
} else if (stick->GetRawButton(11) == true) // press 11 on the left stick
{
bar->SetAngle(-60); //set the angles to -60...counterclockwise?
bar2->SetAngle(-60);
}
// Initialize functions...
// RelayServo();
//PreciseBelt();
//UltrasonicRange();
// Accelerometer();
//dash->GetPacketNumber();
// send data back to dashboard
dash->GetPacketNumber(); //not sure why this is here 0_0
//int limitValue= limitSwitch->Get() ? 1 : 0; // retrieve 1 and 0 only.../ look for 0 and 1
float servoAngle = bar->GetAngle();
//dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Limit State: %d", limitValue); //send data back to driver station...
// dsLCD->Printf(DriverStationLCD::kUser_Line2, 2, "Servo Angle: %f", servoAngle); //send data back to driver station...
float gyroVal = gyro -> GetVoltage();//Gets voltage from gyro
float ultraVal = rangeFinder -> GetVoltage(); //Get voltage from ultrasonic sensor
float tempVal = Temperature -> GetVoltage();//Gets temperature
//Do the math to convert data received from the ultrasonic volts->miliVolts->milivolts per inch->inches
float Vm = (ultraVal*1000);
float Ri = (Vm/9.765625);
// Print data back to dashboard
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Ultrasonic Range: %f",Ri);
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Gyro: %f", gyroVal);
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Temperature: %f", tempVal);
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Cim1 Speed: %f%%", (cimValue1*100)); //display speed that the mototrs are reunning at different percentages...
dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Cim2 Speed: %f%%", (cimValue2*100));
//Update dashboard
dsLCD->UpdateLCD();
}
}
void TeleopPeriodic() override {
float leftPower, rightPower; // Get the values for the main drive train joystick controllers
leftPower = -leftjoystick->GetY();
rightPower = -rightjoystick->GetY();
float multiplier; // TURBO mode
if (rightjoystick->GetRawButton(1))
{
multiplier = 1;
} else {
multiplier = 0.5;
}
// wtf is a setpoint - it's an angle to turn to
if (leftjoystick->GetRawButton(6)) {
turnController->Reset();
turnController->SetSetpoint(0);
turnController->Enable();
ahrs->ZeroYaw();
//ahrs->Reset();
}
// Press button to auto calculate angle to rotate bot to nearest ball
// if(leftjoystick->GetRawButton(99))
// {
// ahrs->ZeroYaw();
// turnController->Reset();
// turnController->SetSetpoint(mqServer.GetDouble("angle"));
// turnController->Enable();
// aimState = 1;
// }
switch(aimState)
{
default:
case 0: // No camera assisted turning
//Drive straight with one controller, else: drive with two controllers
if (leftjoystick->GetRawButton(1)) {
drive->TankDrive(leftPower * multiplier, leftPower * multiplier,
false);
} else if (leftjoystick->GetRawButton(2)) {
drive->TankDrive(leftPower * multiplier + rotateRate,
leftPower * multiplier + -rotateRate, false);
} else {
drive->TankDrive(leftPower * multiplier, rightPower * multiplier,
false);
}
break;
case 1: // Camera assisted turning, deny input from controllers
drive->TankDrive(rotateRate, -rotateRate, false);
if(turnController->OnTarget() || leftjoystick->GetRawButton(97)) {
aimState = 0; // Finished turning, auto assist off
turnController->Disable();
turnController->Reset();
}
break;
}
// That little flap at the bottom of the joystick
float scaleIntake = (1 - (controlstick->GetThrottle() + 1) / 2);
// Depending on the button, our intake will eat or shoot the ball
if (controlstick->GetRawButton(2)) {
intake->Set(-scaleIntake);
shooter->Set(scaleIntake);
} else if (controlstick->GetRawButton(1)) {
intake->Set(scaleIntake);
shooter->Set(-scaleIntake);
} else {
intake->Set(0);
shooter->Set(0);
}
// Control the motor that lifts and descends the intake bar
float intake_lever_power = 0;
if (controlstick->GetRawButton(6)) {
manual = true;
intake_lever_power = .3;
// intakeLever->Set(.30); // close
} else if (controlstick->GetRawButton(4)) {
manual = true;
intake_lever_power = -.4;
// intakeLever->Set(-.40); // open
} else if (controlstick->GetRawButton(3)){
manual = true;
intake_lever_power = -scaleIntake;
// intakeLever->Set(-scaleIntake);
} else if (controlstick->GetRawButton(5)) {
manual = true;
intake_lever_power = scaleIntake;
// intakeLever->Set(scaleIntake);
} else {
if (manual) {
manual = false;
lastLiftPos = intakeLever->GetEncPosition();
intakeLever->SetControlMode(CANTalon::ControlMode::kPosition);
intakeLever->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
intakeLever->SetPID(1, 0.001, 0.0);
intakeLever->EnableControl();
}
intake_hold = true;
intakeLever->Set(lastLiftPos);
}
if (manual) {
intake_hold = false;
intakeLever->SetControlMode(CANTalon::ControlMode::kPercentVbus);
intakeLever->Set(intake_lever_power);
}
if (controlstick->GetRawButton(11)) {
lift->Set(true);
liftdown->Set(false);
} else if (controlstick->GetRawButton(12)){
lift->Set(false);
liftdown->Set(true);
} else if (controlstick->GetRawButton(7)) {
liftdown->Set(false);
}
if (controlstick->GetRawButton(9)) {
winch->Set(scaleIntake);
} else if (controlstick->GetRawButton(10)) {
winch->Set(-scaleIntake);
} else {
winch->Set(0);
}
if (controlstick->GetPOV() == 0 && !bounce ) {
constantLift -= 0.05;
bounce = true;
} else if (controlstick->GetPOV() == 180 && !bounce) {
constantLift += 0.05;
bounce = true;
} else if (controlstick->GetPOV() == 270 && !bounce) {
constantLift = 0;
bounce = true;
} else {
bounce = false;
}
UpdateDashboard();
}
bool getStart()
{
return m_joy->GetRawButton(8);
}
void OperatorControl()
{
while (IsOperatorControl() && IsEnabled())
{
robotDrive.ArcadeDrive(scaler(stick.GetZ()),scaler(stick.GetY()));
SmartDashboard::PutNumber("StickZ",stick.GetZ());
SmartDashboard::PutNumber("StickZscaled",scaler(stick.GetZ()));
finger_Motor.Set(scaler(stick2.GetRawAxis(thumbpadL_Y)));
// scalerValue = stick.GetRawAxis(3);
arm_Motor.Set(scaler(stick2.GetRawAxis(thumbpadR_Y)));
manualShooter();
shootingModes();
toggleIntake();
toggleIntakeMode();
setScalerValue();
// double volts = ourRangefinder->GetVoltage();
// SmartDashboard::PutNumber("Voltage",volts);
//t_motor.Set(stick2.GetZ());
//t_motor.Set(stick.GetAxis(Joystick::kDefaultThrottleAxis));
// t_motor.Set(stick.GetAxis(Joystick::kThrottleAxis));
// finger_Motor.Set(stick2.GetAxis(Joystick::kThrottleAxis));
// arm_Motor.Set(stick2.GetY());
// Current Control mode Debug
SmartDashboard::PutNumber("Motor30 Current",t_motor.GetOutputCurrent());
SmartDashboard::PutNumber("Position",t_motor.GetPosition());
// t_motor.Set(stick.GetAxis(Joystick::Slider));
// if (stick.GetRawButton(3) == true){
// t_motor.SetPosition(10000);
// }
// if (stick2.GetRawButton(7) == true and buttonpress == false){
//
// }
// else if (stick2.GetRawButton(3) == false and buttonpress == true){
// buttonpress = false;// drive with arcade style (use right stick)
//
// }
SmartDashboard::PutBoolean("buttonpress state",buttonpress);
if (stick2.GetRawButton(buttonBack) == true){
stateDisarmed = true;
stateArming1 = false;
stateArming2 = false;
stateArmed = false;
stateFiring1 = false;
stateFiring2 = false;
t_motor.SetPosition(0);
}
toggleIntake();
// if (stick2.GetRawButton(5) == true){
// intake_Spin_Motor.Set(1);
// }
// if (stick2.GetRawButton(5) == false){
// intake_Spin_Motor.Set(0);
// }
if (stick.GetRawButton(2) == true and buttonpress2 == false){
myServo->SetAngle(175);
}
if (stick.GetRawButton(2) == false and buttonpress2 == true){
myServo->SetAngle(5);
}
// servoSetPos(servoState);
// myServo->Set(.5);
Wait(0.005);// wait for a motor update time
// motorSetPos(launcherState);
}
}
void TeleopPeriodic(void) {
// increment the number of teleop periodic loops completed
GetWatchdog().Feed();
m_telePeriodicLoops++;
/*
* No longer needed since periodic loops are now synchronized with incoming packets.
if (m_ds->GetPacketNumber() != m_priorPacketNumber) {
*/
/*
* Code placed in here will be called only when a new packet of information
* has been received by the Driver Station. Any code which needs new information
* from the DS should go in here
*/
m_dsPacketsReceivedInCurrentSecond++; // increment DS packets received
// put Driver Station-dependent code here
// Demonstrate the use of the Joystick buttons
// DemonstrateJoystickButtons(m_rightStick, m_rightStickButtonState, "Right Stick", &m_solenoids[1]);
// DemonstrateJoystickButtons(m_leftStick, m_leftStickButtonState, "Left Stick ", &m_solenoids[5]);
/***
// determine if tank or arcade mode, based upon position of "Z" wheel on kit joystick
if (m_rightStick->GetZ() <= 0) { // Logitech Attack3 has z-polarity reversed; up is negative
// use arcade drive
m_robotDrive->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick)
if (m_driveMode != ARCADE_DRIVE) {
// if newly entered arcade drive, print out a message
printf("Arcade Drive\n");
m_driveMode = ARCADE_DRIVE;
}
} else {
// use tank drive
m_robotDrive->TankDrive(m_leftStick, m_rightStick); // drive with tank style
if (m_driveMode != TANK_DRIVE) {
// if newly entered tank drive, print out a message
printf("Tank Drive\n");
m_driveMode = TANK_DRIVE;
}
}
***/
/* Grab z-wheel value and transform from [1, -1] to [0,4600]
* 4600 rpm is a guess */
float rawZ, transformedZ;
rawZ = m_leftStick->GetZ();
// transformedZ = (1.0 - rawZ)/(-2.0);
transformedZ = -2300.0 * rawZ + 2300.0;
/* Driver station display output. */
char msg[256];
static DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
sprintf(msg, "Launcher Speed = %f RPM", transformedZ);
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, msg);
sprintf(msg, "Loader Limit = %u", m_loaderLimit->Get());
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, msg);
sprintf(msg, "Loader Trigger = %u", m_leftStick->GetTrigger());
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, msg);
// line number (enum), starting col, format string, args for format string ...
dsLCD->UpdateLCD();
// use arcade drive
//m_driveGain = (1.0 - m_rightStick->GetZ())/(-2.0);
m_robot->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick)
// m_robotFrontDrive->Drive(-m_driveGain*m_rightStick->GetY(),-m_driveGain*m_rightStick->GetX()); // negative sign to fix bug in turn
// m_robotRearDrive->Drive(m_driveGain*m_rightStick->GetY(),-m_driveGain*m_rightStick->GetX()); // negative sign to fix bug in turn
// -Add an elevation control, add a motor controller to port 8, and add a joystick button pair to control up/down
if (m_rightStick->GetRawButton(m_elevatorUpButton)){
m_elevatorMotor->Set(m_elevatorUpSpeed);
}
else if (m_rightStick->GetRawButton(m_elevatorDownButton)){
m_elevatorMotor->Set(m_elevatorDownSpeed);
}
else {
m_elevatorMotor->Set(0.0);
}
// trigger button activates feeding and loading mechanisms
if ((m_leftStick->GetTop() || m_feeding) && (!m_loading) && (m_feedCounter <= m_feedCount)){
m_feeding = true;
m_feedCounter++;
m_feedMotor->Set(m_feedSpeed);
}
else {
m_feeding = false;
m_feedCounter = 0;
m_feedMotor->Set(0.);
}
m_loaderLimit->Get(); //just testing digital input port 1
// top(2) button uses the Z wheel to control the speed of the loading mechanisms
if (((m_leftStick->GetTrigger() || m_loading) && (!m_feeding)) && (m_loadCounter <= m_loadCount)){//comment out this line
// if (((m_leftStick->GetTrigger() || m_loading) && (!m_feeding)) && (bool) m_loaderLimit->Get()){ //uncomment this line
m_loading = true;
m_loadCounter++; // comment out this line
m_loadMotor->Set(m_loadSpeed); // load breech
}
else if ((m_loading && (!m_feeding)) && (m_loadCounter > m_loadCount) && (m_loadCounter <= ((m_loadCount+m_loadPauseCount)))) { // comment out the pause logic
m_loading = true;// comment out the pause logic
m_loadCounter++;// comment out the pause logic,
m_loadMotor->Set(0.); // pause loader // comment out the pause logic
}
// else if ((m_loading && (!m_feeding)) && (m_loadCounter >= (m_loadCount+m_loadPauseCount)) && (m_loadCounter<= 2*m_loadCount-13)) { // comment out this line
else if ((m_loading && (!m_feeding)) && (m_loadCounter >= (m_loadCount+m_loadPauseCount)) && (m_loaderLimit->Get()!=0)) {
//Retracting loader arm (feeder), have not yet hit the limit switch.
// else if ((m_loading &! m_feeding) && (bool) m_loaderResetLimit->Get()) { // uncomment this line
m_loading = true;
m_loadCounter++; // comment out the pause logic
m_loadMotor->Set(-m_loadSpeed);; // reset loader
}
else {
//Finished retracting loader arm (feeder), so stop it.
m_loading = false;
m_loadCounter = 0;
m_loadMotor->Set(0.);; // stop loader
}
// Set launcher motor command
m_launchMotor->Set(-(1.0 - m_leftStick->GetZ())/(-2.0)); // transform from [-1.0, +1.0] to [0.0, +1.0]
/*
} // if (m_ds->GetPacketNumber()...
*/
// use buttons 8 and 9 to trim load motor position in maintenance mode
/*if (m_leftStick->GetRawButton(8) && (!m_feeding) && (!m_loading)){
m_loadMotor->Set(0.12);
}
else if (m_leftStick->GetRawButton(9) && (!m_feeding) && (!m_loading)){
m_loadMotor->Set(-0.1);
}*/
//Use buttons 10 and 11 to trim feed motor position in maintenance mode
if (m_leftStick->GetRawButton(11) && (!m_feeding) && (!m_loading)){
m_feedMotor->Set(0.15);
}
else if (m_leftStick->GetRawButton(10) && (!m_feeding) && (!m_loading)){
m_feedMotor->Set(-0.1);
}
} // TeleopPeriodic(void)
bool getGreen()
{
return m_joy->GetRawButton(1);
}
void GetStateForArm()
{
//stay in manual overide until another console button is pressed
if(leftStick->GetRawButton(2) || leftStick->GetRawButton(3) ||
leftStick->GetRawButton(4) || leftStick->GetRawButton(5) ||
rightStick->GetRawButton(2) || rightStick->GetRawButton(3) ||
rightStick->GetRawButton(4) || rightStick->GetRawButton(5))
{
modeArm=DDCArm::kManualOveride;
peg=0;
}
else
{
if(dseio.GetDigital(1))
{
modeArm=DDCArm::kInverseKinematics;
peg=1;
}
if(dseio.GetDigital(2))
{
modeArm=DDCArm::kInverseKinematics;
peg=2;
}
if(dseio.GetDigital(3))
{
modeArm=DDCArm::kInverseKinematics;
peg=3;
}
if(dseio.GetDigital(4))
{
modeArm=DDCArm::kInverseKinematics;
peg=4;
}
if(dseio.GetDigital(5))
{
modeArm=DDCArm::kInverseKinematics;
peg=5;
}
if(dseio.GetDigital(6))
{
modeArm=DDCArm::kInverseKinematics;
peg=6;
}
if(dseio.GetDigital(7))//pickup
{
modeArm=DDCArm::kPickupTube;
peg=0;
}
if(dseio.GetDigital(8))//retrieve
{
modeArm=DDCArm::kRetrieveTube;
peg=0;
}
if(dseio.GetDigital(9))//stow
{
modeArm=DDCArm::kStow;
peg=0;
}
if(dseio.GetDigital(10))//stop
{
modeArm = DDCArm::kStop;
peg=0;
}
}
if(dseio.GetAnalogIn(2)>2.5)
fire=true;
else
fire=false;
if(dseio.GetAnalogIn(2)<1)
pull=true;
else
pull=false;
}
bool getRed()
{
return m_joy->GetRawButton(2);
}
void TeleopPeriodic()
{
if(stick.GetRawButton(3))
{
sm2kl.Set(1.0);
sm2kr.Set(1.0);
}
if(rSwitch.Get())
{
sm2kr.Set(0.0);
sm2kl.Set(0.0);
SmartDashboard::PutNumber("Tests",675);
}
if(stick.GetRawButton(5))
{
sm2kl.Set(-1.0);
sm2kr.Set(-1.0);
SmartDashboard::PutNumber("Tests",675);
}
if((lEnc.GetDistance() > 1080)||(rEnc.GetDistance() > 1080))
{
sm2kl.Set(0.0);
sm2kr.Set(0.0);
}
if(stick.GetRawButton(10))
{
actintake.Set(1.0);
}
else if(stick.GetRawButton(9))
{
actintake.Set(-1.0);
}
else
{
actintake.Set(0.0);
}
xvalue = -stick.GetZ()*.5;
yvalue = -stick.GetY();
if(stick.GetRawButton(7)&&!toggle)
{
latch=!latch;
toggle=true;
}
if(!stick.GetRawButton(7)&&toggle)
{
toggle=false;
}
if(latch)
{
yvalue=-yvalue;
}
if(stick.GetRawButton(1))
{
launcher.Set(1.0);
}
if(stick.GetRawButton(11))
{
intake.Set(1.0);
}
myRobot.ArcadeDrive(yvalue,xvalue);
SmartDashboard::PutNumber("rSwitch",rSwitch.Get());
SmartDashboard::PutBoolean("latch",latch);
SmartDashboard::PutNumber("Button",stick.GetRawButton(5));
//myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
}
bool getBlue()
{
return m_joy->GetRawButton(3);
}
/******************************************************************************
* 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 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);
}
}