54 lines
1.5 KiB
Java
54 lines
1.5 KiB
Java
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
package frc.robot.subsystems;
|
|
|
|
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
|
import com.kauailabs.navx.frc.AHRS;
|
|
|
|
import edu.wpi.first.wpilibj.drive.MecanumDrive;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import frc.robot.Constants;
|
|
|
|
public class BasePilotable extends SubsystemBase {
|
|
|
|
|
|
private WPI_TalonSRX avantGauche = new WPI_TalonSRX(Constants.MoteurAvantGauche);
|
|
|
|
private WPI_TalonSRX avantDroit = new WPI_TalonSRX(Constants.MoteurAvantDroit);
|
|
|
|
private WPI_TalonSRX arriereGauche = new WPI_TalonSRX(Constants.MoteurArriereGauche);
|
|
|
|
private WPI_TalonSRX arriereDroit = new WPI_TalonSRX(Constants.MoteurArriereDroit);
|
|
|
|
private MecanumDrive mecanum;
|
|
|
|
private AHRS gyroscope = new AHRS();
|
|
public double getangle() {
|
|
return gyroscope.getAngle();
|
|
}
|
|
public void drive(double y, double x, double rot){
|
|
mecanum.driveCartesian(y, rot, x, -getangle());
|
|
}
|
|
public void resetgyro(){
|
|
try {
|
|
gyroscope.reset();
|
|
}
|
|
|
|
|
|
}
|
|
/** Creates a new BasePilotable. */
|
|
public BasePilotable() {
|
|
avantDroit.setInverted(true);
|
|
arriereDroit.setInverted(true);
|
|
mecanum = new MecanumDrive(avantGauche, arriereGauche, avantDroit, arriereDroit);
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
// This method will be called once per scheduler run
|
|
}
|
|
|
|
}
|