Quote:
Originally Posted by rich2202
Post your code.
Errors?
Doesn't execute the code?
Which DIO port is it on?
|
It's on DIO port 0. There should be no issues, if you can spot one let me know though!
Code:
package org.usfirst.frc.team6077.robot;
import com.ni.vision.NIVision;
import com.ni.vision.NIVision.Image;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.VictorSP;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class Robot extends SampleRobot {
// Declarations
boolean limit1b;
int session;
Image frame;
RobotDrive drive;
Compressor compressor;
DoubleSolenoid claw;
Solenoid pusher;
DigitalInput limit1;
Joystick xboxController;
Joystick logitech3dpro;
VictorSP drivemotor1;
VictorSP drivemotor2;
VictorSP drivemotor3;
VictorSP drivemotor4;
VictorSP slideMotor;
VictorSP armMotor;
// Autonomous Names
final String defaultAuto = "Default";
final String customAuto = "Autonomous Choice 1";
final String customAuto2 = "Autonomous Choice 2";
final String customAuto3 = "Autonomous Choice 3";
final String customAuto4 = "Autonomous Choice 4";
final String customAuto5 = "Autonomous Choice 5";
// Chooser
SendableChooser chooser;
public Robot() {
frame = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);
session = NIVision.IMAQdxOpenCamera("cam0",
NIVision.IMAQdxCameraControlMode.CameraControlModeController);
NIVision.IMAQdxConfigureGrab(session);
//Change values later
drivemotor1 = new VictorSP(0);
drivemotor2 = new VictorSP(1);
drivemotor3 = new VictorSP(2);
drivemotor4 = new VictorSP(3);
compressor = new Compressor(0);
drive = new RobotDrive(drivemotor1, drivemotor2,
drivemotor3, drivemotor4);
drive.setExpiration(0.1);
xboxController = new Joystick(0);
logitech3dpro = new Joystick(1);
armMotor = new VictorSP(5);
slideMotor = new VictorSP(4);
claw = new DoubleSolenoid(0, 1);
pusher = new Solenoid(2);
limit1 = new DigitalInput(0);
chooser = new SendableChooser();
}
public void robotInit() {
compressor.setClosedLoopControl(true);
compressor.start();
chooser.addDefault("Default Autonomous", defaultAuto);
chooser.addObject("Autonomous Choice 1", customAuto);
chooser.addObject("Autonomous Choice 2", customAuto2);
chooser.addObject("Autonomous Choice 3", customAuto3);
chooser.addObject("Autonomous Choice 4", customAuto4);
chooser.addObject("Autonomous Choice 5", customAuto5);
SmartDashboard.putData("Autonomous Chooser", chooser);
}
public void autonomous() {
drive.setSafetyEnabled(false);
String autoSelected = (String) chooser.getSelected();
System.out.println("Autonomous Mode selected: " + autoSelected);
switch (autoSelected) {
case customAuto5:
/*Only use if have to
* Goal: Go through the portcullis & pick up a ball
* Note: Values might change as testing is done
*/
//Drive forwards while lowering arms to default
drive.drive(1, 0);
armMotor.set(-0.5);
Timer.delay(1);
armMotor.set(1);
Timer.delay(1);
drive.drive(1, 0);
Timer.delay(2);
drive.drive(0.5, 1);
claw.set(DoubleSolenoid.Value.kForward);
Timer.delay(1);
claw.set(DoubleSolenoid.Value.kOff);
break;
case customAuto4:
/*Hallway autonomous test
* Goal: Drive forwards, turn around, come back
*/
drive.drive(-0.25, 0);
Timer.delay(3);
drive.drive(0.50, 1);
Timer.delay(1.75);
drive.drive(-0.1, 0);
break;
case customAuto3:
break;
case customAuto2:
break;
case customAuto:
break;
case defaultAuto:
default:
/*Preferred w/ preferred spawn point
Goal: Go through the low bar and pick up a ball. */
//Drive forwards
drive.drive(-1, 0);
Timer.delay(0.5);
drive.drive(0, 0);
armMotor.set(-1);
Timer.delay(0.5);
armMotor.stopMotor();
claw.set(DoubleSolenoid.Value.kReverse);
Timer.delay(1);
claw.set(DoubleSolenoid.Value.kForward);
Timer.delay(1);
break;
}
Scheduler.getInstance().run();
}
public void operatorControl() {
NIVision.IMAQdxStartAcquisition(session);
drive.setSafetyEnabled(true);
while (isOperatorControl() && isEnabled()) {
NIVision.IMAQdxGrab(session, frame, 1);
CameraServer.getInstance().setImage(frame);
//Forwards limitswitch for slide motor.
if (limit1.get()) {
slideMotor.stopMotor();
}
compressor.start();
// Move robot using left and right joystick
//drive.tankDrive(xboxController.getRawAxis(1), xboxController.getRawAxis(5));
drive.arcadeDrive(xboxController.getRawAxis(1), -xboxController.getRawAxis(0));
// Lift and lower the arms using the right and left bumper.
//Arm Motor Test With Triggers
if (Math.abs(xboxController.getRawAxis(3)) > .1) {
armMotor.set(xboxController.getRawAxis(3));
} else if (Math.abs(xboxController.getRawAxis(2)) > .1) {
armMotor.set(-xboxController.getRawAxis(2));
} else {
armMotor.set(0);
}
// Move sliding mechanism forwards and backwards
if (Math.abs(xboxController.getRawAxis(5)) > .1) {
slideMotor.set(-xboxController.getRawAxis(5));
} else {
slideMotor.set(0);
}
// Open and close the claw
if (xboxController.getRawButton(1)) {
claw.set(DoubleSolenoid.Value.kForward);
} else if (xboxController.getRawButton(4)) {
claw.set(DoubleSolenoid.Value.kReverse);
}
//Open and close pushing cylinder
if (xboxController.getRawButton(3)) {
claw.set(DoubleSolenoid.Value.kReverse);
pusher.set(true);
} else if (xboxController.getRawButton(2)) {
pusher.set(false);
}
//Logitech 3D PRO
if (logitech3dpro.getRawButton(1)) {
pusher.set(true);
claw.set(DoubleSolenoid.Value.kReverse);
}
if (logitech3dpro.getRawAxis(1) > .1) {
claw.set(DoubleSolenoid.Value.kReverse);
} else if (logitech3dpro.getRawAxis(1) < -.1) {
claw.set(DoubleSolenoid.Value.kForward);
}
Timer.delay(0.005); //Wait for a motor update time
}
NIVision.IMAQdxStopAcquisition(session);
}
public void test() {
}
}