added basic pid distance driving functionality
This commit is contained in:
parent
877b7aaf8d
commit
901aa58894
@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.buttons.Button;
|
|||||||
import edu.wpi.first.wpilibj.command.Command;
|
import edu.wpi.first.wpilibj.command.Command;
|
||||||
import edu.wpi.first.wpilibj.buttons.JoystickButton;
|
import edu.wpi.first.wpilibj.buttons.JoystickButton;
|
||||||
import org.usfirst.frc.team2059.robot.commands.LogEncoder;
|
import org.usfirst.frc.team2059.robot.commands.LogEncoder;
|
||||||
|
import org.usfirst.frc.team2059.robot.commands.PIDDrive;
|
||||||
import org.usfirst.frc.team2059.robot.commands.MoveArm;
|
import org.usfirst.frc.team2059.robot.commands.MoveArm;
|
||||||
import org.usfirst.frc.team2059.robot.commands.SetArmPosition;
|
import org.usfirst.frc.team2059.robot.commands.SetArmPosition;
|
||||||
import org.usfirst.frc.team2059.robot.commands.SetShooterState;
|
import org.usfirst.frc.team2059.robot.commands.SetShooterState;
|
||||||
@ -33,6 +34,7 @@ public class OI {
|
|||||||
//joystickButtons[0][0].whenPressed(new LogEncoder());
|
//joystickButtons[0][0].whenPressed(new LogEncoder());
|
||||||
joystickButtons[0][0].whileHeld(new SpinRollers(1));
|
joystickButtons[0][0].whileHeld(new SpinRollers(1));
|
||||||
joystickButtons[0][1].whileHeld(new SetShooterState(true));
|
joystickButtons[0][1].whileHeld(new SetShooterState(true));
|
||||||
|
// joystickButtons[0][2].whileHeld(new PIDDrive(400));
|
||||||
|
|
||||||
joystickButtons[1][0].whileHeld(new MoveArm(0.5));
|
joystickButtons[1][0].whileHeld(new MoveArm(0.5));
|
||||||
joystickButtons[1][1].whileHeld(new MoveArm(-0.5));
|
joystickButtons[1][1].whileHeld(new MoveArm(-0.5));
|
||||||
|
@ -16,6 +16,7 @@ public class Robot extends IterativeRobot {
|
|||||||
chooser = new SendableChooser();
|
chooser = new SendableChooser();
|
||||||
SmartDashboard.putData("Auto mode", chooser);
|
SmartDashboard.putData("Auto mode", chooser);
|
||||||
SmartDashboard.putData("MainArm",CommandBase.mainArm.getPIDController());
|
SmartDashboard.putData("MainArm",CommandBase.mainArm.getPIDController());
|
||||||
|
SmartDashboard.putData("LeftEncoderController",CommandBase.driveBase.getLeftController());
|
||||||
SmartDashboard.putBoolean("CompressorEnabled",true);
|
SmartDashboard.putBoolean("CompressorEnabled",true);
|
||||||
//Automatically determine if rolling in or rolling out
|
//Automatically determine if rolling in or rolling out
|
||||||
SmartDashboard.putBoolean("SmartRollers",true);
|
SmartDashboard.putBoolean("SmartRollers",true);
|
||||||
@ -45,6 +46,7 @@ public class Robot extends IterativeRobot {
|
|||||||
Scheduler.getInstance().run();
|
Scheduler.getInstance().run();
|
||||||
SmartDashboard.putNumber("ArmAngleRaw",CommandBase.mainArm.getRaw());
|
SmartDashboard.putNumber("ArmAngleRaw",CommandBase.mainArm.getRaw());
|
||||||
SmartDashboard.putNumber("ArmAngleDegrees",CommandBase.mainArm.getDegrees());
|
SmartDashboard.putNumber("ArmAngleDegrees",CommandBase.mainArm.getDegrees());
|
||||||
|
SmartDashboard.putNumber("tmpRotations",CommandBase.driveBase.getLeftRotations());
|
||||||
CommandBase.pneumatics.setCompressorEnabled(SmartDashboard.getBoolean("CompressorEnabled"));
|
CommandBase.pneumatics.setCompressorEnabled(SmartDashboard.getBoolean("CompressorEnabled"));
|
||||||
System.out.println(CommandBase.mainArm.getDegrees());
|
System.out.println(CommandBase.mainArm.getDegrees());
|
||||||
}
|
}
|
||||||
|
@ -5,11 +5,11 @@ public class RobotMap {
|
|||||||
public static int driveLeftMotorTwo = 2;
|
public static int driveLeftMotorTwo = 2;
|
||||||
public static int driveRightMotorOne = 3;
|
public static int driveRightMotorOne = 3;
|
||||||
public static int driveRightMotorTwo = 4;
|
public static int driveRightMotorTwo = 4;
|
||||||
public static int driveRightEncoder = 0;
|
public static int driveLeftEncoderA = 0;
|
||||||
public static int driveLeftEncoder = 1;
|
public static int driveLeftEncoderB = 1;
|
||||||
//Arm
|
//Arm
|
||||||
public static double zeroDegrees = 2.08;
|
public static double zeroDegrees = 2.019;
|
||||||
public static double ninetyDegrees = 3.54;
|
public static double ninetyDegrees = 3.512;
|
||||||
public static int armPot = 0;
|
public static int armPot = 0;
|
||||||
public static int armLeftMotor = 5;
|
public static int armLeftMotor = 5;
|
||||||
public static int armRightMotor = 6;
|
public static int armRightMotor = 6;
|
||||||
@ -25,7 +25,7 @@ public class RobotMap {
|
|||||||
public static int armStopSolenoidOne = 4;
|
public static int armStopSolenoidOne = 4;
|
||||||
public static int armStopSolenoidTwo = 5;
|
public static int armStopSolenoidTwo = 5;
|
||||||
//Misc
|
//Misc
|
||||||
public static int mainArmPresetCollect = -3;
|
public static int mainArmPresetCollect = -5;
|
||||||
public static int mainArmPresetTraverse = 11;
|
public static int mainArmPresetTraverse = 11;
|
||||||
public static int mainArmPresetCloseShot = 90;
|
public static int mainArmPresetCloseShot = 90;
|
||||||
public static int mainArmPresetFarShot = 70;
|
public static int mainArmPresetFarShot = 70;
|
||||||
|
32
src/org/usfirst/frc/team2059/robot/commands/PIDDrive.java
Normal file
32
src/org/usfirst/frc/team2059/robot/commands/PIDDrive.java
Normal file
@ -0,0 +1,32 @@
|
|||||||
|
package org.usfirst.frc.team2059.robot.commands;
|
||||||
|
import org.usfirst.frc.team2059.robot.commands.CommandBase;
|
||||||
|
import org.usfirst.frc.team2059.robot.Robot;
|
||||||
|
/**
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
public class PIDDrive extends CommandBase {
|
||||||
|
double count;
|
||||||
|
public PIDDrive(double c) {
|
||||||
|
requires(driveBase);
|
||||||
|
count=c;
|
||||||
|
}
|
||||||
|
// Called just before this Command runs the first time
|
||||||
|
protected void initialize() {
|
||||||
|
}
|
||||||
|
// Called repeatedly when this Command is scheduled to run
|
||||||
|
protected void execute() {
|
||||||
|
driveBase.pidDrive(count);
|
||||||
|
}
|
||||||
|
// Make this return true when this Command no longer needs to run execute()
|
||||||
|
protected boolean isFinished() {
|
||||||
|
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() {
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// vim: sw=2:ts=2:sts=2
|
@ -19,7 +19,7 @@ public class SetArmPosition extends CommandBase {
|
|||||||
//Move the arm stop
|
//Move the arm stop
|
||||||
if(pos==RobotMap.mainArmPresetCollect){
|
if(pos==RobotMap.mainArmPresetCollect){
|
||||||
pneumatics.setArmStopState(true);
|
pneumatics.setArmStopState(true);
|
||||||
}else if((pos!=RobotMap.mainArmPresetCollect)&&(mainArm.getDegrees()>15)){
|
}else if((pos!=RobotMap.mainArmPresetCollect)&&(mainArm.getDegrees()>2)){
|
||||||
pneumatics.setArmStopState(false);
|
pneumatics.setArmStopState(false);
|
||||||
}
|
}
|
||||||
mainArm.enable();
|
mainArm.enable();
|
||||||
|
@ -2,12 +2,19 @@ package org.usfirst.frc.team2059.robot.subsystems;
|
|||||||
import org.usfirst.frc.team2059.robot.RobotMap;
|
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||||
import org.usfirst.frc.team2059.robot.commands.Drive;
|
import org.usfirst.frc.team2059.robot.commands.Drive;
|
||||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||||
|
import edu.wpi.first.wpilibj.PIDController;
|
||||||
|
import edu.wpi.first.wpilibj.PIDOutput;
|
||||||
|
import edu.wpi.first.wpilibj.Encoder;
|
||||||
import edu.wpi.first.wpilibj.CANTalon;
|
import edu.wpi.first.wpilibj.CANTalon;
|
||||||
public class DriveBase extends Subsystem {
|
public class DriveBase extends Subsystem {
|
||||||
CANTalon leftMotorOne = new CANTalon(RobotMap.driveLeftMotorOne);
|
CANTalon leftMotorOne = new CANTalon(RobotMap.driveLeftMotorOne);
|
||||||
CANTalon leftMotorTwo = new CANTalon(RobotMap.driveLeftMotorTwo);
|
CANTalon leftMotorTwo = new CANTalon(RobotMap.driveLeftMotorTwo);
|
||||||
CANTalon rightMotorOne = new CANTalon(RobotMap.driveRightMotorOne);
|
CANTalon rightMotorOne = new CANTalon(RobotMap.driveRightMotorOne);
|
||||||
CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorTwo);
|
CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorTwo);
|
||||||
|
Encoder leftEncoder = new Encoder(RobotMap.driveLeftEncoderA,RobotMap.driveLeftEncoderB,false,Encoder.EncodingType.k2X);
|
||||||
|
|
||||||
|
PIDController leftEncoderController = new PIDController(0.02,0.002,0.017,leftEncoder,new MotorsPIDOutput());
|
||||||
|
|
||||||
public void initDefaultCommand() {
|
public void initDefaultCommand() {
|
||||||
setDefaultCommand(new Drive());
|
setDefaultCommand(new Drive());
|
||||||
}
|
}
|
||||||
@ -17,5 +24,26 @@ public class DriveBase extends Subsystem {
|
|||||||
rightMotorOne.set(y + (x + z));
|
rightMotorOne.set(y + (x + z));
|
||||||
rightMotorTwo.set(y + (x + z));
|
rightMotorTwo.set(y + (x + z));
|
||||||
}
|
}
|
||||||
|
public void pidDrive(double setpoint){
|
||||||
|
leftEncoder.reset();
|
||||||
|
leftEncoderController.enable();
|
||||||
|
leftEncoderController.setSetpoint(setpoint);
|
||||||
|
}
|
||||||
|
public PIDController getLeftController(){
|
||||||
|
return leftEncoderController;
|
||||||
|
}
|
||||||
|
public double getLeftRotations(){
|
||||||
|
return leftEncoder.get();
|
||||||
|
}
|
||||||
|
|
||||||
|
public class MotorsPIDOutput implements PIDOutput{
|
||||||
|
@Override
|
||||||
|
public void pidWrite(double output){
|
||||||
|
leftMotorOne.pidWrite(output);
|
||||||
|
leftMotorTwo.pidWrite(output);
|
||||||
|
rightMotorOne.pidWrite(-output);
|
||||||
|
rightMotorTwo.pidWrite(-output);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
// vim: sw=2:ts=2:sts=2
|
// vim: sw=2:ts=2:sts=2
|
||||||
|
@ -9,7 +9,7 @@ public class EncoderBase extends Subsystem {
|
|||||||
public EncoderBase() {
|
public EncoderBase() {
|
||||||
}
|
}
|
||||||
//Encoder enc = new Encoder(0, 1, false, Encoder.EncodingType.k1X);
|
//Encoder enc = new Encoder(0, 1, false, Encoder.EncodingType.k1X);
|
||||||
Encoder enc = new Encoder(0, 1);
|
Encoder enc = new Encoder(8, 9);
|
||||||
public void initDefaultCommand() {
|
public void initDefaultCommand() {
|
||||||
//TODO: Not sure if we need a default command, not setting one
|
//TODO: Not sure if we need a default command, not setting one
|
||||||
//TODO: These are just defaults, they wil lneed to be changed
|
//TODO: These are just defaults, they wil lneed to be changed
|
||||||
|
Loading…
Reference in New Issue
Block a user