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);
}
}