team2059/ford-java
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

vision almost works

Browse Source
This commit is contained in:
Adam Long 2016-10-13 19:09:30 +00:00
parent d0c46f6e12
commit 702df28cb2
8 changed files with 74 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/org/usfirst/frc/team2059/robot/OI.java
View File

@ -15,6 +15,7 @@ import org.usfirst.frc.team2059.robot.commands.shooter.SetShooterState;
import org.usfirst.frc.team2059.robot.commands.shooter.SetArmStopState;
import org.usfirst.frc.team2059.robot.commands.shooter.ShootAtSpeed;
import org.usfirst.frc.team2059.robot.commands.shooter.SpinRollers;
import org.usfirst.frc.team2059.robot.commands.shooter.AlignVertical;
import org.usfirst.frc.team2059.robot.commands.visionhelper.AlignHorizontal;
import org.usfirst.frc.team2059.robot.RobotMap;
/**
@ -39,6 +40,7 @@ public class OI {
//joystickButtons[0][0].whenPressed(new LogEncoder());
// joystickButtons[0][0].whileHeld(new SetShooterState(true));
joystickButtons[0][0].whileHeld(new AlignHorizontal());
joystickButtons[0][1].whileHeld(new SetShooterState(true));
joystickButtons[0][2].whileHeld(new SpinRollers(-0.5, false));
joystickButtons[0][3].whileHeld(new SpinRollers(1, false));
joystickButtons[0][4].whileHeld(new DriveStraight(0.25));
@ -46,13 +48,14 @@ public class OI {
joystickButtons[0][6].whileHeld(new SpinRollers(1, true));
joystickButtons[0][9].whileHeld(new Drive());
// joystickButtons[0][2].whileHeld(new PIDDrive(400));
joystickButtons[1][0].whileHeld(new MoveArm(1));
joystickButtons[1][1].whileHeld(new MoveArm(-1));
//joystickButtons[1][2].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCollect));
joystickButtons[1][0].whileHeld(new AlignVertical());
joystickButtons[1][2].whileHeld(new ResetLower(-1));
joystickButtons[1][3].whileHeld(new SetArmPosition(RobotMap.mainArmPresetTraverse));
joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot));
joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetLowShot));
joystickButtons[1][7].whileHeld(new MoveArm(1));
joystickButtons[1][8].whileHeld(new MoveArm(-1));
}
public Joystick[] getJoysticks() {
return joysticks;

1
src/org/usfirst/frc/team2059/robot/Robot.java
View File

@ -83,6 +83,7 @@ public class Robot extends IterativeRobot {
SmartDashboard.putNumber("sonarDistance", CommandBase.mainArm.getSonarDistance());
SmartDashboard.putNumber("sonarDistanceRaw", CommandBase.mainArm.getSonarDistanceRaw());
SmartDashboard.putNumber("shooterAngleError", CommandBase.mainArm.getShooterAngleError());
SmartDashboard.putNumber("shooterAngleErrorFixed", CommandBase.mainArm.getShooterAngleError()+50);
SmartDashboard.putNumber("goalDistance", 54 / Math.tan((Math.PI/180) * (60 - CommandBase.visionHelper.getVerticalError())));
if (Robot.oi.getJoysticks()[1].getRawButton(3)) {
CommandBase.pneumatics.setArmStopState(true);

3
src/org/usfirst/frc/team2059/robot/RobotMap.java
View File

@ -37,7 +37,8 @@ public class RobotMap {
public static double fWidth = 483.467261958;
public static double fHeight = 362.600446468;
public static int sonar = 2;
public static int goalHeight = 72;
//public static int goalHeight = 102;
public static int goalHeight = 64;
//Misc
public static int mainArmPresetCollect = -5;
public static int mainArmPresetTraverse = 10;

36
src/org/usfirst/frc/team2059/robot/commands/shooter/AlignVertical.java Normal file
View File

@ -0,0 +1,36 @@
package org.usfirst.frc.team2059.robot.commands.shooter;
import org.usfirst.frc.team2059.robot.commands.CommandBase;
import org.usfirst.frc.team2059.robot.Robot;
import org.usfirst.frc.team2059.robot.RobotMap;
/**
*
*/
public class AlignVertical extends CommandBase {
public AlignVertical() {
requires(mainArm);
}
// Called just before this Command runs the first time
protected void initialize() {
mainArm.setSetpoint(mainArm.getShooterAngleError()+50);
mainArm.enable();
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
//Move the arm stop
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
// Stop when either limit switch is hit
return false;
}
// Called once after isFinished returns true
protected void end() {
mainArm.disable();
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}
// vim: sw=2:ts=2:sts=2

5
src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java
View File

@ -6,7 +6,10 @@ public class AlignHorizontal extends CommandBase {
}
protected void initialize() {
error = visionHelper.getHorizontalError();
driveBase.setDriveEnabled(false);
driveBase.resetGyro();
mainArm.setSetpoint(90);
mainArm.enable();
}
protected boolean isFinished() {
return false;
@ -16,6 +19,8 @@ public class AlignHorizontal extends CommandBase {
}
protected void end() {
driveBase.getGyroController().disable();
driveBase.setDriveEnabled(true);
mainArm.disable();
}
protected void interrupted() {
end();

16
src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java
View File

@ -20,8 +20,9 @@ public class DriveBase extends Subsystem {
PIDController gyroController = new PIDController(0.07, 0.002, 0.08, new GyroPIDSource(), new MotorsPIDOutputReversed());
PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput());
double encoderGyroCorrection = 1;
boolean driveEnabled = true;
public void initDefaultCommand() {
// setDefaultCommand(new Drive());
setDefaultCommand(new Drive());
}
public void stopDriving() {
leftMotorOne.set(0);
@ -30,10 +31,12 @@ public class DriveBase extends Subsystem {
rightMotorTwo.set(0);
}
public void driveArcade(double x, double y, double z, double sensitivity) {
leftMotorOne.set((-y + (x + z)) * sensitivity);
leftMotorTwo.set((-y + (x + z)) * sensitivity);
rightMotorOne.set((y + (x + z)) * sensitivity);
rightMotorTwo.set((y + (x + z)) * sensitivity);
if(driveEnabled){
leftMotorOne.set((-y + (x + z)) * sensitivity);
leftMotorTwo.set((-y + (x + z)) * sensitivity);
rightMotorOne.set((y + (x + z)) * sensitivity);
rightMotorTwo.set((y + (x + z)) * sensitivity);
}
}
public void driveStraight(double y, double correction) {
SmartDashboard.putNumber("GyroAngle", gyro.getAngle());
@ -53,6 +56,9 @@ public class DriveBase extends Subsystem {
gyroController.enable();
gyroController.setSetpoint(angle);
}
public void setDriveEnabled(boolean enabled){
driveEnabled = enabled;
}
public PIDController getLeftController() {
return leftEncoderController;
}

6
src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java
View File

@ -16,7 +16,7 @@ public class MainArm extends PIDSubsystem {
private double min = RobotMap.zeroDegrees;
private double max = RobotMap.ninetyDegrees;
public MainArm() {
super("MainArm", 0.1, 0.0, 0.002);
super("MainArm", 0.08, 0.0, 0.002);
getPIDController().setContinuous(false);
setSetpoint(70);
enable();
@ -90,8 +90,8 @@ public class MainArm extends PIDSubsystem {
return false;
}
public double getShooterAngleError(){
double correctedHeight = RobotMap.goalHeight - 44; //44 is the distance between the ground and the sonar
return (180/Math.PI) * Math.atan(correctedHeight/getSonarDistance());
double correctedHeight = (RobotMap.goalHeight + 26) - 44; //44 is the distance between the ground and the sonar
return (180/Math.PI) * Math.atan(correctedHeight/(getSonarDistance()-5));
}
public double getSonarDistanceRaw(){
return sonar.getVoltage();

15
src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java
View File

@ -15,8 +15,17 @@ public class VisionHelper {
contoursTable = NetworkTable.getTable("GRIP/contours");
}
public double getCenterContourX(){
int highestAreaIndex = 0;
int index = 0;
double areas[] = linesTable.getNumberArray("area", new double[0]);
for(double area : areas){
if(area >= areas[highestAreaIndex]){
highestAreaIndex=index;
}
index++;
}
try{
return contoursTable.getNumberArray("centerX",new double[0])[0];
return contoursTable.getNumberArray("centerX",new double[0])[highestAreaIndex];
}catch(Exception e){
return 0;
}
@ -53,7 +62,6 @@ public class VisionHelper {
}
index++;
}
System.out.println("Highest Length Index:"+ highestLengthIndex);
try {
return (x1s[highestLengthIndex]+x2s[highestLengthIndex])/2;
} catch (Exception e) {
@ -72,7 +80,6 @@ public class VisionHelper {
}
index++;
}
System.out.println("Highest Length Index:"+ highestLengthIndex);
try {
return (y1s[highestLengthIndex]+y2s[highestLengthIndex])/2;
} catch (Exception e) {
@ -80,7 +87,7 @@ public class VisionHelper {
}
}
public double getHorizontalError() {
return (180/Math.PI) * (Math.atan((getCenterContourX() - (RobotMap.imageWidth / 2)) / RobotMap.fWidth));
return ((180/Math.PI) * (Math.atan((getCenterContourX() - (RobotMap.imageWidth / 2)) / RobotMap.fWidth)))+3.5;
}
public double getVerticalError() {
return (180/Math.PI) * (Math.atan((getCenterY() - (RobotMap.imageHeight / 2)) / RobotMap.fHeight));