We spend about 4 hours today documenting this.
We used our robot from Steamworks that used Mecanum with this years code and changes to the way the Mecanum drive is defined.
This is how our robot works:
The Z rotation of the joystick controls Turn Left, Turn Right
The X axis of the joystick controls Forward/Reverse
The Y axis of the joystick controls Strafe Left, Strafe Right.
We tried every permutation of inversion of pairs of motors to get this to work the way we remembered Mecanum to work (Z strafes, Y forward/reverse, X turn LR)
We verified our CAN addressing to the motors.
We coded arcade drive to confirm the joystick function properly.
We even tried changing the order that the Z, X and Y are feed into the driveTrain.driveCartesian(ySpeed, xSpeed, zRotation); statement - that made things weird too.
Below is the code deployed to this robot:
package org.usfirst.frc.team5000.robot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
// import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
// import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.DoubleSolenoid;
// import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.VictorSPX;
// import edu.wpi.first.wpilibj.PWMSpeedController;
import edu.wpi.first.wpilibj.Spark;
// import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import com.kauailabs.navx.frc.AHRS;
// import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.Encoder;
/**
-
The VM is configured to automatically run this class, and to call the
-
functions corresponding to each mode, as described in the IterativeRobot
-
documentation. If you change the name of this class or the package after
-
creating this project, you must also update the manifest file in the resource
-
directory.
*/
public class Robot extends TimedRobot {
final String defaultAuto = “Default”;
final String customAuto = “My Auto”;
String autoSelected;
SendableChooser<String> chooser = new SendableChooser<>();
WPI_TalonSRX frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor;
VictorSPX intakeMotor;
Joystick driveJoystick, buttonsJoystick;
HHJoystickButtons buttonsJoystickButtons;
HHJoystickButtons driveJoystickButtons;
PowerDistributionPanel pdp;
CameraServer cameraServer;
MecanumDrive driveTrain;
// DifferentialDrive driveTrain;
Spark liftA, liftB;
// UsbCamera usbCamera;
SpeedControllerGroup lift;
static final Double liftPower = .5;
static final Double driveDeadband = 0.0;
Encoder enc;
DoubleSolenoid intake, kicker, portal;
static final int forwardChannel1 = 0;
static final int reverseChannel1 = 1;
static final int intakeUp = 2;
static final int intakeDown = 3;
static final int forwardChannel2 = 2;
static final int reverseChannel2 = 3;
static final int kickerOut = 4;
static final int kickerIn = 5;
static final int forwardChannel3 = 4;
static final int reverseChannel3 = 5;
static final int portalOut = 6;
static final int portalIn = 7;
static final int intakeMotorOn = 1;
static final int intakeMotorOff = 2;
static final double intakeMotorPower = .25;
double joystickB = .2;
double joystickA;
double joystickY;
double joystickZ;
AHRS ahrs;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
@Override
public void robotInit() {
chooser.setDefaultOption("Default Auto", defaultAuto);
chooser.addOption("My Auto", customAuto);
SmartDashboard.putData("Auto choices", chooser);
/**frontLeftMotor = new WPI_TalonSRX(2);
rearLeftMotor = new WPI_TalonSRX(0);
frontRightMotor = new WPI_TalonSRX(3);
rearRightMotor = new WPI_TalonSRX(1);
intakeMotor = new VictorSPX(0);
*/
frontLeftMotor = new WPI_TalonSRX(2);
rearLeftMotor = new WPI_TalonSRX(3);
frontRightMotor = new WPI_TalonSRX(1);
rearRightMotor = new WPI_TalonSRX(0);
liftA = new Spark(0);
liftB = new Spark(1);
lift = new SpeedControllerGroup((liftA), (liftB));
// SpeedControllerGroup Right = new SpeedControllerGroup(frontRightMotor, rearRightMotor);
// SpeedControllerGroup Left = new SpeedControllerGroup(frontLeftMotor, rearLeftMotor);
cameraServer = CameraServer.getInstance();
cameraServer.startAutomaticCapture("cam0", 0);
enc = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
enc.reset();
driveJoystick = new Joystick(0);
buttonsJoystick = new Joystick(1);
buttonsJoystickButtons = new HHJoystickButtons(buttonsJoystick, 10);
driveJoystickButtons = new HHJoystickButtons(driveJoystick, 10);
pdp = new PowerDistributionPanel();
// driveTrain.setDeadband(driveDeadband);
// driveTrain.setRightSideInverted(true);
// frontLeftMotor.setInverted(true);
// rearLeftMotor.setInverted(true);
// frontRightMotor.setInverted(true);
// rearRightMotor.setInverted(true);
driveTrain = new MecanumDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
// driveTrain = new DifferentialDrive(Left, Right);
intake = new DoubleSolenoid(forwardChannel1,reverseChannel1);
kicker = new DoubleSolenoid(forwardChannel2,reverseChannel2);
portal = new DoubleSolenoid(forwardChannel3, reverseChannel3);
// ahrs = new AHRS(SerialPort.Port.kMXP.kUSB);
ahrs = new AHRS(I2C.Port.kOnboard);
}
/**
* This autonomous (along with the chooser code above) shows how to select
* between different autonomous modes using the dashboard. The sendable
* chooser code works with the Java SmartDashboard. If you prefer the
* LabVIEW Dashboard, remove all of the chooser code and uncomment the
* getString line to get the auto name from the text box below the Gyro
*
* You can add additional auto modes by adding additional comparisons to the
* switch structure below with additional strings. If using the
* SendableChooser make sure to add them to the chooser code above as well.
*/
@Override
public void autonomousInit() {
autoSelected = chooser.getSelected();
// autoSelected = SmartDashboard.getString(“Auto Selector”,
// defaultAuto);
System.out.println("Auto selected: " + autoSelected);
}
/**
* This function is called periodically during autonomous
*/
@Override
public void autonomousPeriodic() {
switch (autoSelected) {
case customAuto:
// Put custom auto code here
break;
case defaultAuto:
default:
// Put default auto code here
break;
}
}
/** Need to add teleopInit JF
*/
@Override
public void teleopInit() {
}
/**
* This function is called periodically during operator control
*/
@Override
public void teleopPeriodic() {
SmartDashboard.putBoolean("IMU_Connected", ahrs.isConnected());
buttonsJoystickButtons.updateState();
driveJoystickButtons.updateState();
double xSpeed = driveJoystick.getX();
double ySpeed = driveJoystick.getY();
double zRotation = driveJoystick.getTwist();
driveTrain.driveCartesian(ySpeed, xSpeed, zRotation);
// driveTrain.arcadeDrive(xSpeed, zRotation);
intake();
lift();
// kicker();
}
void intake() {
if (buttonsJoystickButtons.isPressed(intakeDown)) {
intakeMotor.set(ControlMode.PercentOutput, 0);
intake.set(DoubleSolenoid.Value.kReverse);
} else if (driveJoystickButtons.isPressed(intakeUp)) {
intake.set(DoubleSolenoid.Value.kForward);
intakeMotor.set(ControlMode.PercentOutput, intakeMotorPower);
}
}
void lift() {
double X = buttonsJoystick.getY();
double x1 = X * liftPower;
lift.set(x1);
double Current = pdp.getCurrent(14);//change # for actual robot
SmartDashboard.putNumber("Lift Current", Current);
SmartDashboard.putNumber("Encoder ",enc.getRaw());
}
/**
* This function is called periodically during test mode
*/
@Override
public void testPeriodic() {
}
}
