Home
FIRST Robotics Team 2648 Infinite Loop
Home of Maine's FIRST Offseason Event

Breakaway Robot Code


/*----------------------------------------------------------------------------*
 *
 * Basic Robot Code Compiled By Blake from Team 2648
 *
 * Questions? blake (at) team2648 [dot] com
 *
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Timer;
 
 
/* Imports are not needed when the package is edu.wpi.first.wpilibj; contrary to that of the basic netbeans frc project
import edu.wpi.first.wpilibj.RobotDrive;*/
public class SimpleSoccer extends SimpleRobot
{
 
    private Jaguar Rdrive, Ldrive;
    private RobotDrive drivetrain;
    private Joystick LS; //One Joystick controls the robot
    private Joystick RS; //controls camera
    private Victor winch; // The winch that controls the lift
    private Jaguar roller; // Controls the soccer balls to placement in front of kicker
    private Victor kicker; // The kicker
    private Victor quickRelease; // The motor that releases the arm
    private DigitalInput liftDwn;
    private DigitalInput kickerReady;
    private DigitalInput switch1;
    private DigitalInput switch2;
    private DigitalInput haveBall;
    private Gyro gSensor;
    private DriverStationLCD dsLCD;
    private DriverStation ds;
    private int count, zone;
    private String str1, str2, str3;
    private boolean driveCount = true;
    private Timer time;
    private int slot = 4;
 
    public SimpleSoccer()
    {
 
	getWatchdog().setEnabled(false); // Stupid WATCHDOG -- Doubtfull that this is legal
	dsLCD = DriverStationLCD.getInstance();
	ds = DriverStation.getInstance(); //We use this to set lights on our HUD and get info from switches
 
	Rdrive = new Jaguar(slot, 1);
	Ldrive = new Jaguar(slot, 2);
	drivetrain = new RobotDrive(Rdrive, Ldrive); // create RobotDrive
 
	// joysticks
	LS = new Joystick(1); // Driving during teleop
	RS = new Joystick(2); // Driving during finale
 
	liftDwn = new DigitalInput(slot, 14); //Simple microswitch
	kickerReady = new DigitalInput(slot, 13); //Simple microswitch (Is the kicker cocked?)
	switch1 = new DigitalInput(slot, 1); //Simple microswitch for auto code
	switch2 = new DigitalInput(slot, 2); //Simple microswitch for auto code
	haveBall = new DigitalInput(slot, 5); // Optical sensor - in ball caputre device
 
	winch = new Victor(slot, 3); // and the winch to lift the robot in finale
	kicker = new Victor(slot, 5); // and the kicker to kick the ball
	roller = new Jaguar(slot, 4); // and the roller to hold the ball
	quickRelease = new Victor(slot, 6); // and the Quick release to remove the arm tiedown
 
	gSensor = new Gyro(1);
 
	zone = 2; //Default beginning zone if auto switches are missing
	time = new Timer(); // To allow for the calculation of when to iluminate the light on the HUD
    }
 
    public void autonomous()
    {
	dsLCD.updateLCD();
	getWatchdog().setEnabled(false); // Stupid WATCHDOG
	count = 0;  // represents number of kicks
	zone = 2;
 
	while (isAutonomous())
	{
	    if (switch1.get() && switch2.get())
		zone = 1;
	    else if (switch1.get() && !switch2.get())
		zone = 2;
	    else if (!switch1.get() && !switch2.get())
		zone = 3;
	    else
		zone = 2;
 
	    // This once was to print code to the DS screen
	    //dsLCD.println(DriverStationLCD.Line.kMain6, 1, "");
 
	    //This activates the HUD light for when we have captured a ball
	    ds.setDigitalOut(1, !haveBall.get());
 
	    //Some information about the auto code running.
	    str1 = "count: " + count;
	    str2 = "zone: " + zone;
	    dsprint(str1, 1);
	    dsprint(str2, 15);
 
	    //// Psuedo code
	    //=>One ball
	    //Driveforeward, kick, turn, backup
	    //=>Two Balls
	    // fwd, kick, fwd, kick, turn, backup
	    //=>Three Balls
	    // fwd, kick, fwd, kick, fwd, kick, turn, backup
	    str3 = "Current Gyro: " + gSensor.getAngle(); // Remember it zeros at startup
	    dsLCD.println(DriverStationLCD.Line.kUser4, 1, str3);
 
	    reloadKicker();
	    if (count < zone)
		if (!haveBall.get())
		{
		    drivetrain.setLeftRightMotorOutputs(0, 0);
		    if (kickerReady.get())
		    {
			kick();
			Timer.delay(1);
			count++;
		    }
		} else
		    drivetrain.setLeftRightMotorOutputs(-.4, -.4);
	    else
	    {
		str3 = "Current Gyro: " + gSensor.getAngle();
		dsLCD.println(DriverStationLCD.Line.kUser4, 1, str3);
		// Former gyro turn code
//                if (gSensor.getAngle() > -100 && gSensor.getAngle() < 10) {
//                    drivetrain.setLeftRightMotorSpeeds(.75, -.75);
////                    reloadKicker();
//                }
 
		reloadKicker();
		if (kickerReady.get())
		    return;
		else if (driveCount)
		{
		    drivetrain.setLeftRightMotorOutputs(.5, .5);
		    Timer.delay(.5);

		    Timer.delay(.25);
		    stop();
		    driveCount = false;
		}
	    }
	}
    }
 
    public void operatorControl()
    {
	getWatchdog().setEnabled(false); // Stupid WATCHDOG
	time.reset();
	time.start(); // For the HUD light
 
	while (isOperatorControl()) //Let's loop while in operator control
	{
	    reloadKicker();
//=======================================================
	    // Kicker Code
	    if (LS.getTrigger())
		kick();
	    reloadKicker();
 
//=======================================================
	    // Winch Code
	    if (liftDwn.get()) // if the winch is down, only let it go up.
		winchOnlyUp();
	    else
		winchUpAndDown();
//=======================================================
	    //Drive Code
	    if (liftDwn.get()) // In the finale we switch drivers
		drivetrain.arcadeDrive(LS);
	    else
		drivetrain.arcadeDrive(RS);
//=======================================================
	    //(HUD) heads up display code
	    ds.setDigitalOut(1, !haveBall.get());
 
//=======================================================
 
	    //Printout the value of the timer. @todo make this match teh field. Is there a method?
	    dsLCD.println(DriverStationLCD.Line.kUser4, 1, new Double(time.get()).toString());
 
	    dsLCD.updateLCD();
 
	    // If the timer is greater than 84 sec light up the hud.
	    if (time.get() > 85)
		ds.setDigitalOut(2, true);
	    else
		ds.setDigitalOut(2, false);
	}
	stop();
    }
 
    public void kick()
    {
	if (liftDwn.get())
	{
	    kicker.set(1);
	    //==================
	    // This will allow for roller back speed
	    // aka kicker distance
	    //roller.set(-1);
	    Timer.delay(.05);
	    
 
	    // @todo disable kicking when travling over bump!!!!
	} else
	    reloadKicker();
    }
 
     // sets all motors to spped zero
    public void stop()
    {  
	kicker.set(0);
	roller.set(0);
	drivetrain.drive(0, 0);
	winch.set(0);
   }
 
 
    public void reloadKicker()
    {   // Automatically recock the kicker
	if (!kickerReady.get())
	    kicker.set(0.75);
	else
	{
	    kicker.set(0);
	    if (liftDwn.get())
		roller.set(-.61);
	    else
		roller.set(0);
	}
    }
 
    // Only allow the arm to move up
    private void winchOnlyUp()
    {   
	if (RS.getRawButton(3))
	    quickRelease.set(-1);
	else if (RS.getRawButton(7))
	    winch.set(1);
	else
	{
	    quickRelease.set(0);
	    winch.set(0);
	}
    }

    private void winchUpAndDown()
    {
	if (RS.getRawButton(2) || RS.getRawButton(1))
	    winch.set(-1);
	else if (RS.getRawButton(3))
	    quickRelease.set(-1);
	else if (RS.getRawButton(7))
	    winch.set(1);
	else
	{
	    quickRelease.set(0);
	    winch.set(0);
	}
    }
 
    private void dsprint(String str, int col)
    {
	dsLCD.println(DriverStationLCD.Line.kMain6, col, str);
	dsLCD.updateLCD();

    }
}

/*----------------------------------------------------------------------------*
 *
 * Basic Robot Code Compiled By Blake from Team 2648
 *
 * Questions? blake (at) team2648 [dot] com
 *
/*----------------------------------------------------------------------------*/

  • Offseason Event
  • About Our Team
    • History
    • Team Members
    • Mentors
    • Alumni
    • Fundraising
    • Accomplishments
    • Engineering Inspiration
  • Challenge
    • Stronghold 2016
    • Recycle Rush 2015
    • Aerial Assist 2014
    • Ultimate Ascent 2013
    • Rebound Rumble 2012
    • Logomotion 2011
    • Breakaway 2010
    • Lunacy 2009
    • Overdrive 2008
  • Our Robots
  • Sponsors
    • Sponsor Us!
  • Resources
    • Electrical System Overview
    • FRC Vocabulary
  • US FIRST
  • Contact Us
  • Social Media
  • Offseason Event
  • About Our Team
    • History
    • Team Members
    • Mentors
    • Alumni
    • Fundraising
    • Accomplishments
    • Engineering Inspiration
  • Challenge
    • Stronghold 2016
    • Recycle Rush 2015
    • Aerial Assist 2014
    • Ultimate Ascent 2013
    • Rebound Rumble 2012
    • Logomotion 2011
    • Breakaway 2010
    • Lunacy 2009
    • Overdrive 2008
  • Our Robots
  • Sponsors
    • Sponsor Us!
  • Resources
    • Electrical System Overview
    • FRC Vocabulary
  • US FIRST
  • Contact Us
  • Social Media

For the Inspiration and Recognition of Science and Technology

Picture
To create a world where science and technology are celebrated and where young people dream of becoming science and technology heroes.

firstinspires.org

© 2023 Infinite Loop Robotics. FRC Team 2648 All Rights Reserved
Copyrights are the property of their respective owners.
About our website | Mail | Contact Us | Visit our Facebook Page


Powered by Create your own unique website with customizable templates.