Simple Kenect Driver

<< | FRC | SmartDashboard >>

Here are 7 source files to get us started that controls the robot from the joystick and the kenect, along with SmartDashboard commands. If you build your own CommandRobot project, you may pick a different class and package convention. Here, I called it "SimpleKinectDriverBrobot", and the package name i used was "org.usfirst.frc4101" -- if you use your own, then the imports need to be updated to your package naming convention. I gathered all this from the videos and documentation you can find at http://www.mathorama.com/frc/ .

FIRST TIME YOU TRY THIS, PLACE THE ROBOT ON BLOCKS (WHEELS OFF THE GROUND) SO IT WON'T GO ANYWHERE!

OI.java


package org.usfirst.frc4101;

import org.usfirst.frc4101.commands.DriveWithJoystick;
import org.usfirst.frc4101.commands.DriveWithKenect;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.KinectStick;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class OI {
	//adjust for joystick axis 1.0 means the axis goes from -1.0 to 1.0
	public static final double max=1.0; 

	// Process operator interface input here.
	Joystick joystick = new Joystick(1);
	Button 	button1 = new JoystickButton(joystick, 1),
			button2 = new JoystickButton(joystick, 2);
	KinectStick leftArm = new KinectStick(1),
				rightArm = new KinectStick(2);



	public OI() {
		// bind buttons to commands here, for example to test the driving modes:
		button1.whenPressed(new DriveWithJoystick() );
		button2.whenPressed(new DriveWithKenect() );
		// For testing, you can activate your commands from the SmartDashboard:
		SmartDashboard.putData(new DriveWithJoystick());
		SmartDashboard.putData(new DriveWithKenect());
	}

	public double getLeftArmSpeed() {
		// For safety, reduce to 33% of speed
		return leftArm.getY()*.33;
	}
	public double getRightArmSpeed() {
		// For safety, reduce to 33% of speed
		return rightArm.getY()*.33;
	}

	public double getLeftSpeed() {
		// see http://home.kendra.com/mauser/Joystick.html for algorithm
		double x=-1.0*joystick.getX(); //invert x
		double y=joystick.getY();
		double v=(max-Math.abs(x))*(y/max)+y;
		double w=(max-Math.abs(y))*(x/max)+x;		
		return .5*(v-w);
	}
	public double getRightSpeed() {
		// see http://home.kendra.com/mauser/Joystick.html for algorithm
		double x=-1.0*joystick.getX(); //invert x
		double y=joystick.getY();
		double v=(max-Math.abs(x))*(y/max)+y;
		double w=(max-Math.abs(y))*(x/max)+x;		
		return .5*(v+w);
	}
	public void updateStatus(){
		// if we had a sensor like a pot or a rangefinder, we can see the status 
		// on the smartDashboard like this:
		SmartDashboard.putDouble(" joystick X", joystick.getX());	
		SmartDashboard.putDouble(" joystick Y", joystick.getY());	
		SmartDashboard.putDouble(" kenect right", getRightArmSpeed());
		SmartDashboard.putDouble(" kenect left", getLeftArmSpeed());	
	}
}


RobotMap.java

package org.usfirst.frc4101;

/**
 * The RobotMap is a mapping from the ports sensors and actuators are wired into
 * to a variable name. This provides flexibility changing wiring, makes checking
 * the wiring easier and significantly reduces the number of magic numbers
 * floating around.
 */
public class RobotMap {
    //Motors
    public static final int 
		leftMotor = 1,
    		rightMotor = 2;
 // we'll add later the collector motor
    // we'll add later the elevator motor
    // wel'll add the the tilt shooter motor
    // we'll add later the actuator to shoot
    // we'll add later the the actuator to deploy the wedge

    // Sensors

    // we may wish to at a pot for the motor that delivers the ball to shooter
    // we may wish to add pot for the angle of the shooter

    // the analog module stuff:
    // public static final int 
    		  //shootAnglePot = ???3???;


}

SimpleKinectDriverBrobot.java

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package org.usfirst.frc4101;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.usfirst.frc4101.commands.CommandBase;
import org.usfirst.frc4101.commands.DriveWithKenect;


/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory. (this is done for you if you use the IDE's 'Refactor' command)
 */
public class SimpleKinectDriverBrobot extends IterativeRobot {

    Command autonomousCommand;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        // instantiate the command used for the autonomous period
        autonomousCommand = new DriveWithKenect();

        // Initialize all subsystems
        CommandBase.init();
        // This is good for debugging to see what commands are running, 
        // which you can cancel from SmartDashboard:
        SmartDashboard.putData(Scheduler.getInstance());
    }

    public void autonomousInit() {
        // schedule the autonomous command (example)
        autonomousCommand.start();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }

    public void teleopInit() {
		// This makes sure that the autonomous stops running when
		// teleop starts running. If you want the autonomous to 
		// continue until interrupted by another command, remove
		// this line or comment it out.
		autonomousCommand.cancel();
		// for debugging:
		updateStatus();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
        updateStatus();//for debugging
    }

    public void disabledPeriodic(){
    		updateStatus(); // for debugging
    }
    public void updateStatus() {
    	    // For debugging we can call our subsystem's update status
    	    // to see what is going on in the SmartDashboard
    		CommandBase.drivetrain.updateStatus();
    		CommandBase.oi.updateStatus();
    }
}

CommandBase.java

package org.usfirst.frc4101.commands;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc4101.OI;
import org.usfirst.frc4101.subsystems.DriveTrain;


/**
 * The base for all commands. All atomic commands should subclass CommandBase.
 * CommandBase stores creates and stores each control system. To access a
 * subsystem elsewhere in your code in your code use CommandBase.exampleSubsystem
 * @author Author
 */
public abstract class CommandBase extends Command {

    public static OI oi;
    // Create a single static instance of all of your subsystems
    public static DriveTrain drivetrain = new DriveTrain();
    // we'll add later the collector motor
    // we'll add later the elevator motor
    // wel'll add the the tilt shooter motor
    // we'll add later the actuator to shoot
    // we'll add later the the actuator to deploy the wedge

    public static void init() {
        // This MUST be here. If the OI creates Commands (which it very likely
        // will), constructing it during the construction of CommandBase (from
        // which commands extend), subsystems are not guaranteed to be
        // yet. Thus, their requires() statements may grab null pointers. Bad
        // news. Don't move it.
        oi = new OI();

        // Show what command your subsystem is running on the SmartDashboard
        SmartDashboard.putData(drivetrain);

    }

    public CommandBase(String name) {
        super(name);
    }

    public CommandBase() {
        super();
    }
}

DriveWithJoystick.java


package org.usfirst.frc4101.commands;

public class DriveWithJoystick extends CommandBase {

    public DriveWithJoystick() {
        // Use requires() here to declare subsystem dependencies
        requires(drivetrain);

    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	    		drivetrain.tankDrive(
    	    				oi.getLeftSpeed(), 
    	    				oi.getRightSpeed());

    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	    // if this is default command, always return false
        return false;

    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}

DriveWithKenect.java


package org.usfirst.frc4101.commands;

public class DriveWithKenect extends CommandBase {

    public DriveWithKenect() {
        // Use requires() here to declare subsystem dependencies
        requires(drivetrain);
        //For safety, we timeout after 15 seconds
        this.setTimeout(15.0);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	    		drivetrain.tankDrive(
    	    				oi.getLeftArmSpeed(), 
    	    				oi.getRightArmSpeed());

    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	    // if this is default command, always return false
        //return false;

    		//for safety, we exit after the time we specified in constructor
    		return this.isTimedOut(); 
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}

DriveTrain.java

package org.usfirst.frc4101.subsystems;

import org.usfirst.frc4101.RobotMap;
import org.usfirst.frc4101.commands.DriveWithJoystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class DriveTrain extends Subsystem {
	RobotDrive drive;
	public DriveTrain(){
		super ("DriveTrain");
		drive = new RobotDrive(RobotMap.leftMotor, RobotMap.rightMotor);
	}
	public void initDefaultCommand() {

		setDefaultCommand(new DriveWithJoystick());
	}
	public void tankDrive(double left, double right){
		drive.tankDrive(left, right);
	}
	public void updateStatus(){
		// if we had a sensor like a pot or a rangefinder, we can see the status 
		// on the smartDashboard like this:
		//SmartDashboard.putDouble("Shoot Angle", shootAnglePot.getVoltage());
		double time=this.getCurrentCommand().timeSinceInitialized();
		SmartDashboard.putDouble("Time running", time);
	}

}