Skip to content
This repository has been archived by the owner on Jan 28, 2019. It is now read-only.

Commit

Permalink
Added logic to shooting/intake, fixed psuedoShoot, and autos
Browse files Browse the repository at this point in the history
  • Loading branch information
@VaneRaklan
VaneRaklan committed Mar 26, 2017
1 parent 5ffae4b commit 95e4d80
Show file tree
Hide file tree
Showing 13 changed files with 119 additions and 90 deletions.
40 changes: 23 additions & 17 deletions FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,13 @@ public void robotInit() {

shooterUpdate = new PsuedoShooter_cmd();
visionUpdate = new Vision_cmd();


Main_auto = new Main_auto();
// Main_auto = new Autonomous_GearLeft();
// Main_auto = new Autonomous_GearRight();


fakePID = new PsuedoShooter_cmd();
oi = new OI();
oi.init();
Expand Down Expand Up @@ -166,23 +172,23 @@ public void teleopPeriodic() {
// fakePID.start();
// }

if(oi.a2.get()) {
if(vision.interpretation() == false){
if(vision.findCenterYs() < 100){
RobotMap.robotDrive.arcadeDrive(0,.5);
}
else if(vision.findCenterYs() > 120){
RobotMap.robotDrive.arcadeDrive(0,-.5);
}
else{
RobotMap.robotDrive.arcadeDrive(0,0);
}
}
else{
RobotMap.robotDrive.arcadeDrive(0,0);
}

}
// if(oi.a2.get()) {
// if(vision.interpretation() == false){
// if(vision.findCenterYs() < 100){
// RobotMap.robotDrive.arcadeDrive(0,.5);
// }
// else if(vision.findCenterYs() > 120){
// RobotMap.robotDrive.arcadeDrive(0,-.5);
// }
// else{
// RobotMap.robotDrive.arcadeDrive(0,0);
// }
// }
// else{
// RobotMap.robotDrive.arcadeDrive(0,0);
// }
//
// }
}

/**
Expand Down
4 changes: 2 additions & 2 deletions FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/RobotMap.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,9 +95,9 @@ public static void init(){

shootReq = false;
FALL = false;
_gemini = false;
_gemini = true; //boolean for manipulator, true = gear and false = fuel
shift = true;
drive = true;
drive = true; //boolean for driver, true = gear and false = fuel
_switch = true;
CAngle = 0;
visionShooterSpeed = 0;
Expand Down
6 changes: 5 additions & 1 deletion FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Autonomous_GearLeft.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,10 @@ public Autonomous_GearLeft() {
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
addSequential(new TimedDrive(1.5, false, -.65));
addSequential(new DistanceDrive_cmd(1.8, -.615), 6.5);
addSequential(new TimedDrive(.2, false, .85));
addSequential(new GyroDrive_cmd(60, .65));
addSequential(new DistanceDrive_cmd(1, -.615), 3.5);
addSequential(new Gear_autoCmd());
}
}
10 changes: 5 additions & 5 deletions FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Autonomous_GearRight.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,10 @@ public Autonomous_GearRight() {
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
addSequential(new TimedDrive(1.375, false, -.75));
addSequential(new TimedDrive(.2, false, 0));
addSequential(new TimedDrive(.725, true, -.7));
addSequential(new TimedDrive(.2, false, 0));
addSequential(new TimedDrive(1, false, -.65));
addSequential(new DistanceDrive_cmd(1.8, -.615), 6.5);
addSequential(new TimedDrive(.2, false, .85));
addSequential(new GyroDrive_cmd(-60, -.6));
addSequential(new DistanceDrive_cmd(1, -.615), 3.5);
addSequential(new Gear_autoCmd());
}
}
2 changes: 1 addition & 1 deletion FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/DistanceDrive_cmd.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ public DistanceDrive_cmd(double distance, double power){
// Called just before this Command runs the first time
protected void initialize() {
RobotMap.euler.reset();

// RobotMap.navX.reset();
}

// Called repeatedly when this Command is scheduled to run
Expand Down
2 changes: 1 addition & 1 deletion FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Main_auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ public Main_auto() {
// addParallel(new Autonomus_Shooter(.5));
// addSequential(new Auto_Shooter2(10));

addSequential(new DistanceDrive_cmd(1.82, -.615), 6);
addSequential(new DistanceDrive_cmd(1.8, -.615), 6.5);
addSequential(new TimedDrive(.2, false, .85));
addSequential(new Gear_autoCmd());

Expand Down
2 changes: 1 addition & 1 deletion FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/commands/SmartDashboard_cmd.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ protected void initialize() {
protected void execute() {
Robot.dashboard.velocities();
// Robot.dashboard.vision();
// Robot.dashboard.accelAngle();
Robot.dashboard.accelAngle();
// Robot.dashboard.distance();
// Robot.dashboard.currentDraw();
}
Expand Down
3 changes: 3 additions & 0 deletions FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/Climber_sub.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,9 @@ public void climb(){
if((Robot.oi.LJ2.get() && Robot.oi.RJ2.get()) || (Robot.oi.LJ1.get() && Robot.oi.RJ1.get())){
RobotMap.climber.set(1);
}
else if(Math.abs(Robot.oi.gamepad2.getRawAxis(1)) > 0.1){
RobotMap.climber.set(Math.abs(Robot.oi.gamepad2.getRawAxis(1)));
}
else{
RobotMap.climber.set(0);
}
Expand Down
2 changes: 1 addition & 1 deletion FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/Gear_sub.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ public void gearCamera(){
if(Robot.oi.gamepad1.getRawAxis(5) > .25 || Robot.oi.gamepad2.getRawAxis(5) > .25){
_cam += 1;
}
else if(Robot.oi.gamepad1.getRawAxis(5) > .25 || Robot.oi.gamepad2.getRawAxis(5) < -.25){
else if(Robot.oi.gamepad1.getRawAxis(5) < -.25 || Robot.oi.gamepad2.getRawAxis(5) < -.25){
_cam -=1;
}
else{
Expand Down
16 changes: 15 additions & 1 deletion FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/Intake_sub.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,16 @@ public void initDefaultCommand() {

public void intake(){

if(Robot.oi.RB2.get()){
if(Robot.oi.RB2.get() && (Robot.oi.gamepad2.getRawAxis(3) > 0.1 ) || Robot.oi.b2.get()){
RobotMap.copterAgitator.set(.5);
if((RobotMap.Rshooter.getEncVelocity() > Robot.psuedoShooter.getPrimeR(71) && RobotMap.Rshooter.getEncVelocity() < Robot.psuedoShooter.getPrimeR(79))
|| (RobotMap.Lshooter.getEncVelocity() > Robot.psuedoShooter.getPrimeL(71) && RobotMap.Lshooter.getEncVelocity() < Robot.psuedoShooter.getPrimeL(79)))
{
RobotMap.intake.set(-.6);
RobotMap.agitator.set(-.55);
}
}
else if(Robot.oi.RB2.get()){
RobotMap.intake.set(-.6);
RobotMap.agitator.set(-.55);
RobotMap.copterAgitator.set(.5);
Expand All @@ -38,6 +47,11 @@ else if(Robot.oi.LB2.get()){
RobotMap.agitator.set(0);
RobotMap.copterAgitator.set(0);
}






}

Expand Down
18 changes: 9 additions & 9 deletions FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/PsuedoShooter_sub.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,35 +35,35 @@ public void initDefaultCommand() {

public double speedChangeLeft(){

if(-getLowerL(71) <= lower_left){
if(RobotMap.Lshooter.getEncVelocity() <= -getLowerL(71)){
return RobotMap.Lshooter.get() - 0.025;
}
else if(-getUpperL(79)>= upper_left){
else if(RobotMap.Lshooter.getEncVelocity() >= -getUpperL(79)){
return RobotMap.Lshooter.get() + 0.005;
}
else if(-getPrimeL(75) != prime_left){
else if(RobotMap.Lshooter.getEncVelocity() != -getPrimeL(75)){
if(-getPrimeL(75) < prime_left){
return RobotMap.Lshooter.get() - 0.0025;
}
else if(-getPrimeL(75) > prime_left){
else if(RobotMap.Lshooter.getEncVelocity() > -getPrimeL(75)){
return RobotMap.Lshooter.get() + 0.0015;
}
}
return RobotMap.Lshooter.get();
}
public double speedChangeRight(){

if(getLowerR(71) <= lower_right){
if( RobotMap.Rshooter.getEncVelocity() <= getLowerR(71)){
return RobotMap.Rshooter.get() + 0.025;
}
else if(getUpperR(79) >= upper_right){
else if(RobotMap.Rshooter.getEncVelocity() >= getUpperR(79)){
return RobotMap.Rshooter.get() - 0.005;
}
else if(getPrimeR(75) != prime_right){
if(getPrimeR(75) < prime_right){
else if(RobotMap.Rshooter.getEncVelocity() != getPrimeR(75)){
if(RobotMap.Rshooter.getEncVelocity() < getPrimeR(75)){
return RobotMap.Rshooter.get() + 0.0025;
}
else if(getPrimeR(75) > prime_right){
else if(RobotMap.Rshooter.getEncVelocity() > getPrimeR(75)){
return RobotMap.Rshooter.get() - 0.0015;
}
}
Expand Down
14 changes: 8 additions & 6 deletions FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/SmartDashboard_sub.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,12 +41,14 @@ public void currentDraw(){
SmartDashboard.putNumber("Back Right Drive is pulling current: ", RobotMap.BRdrive.getOutputCurrent());
}
public void accelAngle(){
SmartDashboard.putNumber("X Axis: ", RobotMap.navX.getRawAccelX() * _convert);
SmartDashboard.putNumber("Y Axis: ", RobotMap.navX.getRawAccelY() * _convert);
SmartDashboard.putNumber("Z Axis: ", RobotMap.navX.getRawAccelZ() * _convert);
SmartDashboard.putNumber("Left Servo: ", RobotMap.leftAgitator.getAngle());
SmartDashboard.putNumber("Right Servo: ", RobotMap.rightAgitator.getAngle());
SmartDashboard.putNumber("Camera Angle", RobotMap.cameraServo.getAngle());
// SmartDashboard.putNumber("X Axis: ", RobotMap.navX.getRawAccelX() * _convert);
// SmartDashboard.putNumber("Y Axis: ", RobotMap.navX.getRawAccelY() * _convert);
// SmartDashboard.putNumber("Z Axis: ", RobotMap.navX.getRawAccelZ() * _convert);
// SmartDashboard.putNumber("Left Servo: ", RobotMap.leftAgitator.getAngle());
// SmartDashboard.putNumber("Right Servo: ", RobotMap.rightAgitator.getAngle());
// SmartDashboard.putNumber("Camera Angle", RobotMap.cameraServo.getAngle());
SmartDashboard.putNumber("Robot Angle", RobotMap.navX.getAngle());

}
}

90 changes: 45 additions & 45 deletions FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/vision/VisionArray_sub.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,51 +179,51 @@ public void shooterAdjustment(){
}
}

public boolean fuelInterpretation(){
boolean _height, _x, _y;
///////////
if(findHeights() < 50 && findHeights() > 15){
_height = true;
}
else{
_height = false;
}
///////////
if(findCenterXs() < 100 && findCenterXs() > 50){
_x = true;
}
else{
_x = false;
}
///////////
if(findCenterYs() < 160 && findCenterYs() > 100){
_y = true;
}
else{
_y = false;
}

return _height && _x && _y;
if(_height && _x && _y){
RobotMap.shootReq = true;

}
}
public boolean gearInterpretation(){
boolean tooLeft;
boolean tooRight;
boolean goodToGear;
if((heights[0] > heights[1] + 3 && centerXs[0] > centerXs[1] + 3 && centerYs[0] > centerYs[1] + 3) && heights[0] > 20/*to be determined*/ && heights[1] > 20/*to be determined*/){
tooLeft = true;
}
else if((heights[1] > heights[0] + 3 && centerXs[1] > centerXs[0] + 3 && centerYs[1] > centerYs[0] + 3) && heights[0] > 20/*to be determined*/ && heights[1] > 20/*to be determined*/){
tooRight = true;
}
else{
goodToGear = true;
}
return tooLeft && tooRight && goodToGear;
}
// public boolean fuelInterpretation(){
// boolean _height, _x, _y;
// ///////////
// if(findHeights() < 50 && findHeights() > 15){
// _height = true;
// }
// else{
// _height = false;
// }
// ///////////
// if(findCenterXs() < 100 && findCenterXs() > 50){
// _x = true;
// }
// else{
// _x = false;
// }
// ///////////
// if(findCenterYs() < 160 && findCenterYs() > 100){
// _y = true;
// }
// else{
// _y = false;
// }
//
// return _height && _x && _y;
// if(_height && _x && _y){
// RobotMap.shootReq = true;
//
// }
// }
// public boolean gearInterpretation(){
// boolean tooLeft;
// boolean tooRight;
// boolean goodToGear;
// if((heights[0] > heights[1] + 3 && centerXs[0] > centerXs[1] + 3 && centerYs[0] > centerYs[1] + 3) && heights[0] > 20/*to be determined*/ && heights[1] > 20/*to be determined*/){
// tooLeft = true;
// }
// else if((heights[1] > heights[0] + 3 && centerXs[1] > centerXs[0] + 3 && centerYs[1] > centerYs[0] + 3) && heights[0] > 20/*to be determined*/ && heights[1] > 20/*to be determined*/){
// tooRight = true;
// }
// else{
// goodToGear = true;
// }
// return tooLeft && tooRight && goodToGear;
// }

// public void interpretCamera(){
// if(areas.length > 0 && widthDataCount > 0 && heightDataCount > 0 && centerXDataCount > 0 && centerYDataCount > 0){
Expand Down

0 comments on commit 95e4d80

Please sign in to comment.