Skip to content

Instantly share code, notes, and snippets.

@zavallabots
Created February 4, 2021 02:31
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save zavallabots/e1ca58903215a4c8a221a3aceb646a90 to your computer and use it in GitHub Desktop.
Save zavallabots/e1ca58903215a4c8a221a3aceb646a90 to your computer and use it in GitHub Desktop.
Trying to create a code for teleop only basic tank drive with controller
package frc.robot;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import edu.wpi.first.wpilibj.command.Subsystem;
public class Drivetrain extends Subsystem {
private TalonSRX motorLeft1 = new TalonSRX(RobotMap.MOTOR_LEFT_1_ID);
private TalonSRX motorLeft2 = new TalonSRX(RobotMap.MOTOR_LEFT_2_ID);
private TalonSRX motorRight1 = new TalonSRX(RobotMap.MOTOR_RIGHT_1_ID);
private TalonSRX motorRight2 = new TalonSRX(RobotMap.MOTOR_RIGHT_2_ID);
@Override
public void initDefaultCommand() {
setDefaultCommand(new TankDrive());
}
public void setLeftMotors(double speed) {
motorLeft1.set(ControlMode.PercentOutput, -speed);
motorLeft2.set(ControlMode.PercentOutput, -speed);
}
public void setRightMotors(double speed) {
motorRight1.set(ControlMode.PercentOutput, speed);
motorRight2.set(ControlMode.PercentOutput, speed);
}
}
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc.robot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all.
* Unless you know what you are doing, do not modify this file except to
* change the parameter class to the startRobot call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(frc.robot.Robot::new);
}
}
package frc.robot;
import edu.wpi.first.wpilibj.XboxController;
public class OI {
private XboxController driverController = new XboxController(RobotMap.DRIVER_CONTROLLER);
public double GetDriverRawAxis(int axis){
return driverController.getRawAxis(axis);
}
}
package frc.robot;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
@SuppressWarnings("deprecation")
public class Robot extends SampleRobot
{
DifferentialDrive robotDrive;
Talon motorLeft, motorRight;
Joystick stick;
/**
* Constructor. Called once when this class is created.
*/
public Robot()
{
System.out.println("Robot.constructor()");
}
/**
* Called once after class load complete.
* Use to perform any needed initializations.
* Very similar to the constructor.
*/
public void robotInit()
{
System.out.println("Robot.robotInit()");
// Create two PWM Talon motor controller objects for left & right motors on PWM ports 0 & 1.
// Assumes robot has two motors controlled by Talon controllers connected via PWM.
// Add them to a drive controller class that can do tank and arcade driving based on
// joystick input.
motorLeft = new Talon(0);
motorRight = new Talon(1);
robotDrive = new DifferentialDrive(motorLeft, motorRight);
robotDrive.setExpiration(0.1); // need to see motor input at least every
// 10th of a second or stop motors.
// One side has motors reversed so the wheels turn in the same direction.
robotDrive.setRightSideInverted(false);
stick = new Joystick(0); // joystick on usb port 0.
}
/**
* Executes a simple autonomous program.
* Called by the driver station or field control system at the
* start of the autonomous period.
*/
public void autonomous()
{
System.out.println("Robot.autonomous()");
robotDrive.setSafetyEnabled(false); // motor safety off due to the fact
// we want the motor to run 2 sec
// with no other input.
robotDrive.tankDrive(0.5, 0.5); // drive forwards half speed
Timer.delay(2.0); // for 2 seconds.
robotDrive.tankDrive(0.0, 0.0); // stop motors.
}
/**
* Runs the motors with arcade steering, input from joystick.
* Called by the driver station or field control system at the
* start of the operator control (teleop) period. Runs in a loop
* until robot is disabled.
*/
public void operatorControl()
{
System.out.println("Robot.operatorControl()");
robotDrive.setSafetyEnabled(true); // motor safety back on.
while (isOperatorControl() && isEnabled())
{
robotDrive.arcadeDrive(stick.getX(), stick.getY()); // drive with arcade style.
Timer.delay(0.020); // wait for a joystick update.
}
}
/**
* Called whenever the robot is disabled by the DS or field control.
* Use to perform any needed resets or changes when switching between modes.
*/
public void disabled()
{
System.out.println("Robot.disabled()");
}
/**
* Runs during test mode
*/
public void test()
{
System.out.println("Robot.test()");
}
}
package frc.robot;
public class RobotMap {
public static final int MOTOR_LEFT_1_ID = 0;
public static final int MOTOR_LEFT_2_ID = 0;
public static final int MOTOR_RIGHT_1_ID = 0;
public static final int MOTOR_RIGHT_2_ID = 0;
public static final int DRIVER_CONTROLLER = 0;
public static final int LEFT_STICK_Y = 1;
public static final int RIGHT_STICK_Y = 5;
}
package frc.robot;
import edu.wpi.first.wpilibj.command.Command;
public class TankDrive extends Command {
public TankDrive(){
requires(Robot.driveTrain);
}
@Override
protected void initialize(){
}
@Override
protected void execute(){
double leftStickY = Robot.m_oi.GetDriverRawAxis(RobotMap.LEFT_STICK_Y);
double RightStickY = Robot.m_oi.GetDriverRawAxis(RobotMap.RIGHT_STICK_Y);
Robot.driveTrain.setLeftMotors(leftStickY);
Robot.driveTrain.setRightMotors(RightStickY);
}
@Override
protected boolean isFinished(){
return false;
}
@Override
protected void end(){
Robot.driveTrain.setLeftMotors(0);
Robot.driveTrain.setRightMotors(0);
}
@Override
protected void interrupted(){
this.end();
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment