diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/MecanumDrive/src/Robot.cpp b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/MecanumDrive/src/Robot.cpp new file mode 100755 index 0000000000..c1f9d8a8ff --- /dev/null +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/MecanumDrive/src/Robot.cpp @@ -0,0 +1,49 @@ +#include "WPILib.h" + +/** + * This is a demo program showing how to use Mecanum control with the RobotDrive class. + */ +class Robot: public SampleRobot +{ + + // Channels for the wheels + const static int frontLeftChannel = 2; + const static int rearLeftChannel = 3; + const static int frontRightChannel = 1; + const static int rearRightChannel = 0; + + const static int joystickChannel = 0; + + RobotDrive robotDrive; // robot drive system + Joystick stick; // only joystick + +public: + Robot() : + robotDrive(frontLeftChannel, rearLeftChannel, + frontRightChannel, rearRightChannel), // these must be initialized in the same order + stick(joystickChannel) // as they are declared above. + { + robotDrive.SetExpiration(0.1); + robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // invert the left side motors + robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true); // you may need to change or remove this to match your robot + } + + /** + * Runs the motors with Mecanum drive. + */ + void OperatorControl() + { + robotDrive.SetSafetyEnabled(false); + while (IsOperatorControl()) + { + // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. + // This sample does not use field-oriented drive, so the gyro input is set to zero. + robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ()); + + Wait(0.005); // wait 5ms to avoid hogging CPU cycles + } + } + +}; + +START_ROBOT_CLASS(Robot); diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml index 5f88f8cfa6..ca90c1a929 100644 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml @@ -105,6 +105,24 @@ + + Mecanum Drive + An example program which the use of Mecanum Drive with the RobotDrive class + + Getting Started with C++ + Robot and Motor + Joystick + Complete List + + + src + + + + + + + Getting Started An example program which demonstrates the simplest autonomous and diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/MecanumDrive/src/org/usfirst/frc/team190/robot/Robot.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/MecanumDrive/src/org/usfirst/frc/team190/robot/Robot.java new file mode 100755 index 0000000000..1319d7494f --- /dev/null +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/MecanumDrive/src/org/usfirst/frc/team190/robot/Robot.java @@ -0,0 +1,53 @@ + +package org.usfirst.frc.team190.robot; + + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.RobotDrive; +import edu.wpi.first.wpilibj.RobotDrive.MotorType; +import edu.wpi.first.wpilibj.SampleRobot; +import edu.wpi.first.wpilibj.Timer; + +/** + * This is a demo program showing how to use Mecanum control with the RobotDrive class. + */ +public class Robot extends SampleRobot { + + RobotDrive robotDrive; + Joystick stick; + + // Channels for the wheels + final int frontLeftChannel = 2; + final int rearLeftChannel = 3; + final int frontRightChannel = 1; + final int rearRightChannel = 0; + + // The channel on the driver station that the joystick is connected to + final int joystickChannel = 0; + + public Robot() { + robotDrive.setExpiration(0.1); + robotDrive = new RobotDrive(frontLeftChannel, rearLeftChannel, frontRightChannel, rearRightChannel); + robotDrive.setInvertedMotor(MotorType.kFrontLeft, true); // invert the left side motors + robotDrive.setInvertedMotor(MotorType.kRearLeft, true); // you may need to change or remove this to match your robot + + stick = new Joystick(joystickChannel); + } + + + /** + * Runs the motors with Mecanum drive. + */ + public void operatorControl() { + robotDrive.setSafetyEnabled(true); + while (isOperatorControl()) { + + // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. + // This sample does not use field-oriented drive, so the gyro input is set to zero. + robotDrive.mecanumDrive_Cartesian(stick.getX(), stick.getY(), stick.getZ(), 0); + + Timer.delay(0.005); // wait 5ms to avoid hogging CPU cycles + } + } + +} diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml index 18f4a83979..2c4af2bc60 100755 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml @@ -116,6 +116,25 @@ + + Mecanum Drive + Demonstrate the use of the RobotDrive class doing teleop driving with Mecanum steering + + Actuators + Complete List + Joystick + Robot and Motor + Safety + + + src/$package-dir + + + + + + Motor Controller