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

gave up on vision based distance

Browse Source
This commit is contained in:
Adam Long 2016-10-10 19:20:02 +00:00
parent 7745c589ae
commit e5dee6a5ab
4 changed files with 51 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.buttons.JoystickButton;
import org.usfirst.frc.team2059.robot.commands.drivetrain.LogEncoder;
import org.usfirst.frc.team2059.robot.commands.drivetrain.PIDDrive;
import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
import org.usfirst.frc.team2059.robot.commands.drivetrain.DriveStraight;
import org.usfirst.frc.team2059.robot.commands.shooter.MoveArm;
import org.usfirst.frc.team2059.robot.commands.shooter.ResetLower;
import org.usfirst.frc.team2059.robot.commands.shooter.ResetUpper;
@ -40,6 +41,8 @@ public class OI {
joystickButtons[0][0].whileHeld(new AlignHorizontal());
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));
joystickButtons[0][5].whileHeld(new DriveStraight(-0.25));
joystickButtons[0][6].whileHeld(new SpinRollers(1, true));
joystickButtons[0][9].whileHeld(new Drive());
// joystickButtons[0][2].whileHeld(new PIDDrive(400));

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

@ -21,7 +21,7 @@ public class Robot extends IterativeRobot {
// cameraServer.setQuality(50);
// cameraServer.startAutomaticCapture("cam0");
try {
new ProcessBuilder("/home/lvuser/grip").inheritIO().start();
// new ProcessBuilder("/home/lvuser/grip").inheritIO().start();
} catch (Exception e) {
System.out.println("Error starting GRIP");
}
@ -72,14 +72,17 @@ public class Robot extends IterativeRobot {
}
public void teleopPeriodic() {
Scheduler.getInstance().run();
SmartDashboard.putNumber("GyroAngle", CommandBase.driveBase.getGyro().getAngle());
SmartDashboard.putNumber("ArmAngleRaw", CommandBase.mainArm.getRaw());
SmartDashboard.putNumber("ArmAngleDegrees", CommandBase.mainArm.getDegrees());
SmartDashboard.putNumber("tmpRotations", CommandBase.driveBase.getLeftRotations());
SmartDashboard.putNumber("centerX", CommandBase.visionHelper.getCenterX());
// SmartDashboard.putNumber("centerY", CommandBase.visionHelper.getCenterY());
SmartDashboard.putNumber("centerContoursX", CommandBase.visionHelper.getCenterContourX());
SmartDashboard.putNumber("horizontalError", CommandBase.visionHelper.getHorizontalError());
SmartDashboard.putNumber("verticalError", CommandBase.visionHelper.getVerticalError());
SmartDashboard.putNumber("horizontalErrorCorrected", CommandBase.visionHelper.getHorizontalError() + 5);
SmartDashboard.putNumber("goalDistance", 55 / Math.tan((Math.PI/180) * (64 - CommandBase.visionHelper.getVerticalError())));
SmartDashboard.putNumber("goalDistance", 54 / Math.tan((Math.PI/180) * (60 - CommandBase.visionHelper.getVerticalError())));
if (Robot.oi.getJoysticks()[1].getRawButton(3)) {
CommandBase.pneumatics.setArmStopState(true);
} else {

35
src/org/usfirst/frc/team2059/robot/commands/drivetrain/DriveStraight.java Normal file
View File

@ -0,0 +1,35 @@
package org.usfirst.frc.team2059.robot.commands.drivetrain;
import org.usfirst.frc.team2059.robot.commands.CommandBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc.team2059.robot.Robot;
/**
*
*/
public class DriveStraight extends CommandBase {
double speed;
public DriveStraight(double speed) {
requires(driveBase);
this.speed = speed;
}
// 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.driveStraight(speed, SmartDashboard.getNumber("GyroCorrection"));
}
// 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() {
driveBase.driveStraight(0, SmartDashboard.getNumber("GyroCorrection"));
}
// 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

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

@ -14,6 +14,13 @@ public class VisionHelper {
linesTable = NetworkTable.getTable("GRIP/lines");
contoursTable = NetworkTable.getTable("GRIP/contours");
}
public double getCenterContourX(){
try{
return contoursTable.getNumberArray("centerX",new double[0])[0];
}catch(Exception e){
return 0;
}
}
public double getCenterX() {
int highestLengthIndex = 0;
int index = 0;
@ -53,7 +60,7 @@ public class VisionHelper {
}
}
public double getHorizontalError() {
return (180/Math.PI) * (Math.atan((getCenterX()-20 - (RobotMap.imageWidth / 2)) / RobotMap.fWidth));
return (180/Math.PI) * (Math.atan((getCenterContourX()+25 - (RobotMap.imageWidth / 2)) / RobotMap.fWidth));
}
public double getVerticalError() {
return (180/Math.PI) * (Math.atan((getCenterY() - (RobotMap.imageHeight / 2)) / RobotMap.fHeight));