void TeleopPeriodic(void)
{
//Drive code
leftFrontDrive->Set(-1 * leftStick->GetY());
rightFrontDrive->Set(rightStick->GetY());
leftRearDrive->Set(-1 * leftStick->GetY());
rightRearDrive->Set(rightStick->GetY());
//airCompressor->Start();
// Simple Button Drive Forward
if(rightStick->GetRawButton(3))
{
leftFrontDrive->Set(-1);
rightFrontDrive->Set(1);
leftRearDrive->Set(-1);
rightRearDrive->Set(1);
}
// Simple Button Drive Backwards
if(rightStick->GetRawButton(2))
{
leftFrontDrive->Set(1);
rightFrontDrive->Set(-1);
leftRearDrive->Set(1);
rightRearDrive->Set(-1);
}
// Code to print to the user messages box in the Driver Station
LCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hello World");
LCD->Printf(DriverStationLCD::kUser_Line2,1,"Y Left: %f", leftStick->GetY());
LCD->Printf(DriverStationLCD::kUser_Line3,1,"Y Right: %f", rightStick->GetY());
LCD->UpdateLCD();
Wait(0.2);
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
myRobot.ArcadeDrive(stick.getAxisLeftY(), stick.getAxisLeftX()); // drive with arcade style (use left stick)
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Move: %f4", stick.getAxisLeftY());
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Rotate: %f4", stick.getAxisLeftX());
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
void Print ()
{
if (PrintTime.Get() > PRINT_TIME)
{
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Left Speed = %5.4f", PrimaryController.GetRawAxis(LEFT_JOYSTICK));
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Right Speed = %5.4f", PrimaryController.GetRawAxis(RIGHT_JOYSTICK));
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Charge State = %d", (int)Shooter.chargestate);
//lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Collector speed= %d", Collector.CollectorSpeed());
lcd->UpdateLCD();
PrintTime.Reset();
PrintTime.Start();
}
}
void Disabled()
{
while(IsDisabled())
{
LEDLight->Set(Relay::kForward);
rpi->Read();
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "R: %i", rpi->GetMissingPacketcount());
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "x: %i", rpi->GetXPos());
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "y: %i", rpi->GetYPos());
lcd->UpdateLCD();
}
}
void DriverstationMessages::SendTextLines()
{
DriverStationLCD *lcd =DriverStationLCD::GetInstance();
for(size_t i = 0; i < LENGTH(text);i++){
lcd->Printf((DriverStationLCD::Line)i, 1, "%s", (char *)text[i]);
}
lcd->UpdateLCD();
}
void
DriverMessages::SendTextLines()
{
DriverStationLCD *lcd =DriverStationLCD::GetInstance();
for(int i = 0; i < 3;i++){
lcd->Printf((DriverStationLCD::Line)i, 1, "%s", (char *)lineText[i]);
}
lcd->UpdateLCD();
}
RobotDemo(void)
{
motor = new Jaguar(9);
stick = new Joystick(1);
compressor = new Compressor(1, 1);
valve = new Solenoid(8);
// Construct the dashboard sender object used to send hardware state
// to the driver station
// dds = new DashboardDataSender();
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Plyboy test code: 6:46PM 1/2/2012");
dsLCD->UpdateLCD();
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
Saftey->SetEnabled(false);
//myRobot->SetSafetyEnabled(false);
//myRobot->Drive(0.5, 0.0); // drive forwards half speed
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
//dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Hello World" );
//dsLCD->UpdateLCD();
Wait(0.5);
ds = DriverStation::GetInstance();
switch(ds->GetLocation())
{
case 1:
//Execute Autonomous code #1
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 1");
break;
case 2:
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 2");
//Execute Autonomous code #2
break;
case 3:
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 3");
//Execute Autonomous code #3
break;
}
dsLCD->UpdateLCD();
Saftey->SetEnabled(false);
Wait(0.5); // for 2 seconds
delete Jaguar1;
delete Jaguar2;
delete Saftey;
delete dsLCD;
delete ds;
}
void TeleopPeriodic(void) {
// increment the number of teleop periodic loops completed
m_telePeriodicLoops++;
//Feed joystick inputs to each subsystem here
//Using triggers to turn;
ds->Printf(DriverStationLCD::kUser_Line1,0, "GetY: %f",DriveStick->GetY());
ds->Printf(DriverStationLCD::kUser_Line2,0, "GetZ: %f",DriveStick->GetZ());
MyRobot.DriveRobot(DriveStick->GetY(),(-DriveStick->GetZ()));
//original:MyRobot.DriveRobot(DriveStick->GetY(),DriveStick->GetX());
//MyRobot.DriveRobotTrig(DriveStick->GetY(),DriveStick->GetX());
} // TeleopPeriodic(void)
void OperatorControl (void) {
GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
while (IsOperatorControl() && IsEnabled()) {
GetWatchdog().Feed(); // Feed the Watchdog.
kinectMode = (bool) kinectModeSelector->GetSelected();
demoMode = (bool) demoModeSelector->GetSelected();
speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
if (kinectMode) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
if (!demoMode) {
if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else {
motorDriveLeft.Set(0);
motorDriveRight.Set(0);
}
}
if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
motorTurret.Set(TURRET_POWER);
} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
if (joystickDriveLeft.GetThrottle() == 0) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
motorTurret.Set(TURRET_POWER);
} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(MANIPULATOR_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
motorTurret.Set(joystickManipulator.GetX() * -1 * TURRET_POWER);
}
}
dsLCD->UpdateLCD();
}
GetWatchdog().SetEnabled(false); // Teleop is done, so let's turn off Watchdog.
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
double tm = GetTime();
//AccelerationReset();
AverageWindowFilter<double, 20> fx, fy;
GetWatchdog().SetEnabled(false);
double ax = 0, lastAx = 0;
double ay = 0, lastAy = 0;
for (int i = 0; i < 20; i++)
{
GetAcceleration(ax, ay);
fx.AddPoint(ax);
fy.AddPoint(ay);
Wait(0.05);
}
avgX = fx.GetAverage();
avgY = fy.GetAverage();
double minX = 0, maxX = 0;
double minY = 0, maxY = 0;
GetWatchdog().SetEnabled(true);
while (IsOperatorControl())
{
GetWatchdog().Feed();
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
if (GetTime() - tm > 0.05)
{
GetAcceleration(ax, ay);
fx.AddPoint( ax - avgX );
fy.AddPoint( ay - avgY );
ax = fx.GetAverage();
ay = fy.GetAverage();
minX = min(fabs(ax - lastAx), minX);
maxX = max(fabs(ax - lastAx), maxX);
minY = min(fabs(ay - lastAy), minY);
maxY = max(fabs(ay - lastAy), maxY);
lastAx = ax;
lastAy = ay;
//AccelerationUpdate( ax, ay, .1);
//get the filtered acceleration, velocity and position
//GetAcceleration( &ax, &ay);
// or
// ax = (((axH / 0.01) - .5) * 8.0) * 9.81;
// ay = (((ayH / 0.01) - .5) * 8.0) * 9.81;
DriverStationLCD * lcd = DriverStationLCD::GetInstance();
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "ax: %f m/s^2 ", ax);
lcd->Printf(DriverStationLCD::kUser_Line4, 1, "%.6f %.6f ", minX, maxX);
lcd->Printf(DriverStationLCD::kUser_Line5, 1, "ay: %f m/s^2 ", ay);
lcd->Printf(DriverStationLCD::kUser_Line6, 1, "%.6f %.6f ", minY, maxY);
// euler' integration method.. bad bad bad
//vx += ax / (0.01);
//vy += ay / (0.01);
//lcd->Printf(DriverStationLCD::kUser_Line4, 1, "vx: %.1f", vx);
//lcd->Printf(DriverStationLCD::kUser_Line6, 1, "vy: %.1f", vy);
lcd->UpdateLCD();
tm = GetTime();
}
}
}
/**
* Autonomous code
*/
void Autonomous(void) {
//disable watchdog and start timer
GetWatchdog().SetEnabled(false);
gameTimer->Start();
gameTimer->Reset();
float speed = 0.15; //CHECK-> enough speed to get to the peg in time as shoulder rises slowly
/*
//variables used to hold light sensors' values
rightSensor = 0;
leftSensor = 0;
middleSensor = 0;
*/
while (IsAutonomous())
{
time_to_send++;
if(time_to_send >50 )
{
time_to_send=0;
//Print a message to the Driver Station LCD
dsLCD->Printf(DriverStationLCD::kUser_Line,1, "Autonomous is running, RUN FOR COVER"); //Give output to dsLCD
dsLCD->UpdateLCD();
}
for(int x=0; x<4; x++)
{
if (x==1)
{
myRobot->Drive(0.0,0.0); //Stop the robot initially
}
else if (x==2)
{
myRobot->Drive(speed,0.0); //Move the robot
}
else if (x==3)
{
myRobot->Drive(0.5,0.); //Decrease the speed
}
else
{
myRobot->Drive(0.0,0.0); //If anything else happens, STOP the robot
}
}
break;
}
/*
float releaseVoltage;
bool reachedEndOfLine = false;
int followingLineNumber = 1; //2 is for the 'y' line; reading line number from left to right
//keep following line until robot reaches the 'T' - move shoulder to appropriate position simultaneously
while (reachedEndOfLine == false) {
//read the various sensors
//encoderReading = wheelEncoder->GetDistance();
shoulderPotentiometerReading = (5
-(shoulderPotentiometerChannel->GetVoltage())); // reads the potentiometer at channel 1
// LINE FOLLOWING CODE
leftSensor = left->Get() ? 1 : 0;
middleSensor = middle->Get() ? 1 : 0;
rightSensor = right->Get() ? 1 : 0;
if (leftSensor == 0 && middleSensor == 1 && rightSensor == 1) {
myRobot->Drive(speed, -0.5); // right and middle sensors are on line
} else if (leftSensor == 1 && middleSensor == 1 && rightSensor == 0) {
myRobot->Drive(speed, 0.5); // left and middle sensors are on line
} else if ((leftSensor == 1 && middleSensor == 1 && rightSensor
== 1) || (leftSensor == 1 && middleSensor == 0
&& rightSensor == 1)) {
//CHECK - add support for 'y' line, if necessary
reachedEndOfLine = true;
} else if (leftSensor == 0 && middleSensor == 0 && rightSensor == 0) {
//robot is off the line, stop
myRobot->Drive(speed, 0.0);
} else if (leftSensor == 0 && middleSensor == 1 && rightSensor == 0) {
myRobot->Drive(speed, 0.0); //only middle sensor is on line, go forward
}
//raise the shoulder while following line, depending on which line robot is following
if (followingLineNumber == 1 || followingLineNumber == 3) {
//scoring on the highest, second column, peg
MoveShoulderTo(kTopRowSecondColumn);
releaseVoltage = kTopRowSecondColumn-0.1;
} else {
//scoring on the top row, first or third column, peg
MoveShoulderTo(kTopRow);
releaseVoltage = kTopRow-0.1;
}
}
//drop shoulder a bit, open gripper, and stop driving
while (VoltageReached(shoulderPotentiometerReading, releaseVoltage)
!= 0) {
//at the end of the line, stop
myRobot->Drive(0.0, 0); // stop robot
gripperMotor->SetAngle(55); //opens gripper
MoveShoulderTo(releaseVoltage);
}
//reverse robot and lower shoulder
while (gameTimer->Get() < 14) {
myRobot->Drive(-speed, 0); // reverse robot
MoveShoulderTo(kPickUpPosition);
}
*/
}
/******************************************************
* Drive left & right motors for 2 seconds then stop *
******************************************************/
void Autonomous(void)
{
//increase 2nd RPM
compressor.Start(); // starts the compressor;
bool BRIDGE = bridge.Get();
bool HIGH = high.Get();
bool MIDDLE = middle.Get();
bool TWOSEC = twosec.Get();
bool FIVESEC = fivesec.Get();
int highRPM = 1800; // 1st 1800 short about 5 ft
int secondhighRPM = 1800; //1st 1400 (did not fire)
int rpmForShooter;
DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory
char debugout [100];
// baddog.SetExpiration(30.0);
// baddog.Feed();
dslcd->Clear();
sprintf(debugout,"Bridge=%u",BRIDGE);
dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
sprintf(debugout,"High=%u",HIGH);
dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
sprintf(debugout,"Middle=%u",MIDDLE);
dslcd->Printf(DriverStationLCD::kUser_Line3,3,debugout);
sprintf(debugout,"TwoSec=%u",TWOSEC);
dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
sprintf(debugout,"FiveSec=%u",FIVESEC);
dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
dslcd->UpdateLCD(); // update the Driver Station with the information in the code */
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0)
{
myRobot.Drive(0.0, 0.0);
Wait(10.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //1700 RPM
{
/*myRobot.Drive(-0.5, 0.0);
Wait(3.0);
*/
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1700);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //main autonomous code-default
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM((int)highRPM);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM((int)highRPM);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
// baddog.Feed();
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 1 && FIVESEC == 0)
{
//Wait(2.0);
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
//Wait(3.0);
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1900);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1900);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 1)
{
Wait(5.0);
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
myRobot.Drive(0.0,0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 5)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //position robot-front of key-low RPMs
{
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
//Wait(3.0);
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1600);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 1)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 5)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 1)
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
targeting.SetLMHTarget(BOGEY_H);
while(ShooterTimer.Get()<2)
{
if (targeting.ProcessOneImage())
{
targeting.ChooseBogey();
targeting.MoveTurret();
rpmForShooter = targeting.GetCalculatedRPM();
shooter.setTargetRPM(rpmForShooter);
Wait(0.01);
}
}
targeting.StopPID();
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get() < 7)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(2000); //high RPM
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(2000); //high RPM
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
//baddog.Feed();
myRobot.SetSafetyEnabled(false);
}
void RobotDemo::DriverLCD()
{
if (cycle_counter >= 50)
{
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "RPS Back:%f ",
RPS_back);
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "RPS Front:%f ",
RPS_front);
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "RPS DRPS:%f ",
desired_RPS_control);
#if 0
if (shooter_fire_piston_A->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"Fire ");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"Retracting... ");
}
#endif
//dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"TopLS:%i BotLS:%i ", top_claw_limit_switch->Get(),
// bottom_claw_limit_switch ->Get());
if (top_claw_limit_switch->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "!TOP");
}
else if (!bottom_claw_limit_switch->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "!BOTTOM");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Neither");
}
if (shooter_angle_1->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Up ");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Down ");
}
if (arcadedrive)
{
dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Arcade ");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Tank ");
}
dsLCD->UpdateLCD();
//cycle_counter = 0;
}
//cycle_counter++;
}
void OperatorControl(void)
{
float counter;
timer.Start();
float percent;
deadband = .05;
float pi = 3.141592653589793238462643;
float bpotval, fpotval;
float min = 600, max;
float fchgval = .5;
float bchgval = .5;
while (IsOperatorControl())
{
// comp.checkCompressor();
ShootModeSet();
Shoot(true);
fpotval = fpot.GetValue();
bpotval = bpot.GetValue();
counter = timer.Get();
if (controller.GetRawButton(3))
{
frot.SetSpeed(0);
brot.SetSpeed(0);
flmot.SetSpeed(0);
frmot.SetSpeed(0);
blmot.SetSpeed(0);
brmot.SetSpeed(0);
}
else if (controller.GetRawButton(BTN_L1) == false)
{
if (controller.GetRawButton(7))
{
if (fpotval < 860)
frot.SetSpeed(-0.5);
if (bpotval < 725)
brot.SetSpeed(-0.5);
if (fpotval > 860 and bpotval > 725)
{
frot.Set(0);
brot.Set(0);
flmot.Set(0.5);
frmot.Set(0.5);
blmot.Set(0.5);
brmot.Set(0.5);
}
}
else if (controller.GetRawButton(8))
{
if (fpotval < 860)
frot.SetSpeed(-0.5);
if (bpotval < 725)
brot.SetSpeed(-0.5);
if (fpotval > 860 and bpotval > 725)
{
frot.Set(0);
brot.Set(0);
flmot.Set(-0.5);
frmot.Set(-0.5);
blmot.Set(-0.5);
brmot.Set(-0.5);
}
}
else
{
if (controller.GetRawAxis(4) <= 0)
percent = ((acos(controller.GetRawAxis(3)) / pi));
else if (controller.GetRawAxis(4) > 0)
percent = ((acos(-controller.GetRawAxis(3)) / pi));
fpotval = percent * 550 + 330;
percent = (fpotval - 330) / 550;
bpotval = percent * 500 + 245;
if (fpot.GetValue() < fpotval)
fchgval = -.5;
else if (fpot.GetValue() > fpotval)
fchgval = .5;
if (fpot.GetValue() < fpotval + 10 && fpot.GetValue() > fpotval - 10)
fchgval = 0;
if (bpot.GetValue() > bpotval)
bchgval = -.5;
else if (bpot.GetValue() < bpotval)
bchgval = .5;
if (bpot.GetValue() < bpotval + 20 && bpot.GetValue() > bpotval - 20)
bchgval = 0;
frot.Set(fchgval);
brot.Set(bchgval);
if (pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2) > deadband && controller.GetRawButton(BTN_R1))
{
if (controller.GetRawAxis(4) > 0)
{
flmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
frmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
blmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
brmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
}
else if (controller.GetRawAxis(4) < 0)
{
flmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
frmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
blmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
brmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
}
}
else
{
flmot.Set(0);
frmot.Set(0);
blmot.Set(0);
brmot.Set(0);
}
contaxis4 = 0;
}
}
else
{
if (contaxis4 == 0)
contaxis4 = controller.GetRawAxis(4);
if (controller.GetRawButton(BTN_R1))
{
if (contaxis4 > 0)
{
flmot.Set(-0.25);
frmot.Set(0.25);
blmot.Set(-0.25);
brmot.Set(0.25);
}
else if (contaxis4 < 0)
{
flmot.Set(0.25);
frmot.Set(-0.25);
blmot.Set(0.25);
brmot.Set(-0.25);
}
}
}
if (float (fpot.GetValue()) < min)
min = float (fpot.GetValue());
else if (float (fpot.GetValue()) > max)
max = float (fpot.GetValue());
if (counter >= .1)
{
lcda->Clear();
lcda->Printf(DriverStationLCD::kUser_Line3, 1, "FPot :: %d", fpotval);
lcda->Printf(DriverStationLCD::kUser_Line4, 1, "BPot :: %d", bpotval);
lcda->UpdateLCD();
timer.Reset();
}
}
}
void OperatorControl()
{
timer.Start();
while (IsOperatorControl())
{
x = Stick.GetRawAxis(1);
y = -Stick.GetRawAxis(2);
if (fabs(x) < 0.1)
{
x = 0;
}
if (fabs(y) < 0.1)
{
y = 0;
}
x2 = x*x;
y2 = y*y;
magnitude = pow((x2+y2),0.5);
angle = atan2(y,x)*180/PI;
magnitude = max(magnitude, -1);
magnitude = min(magnitude, 1);
if (angle < 0)
{
angle += 180;
magnitude *= -1;
}
/*front = 730 - (angle * 2.7888);
if ((front + 5) < float(fpot.GetValue()))
{
fturn.Set(0.5);
}
else if ((front - 5) > float(fpot.GetValue()))
{
fturn.Set(-0.5);
}
else
{
fturn.Set(0);
}*/
back = 235 + (angle * 2.8611);
if ((back + 10) < float(bpot.GetValue()))
{
bturn.Set(0.5);
}
else if ((back - 10) > float(bpot.GetValue()))
{
bturn.Set(-0.5);
}
else
{
bturn.Set(0);
}
if (timer.Get() > 0.1)
{
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Front = %5.4f", front);
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Back = %5.4f", back);
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Front Pot = %5.4f", float(fpot.GetValue()));
lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Back Pot = %5.4f", float(bpot.GetValue()));
lcd->Printf(DriverStationLCD::kUser_Line5, 1, "Angle = %5.4f", angle);
lcd->Printf(DriverStationLCD::kUser_Line6, 1, "Magnitude = %5.4f", magnitude);
lcd->UpdateLCD();
timer.Reset();
timer.Start();
}
}
}
void OperatorControl()
{
compressor.Start();
myRobot.SetSafetyEnabled(true);
myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);//Invert rear left Motor
myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);//Invert rear right motor
myRobot.SetInvertedMotor(myRobot.kFrontLeftMotor, true);//Invert rear right motor
myRobot.SetInvertedMotor(myRobot.kFrontRightMotor, true);
DriverStation *ds;
DriverStationLCD *dsLCD;
ds = DriverStation::GetInstance();
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1,1, "Starting Teleop");
dsLCD->UpdateLCD();
while (true)
{
if (!compressor.GetPressureSwitchValue()){
compressor.Start();
}
myRobot.ArcadeDrive(stick);
/*PNEUMATIC CODE*/
if (stick.GetRawButton(8)){
shooterSole.Set(shooterSole.kForward);
}
if (stick.GetRawButton(1)){
shooterSole.Set(shooterSole.kReverse);
}
/*SHOOTER CODE*/
if (stick.GetRawButton(2)){
shooter.SetSpeed(1);
}
if (stick.GetRawButton(4)){
shooter.SetSpeed(-1);
dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Shooter Should be negative");
dsLCD->UpdateLCD();
}
if (!stick.GetRawButton(4) || !stick.GetRawButton(2)){
shooter.SetSpeed(0);
}
/* FORKLIFT CODE*/
if (!stick.GetRawButton(5) || !stick.GetRawButton(6)){
forklift.SetSpeed(0);
}
if (stick.GetRawButton(5)){
forklift.SetSpeed(1);
}
if (stick.GetRawButton(6)){
forklift.SetSpeed(-1);
dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Forklift Should be negative");
dsLCD->UpdateLCD();
}
if (!shoot.Get()){
shooter.SetSpeed(0);
shooterSole.Set(shooterSole.kForward);
}
if (stick.GetRawButton(11)){
//myRobot.m_rearLeftMotor.SetSpeed(1);
//myRobot.m_rearRightMotor.SetSpeed(1);
//myRobot.m_frontLeftMotor.SetSpeed(1);
//myRobot.m_frontRightMotor.SetSpeed(1);
}
//Wait(0.005);// wait for a motor update time
}
}
/**
* 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 OperatorControl()
{
GetWatchdog().SetEnabled(true);
intake->LiftIntake();
LEDLight->Set(Relay::kOff);
while (IsOperatorControl() && !IsDisabled())
{
myRobot->TankDrive(leftStick, rightStick);
rpi->Read();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "L: %f", leftEnco->GetDistance());
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "R: %i", rpi->GetMissingPacketcount());
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "%i", rpi->GetXPos());
lcd->Printf(DriverStationLCD::kUser_Line4, 1, "%i", catapult->GetLoadedLimit1());
lcd->Printf(DriverStationLCD::kUser_Line5, 1, "%i", catapult->GetLoadedLimit2());
lcd->UpdateLCD();
//Driver controls
//Move the intake in if the R-2 trigger is pressed
if(rightStick->GetRawButton(2))
{
intake->RollIn();
}
//Move it out if the R-3 trigger is pressed
else if(rightStick->GetRawButton(3))
{
intake->RollOut();
}
//Intake the ball only far enough for a pass if R-3 is pressed
/*else if(rightStick->GetRawButton(3))
{
intake->GetBallForPass();
}*/
//If none of them are pressed, stop the intake
else
{
intake->Stop();
}
//Catapult stuff
if(rightStick->GetRawButton(1) && leftStick->GetRawButton(1))
{
catapult->StartShoot();
}
else if(leftStick->GetRawButton(10))
{
catapult->StartLoad();
}
else if(leftStick->GetRawButton(11))
{
catapult->StartRelease();
}
//If the right-6 button and l-10 button are pressed, stop the catapult
if(rightStick->GetRawButton(6) && leftStick->GetRawButton(10))
{
catapult->Stop();
}
//These functions need to called all of the time, but don't do anything until
//Their start method is called
catapult->ReleaseHold();
catapult->Shoot();
catapult->Load();
GetWatchdog().Feed();
Wait(0.005); // wait for a motor update time
}
}
void Autonomous()
{
GetWatchdog().SetEnabled(false);
Timer* hotGoalTimer = new Timer();
Timer* reloadTimer = new Timer();
Timer* intakeTimer = new Timer();
Timer* intakeDropTimer = new Timer();
bool goalFound = false;
//bool rightSideHot = false;
hotGoalTimer->Reset();
hotGoalTimer->Start();
gyro->Reset();
leftEnco->Reset();
rightEnco->Reset();
LEDLight->Set(Relay::kForward);
//Find out the type of autonomous we are using
int autonType = (int)SmartDashboard::GetNumber(NUM_BALL_AUTO);
if(autonType == 2)//Set the auton mode to two if we are doing a two ball auto
{
autonMode = TWO_BALL_AUTON;
autonStep = AUTON_TWO_WAIT_FOR_INTAKE;
}
else//Set the auton to one if the value on SD is set to 1 or another random value
{
autonMode = ONE_BALL_AUTON;
autonStep = AUTON_ONE_FIND_HOT;
}
//Bring the intake down
intake->DropIntake();
intakeDropTimer->Reset();
intakeDropTimer->Start();
while(IsAutonomous() && !IsDisabled())
{
rpi->Read();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "L: %f", leftEnco->GetDistance());
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "R: %f", rightEnco->GetDistance());
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "T: %f", hotGoalTimer->Get());
lcd->Printf(DriverStationLCD::kUser_Line4, 1, "i: %i", rpi->GetMissingPacketcount());
lcd->Printf(DriverStationLCD::kUser_Line5, 1, "%i", rpi->GetXPos());
lcd->Printf(DriverStationLCD::kUser_Line6, 1, "%i", rpi->GetYPos());
lcd->UpdateLCD();
LEDLight->Set(Relay::kForward);
if(autonMode == ONE_BALL_AUTON)
{
switch(autonStep)
{
case AUTON_ONE_FIND_HOT:
//Reload the catapult and find the hot goal
rpi->Read();
if(!goalFound)
{
//This is put into an if statement to protect against the
//rpi finding the hot goal and then losing it
goalFound = ((rpi->GetXPos() != RPI_ERROR_VALUE) ||
(rpi->GetYPos() != RPI_ERROR_VALUE));
}
//Wait for the goal to be hot and the intake to move down
if(((goalFound) || (hotGoalTimer->Get() >= 6.75)) && intakeDropTimer->Get() >= INTAKE_DROP_WAIT)
{
autonStep = AUTON_ONE_SHOOT;
catapult->StartRelease();
}
break;
case AUTON_ONE_SHOOT:
//Shoot the catapult
if(!catapult->ReleaseHold())
{
//Move on to the next step when the catapult is released
autonStep = AUTON_ONE_WAIT;
//Start the wait timer
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_ONE_WAIT:
if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
{
autonStep = AUTON_ONE_DRIVE_FORWARDS;
reloadTimer->Stop();
//Start reloading the catapult
catapult->StartLoad();
gyro->Reset();
}
break;
case AUTON_ONE_DRIVE_FORWARDS:
//Drive forwards into the alliance zone and reload the catapult
if((!GyroDrive(0, 0.75, 36)) && (!(bool)catapult->Load()))
{
autonStep = AUTON_END;
}
break;
case AUTON_END:
break;
}
}
else if(autonMode == TWO_BALL_AUTON)
{
switch(autonStep)
{
/*case AUTON_TWO_RELOAD:
//Determine if the hot goal is on the right
if((rpi->GetXPos() != RPI_ERROR_VALUE) &&
(rpi->GetYPos() != RPI_ERROR_VALUE))
{
rightSideHot = true;
}
//Reload the catapult
if(!((bool)catapult->Load()))
{
autonStep = AUTON_TWO_FIRST_SHOOT;
catapult->StartShoot();
}
if((goalFound))
{
//If the goal is found, the right side is hot and we can go to the next step
rightSideHot = true;
autonStep = AUTON_TWO_FIRST_SHOOT;
}
else if(hotGoalTimer->Get() >= 0.5)
{
//If the timer runs of, the right side is not hot and we can go to the next step
rightSideHot = false;
autonStep = AUTON_TWO_FIRST_TURN;
}
break;*/
case AUTON_TWO_WAIT_FOR_INTAKE:
//wait for the intake to drop down
if(intakeDropTimer->Get() >= INTAKE_DROP_WAIT)
{
autonStep = AUTON_TWO_FIRST_SHOOT;
catapult->StartShoot();
}
break;
case AUTON_TWO_FIRST_SHOOT:
if(catapult->GetLoadingState() == LOAD_RELEASE_TENSION)
{
intake->RollIn();
}
if(!((bool)catapult->Shoot()))
{
autonStep = AUTON_TWO_INTAKE;
intakeTimer->Reset();
intakeTimer->Start();
}
break;
case AUTON_TWO_INTAKE:
intake->RollIn();
if(intakeTimer->Get() >= 1.5)
{
intake->Stop();
intakeTimer->Stop();
autonStep = AUTON_TWO_SECOND_SHOOT;
catapult->StartRelease();
}
break;
case AUTON_TWO_SECOND_SHOOT:
if(!catapult->ReleaseHold())
{
autonStep = AUTON_TWO_WAIT;
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_TWO_WAIT:
if(reloadTimer->Get() >= 0.5)
{
reloadTimer->Stop();
leftEnco->Reset();
rightEnco->Reset();
catapult->StartLoad();
gyro->Reset();
autonStep = AUTON_TWO_DRIVE_FORWARDS;
}
break;
case AUTON_TWO_DRIVE_FORWARDS:
if((!GyroDrive(0, 0.9, 36)) && (!((bool)catapult->Load())))
{
autonStep = AUTON_END;
}
break;
/*case AUTON_TWO_FIRST_TURN:
//Turn to the left 5* if the right goal is not hot
if(!rightSideHot)
{
if(!GyroTurn(-5, 0.5))
{
autonStep = AUTON_TWO_FIRST_SHOOT;
}
}
else
{
autonStep = AUTON_TWO_FIRST_SHOOT;
}
break;
case AUTON_TWO_FIRST_SHOOT:
//Release the catapult to shoot
if(!catapult->ReleaseHold())
{
autonStep = AUTON_TWO_FIRST_WAIT;
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_TWO_FIRST_WAIT:
if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
{
autonStep = AUTON_TWO_SECOND_TURN;
catapult->StartLoad();
reloadTimer->Stop();
}
break;
case AUTON_TWO_SECOND_TURN:
//Turn the robot so it's facing forwards and reload the catapult
if((!GyroTurn(0, 0.5)) && (!(bool)catapult->Load()))
{
autonStep = AUTON_TWO_GET_SECOND_BALL;
}
break;
case AUTON_TWO_GET_SECOND_BALL:
//Start up the intake and drive back to pick up the second ball
intake->RollIn();
if(!GyroDrive(0, -0.5, -12))
{
autonStep = AUTON_TWO_THIRD_TURN;
leftEnco->Reset();
rightEnco->Reset();
}
break;
case AUTON_TWO_THIRD_TURN:
//If the right goal was originally hot, turn left
if(rightSideHot)
{
if(!GyroTurn(-5, 0.5))
{
autonStep = AUTON_TWO_SECOND_SHOOT;
}
}
else
{
autonStep = AUTON_TWO_SECOND_SHOOT;
}
break;
case AUTON_TWO_SECOND_SHOOT:
intake->Stop();
if(!catapult->ReleaseHold())
{
autonStep = AUTON_TWO_SECOND_WAIT;
reloadTimer->Reset();
reloadTimer->Start();
}
break;
case AUTON_TWO_SECOND_WAIT:
if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
{
autonStep = AUTON_TWO_DRIVE_FORWARDS;
catapult->StartLoad();
reloadTimer->Stop();
}
break;
case AUTON_TWO_DRIVE_FORWARDS:
if(!GyroDrive(0, 0.75, 60) && (!(bool)catapult->Load()))
{
autonStep = AUTON_END;
}
break;*/
case AUTON_END:
break;
}
}
}
}
