2023-02-06 19:02:17 -05:00
|
|
|
// 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;
|
|
|
|
|
2023-02-13 19:07:40 -05:00
|
|
|
import com.kauailabs.navx.frc.AHRS;
|
2023-02-06 19:37:01 -05:00
|
|
|
import com.revrobotics.CANSparkMax;
|
|
|
|
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
2023-02-13 18:34:10 -05:00
|
|
|
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
|
|
|
import edu.wpi.first.wpilibj.PneumaticsModuleType;
|
2023-02-13 19:19:52 -05:00
|
|
|
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
|
2023-02-06 19:37:01 -05:00
|
|
|
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
|
|
|
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
|
2023-02-06 19:02:17 -05:00
|
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
2023-02-06 19:37:01 -05:00
|
|
|
import frc.robot.Constants;
|
2023-02-06 19:02:17 -05:00
|
|
|
|
|
|
|
public class BasePilotable extends SubsystemBase {
|
2023-02-06 19:37:01 -05:00
|
|
|
final CANSparkMax avantdroit = new CANSparkMax(Constants.avantdroit, MotorType.kBrushless);
|
|
|
|
final CANSparkMax avantgauche = new CANSparkMax(Constants.avantgauche, MotorType.kBrushless);
|
|
|
|
final CANSparkMax arrieredroit = new CANSparkMax(Constants.arrieredroit, MotorType.kBrushless);
|
|
|
|
final CANSparkMax arrieregauche = new CANSparkMax(Constants.arrieregauche, MotorType.kBrushless);
|
|
|
|
final MotorControllerGroup droit = new MotorControllerGroup(avantdroit, arrieredroit);
|
|
|
|
final MotorControllerGroup gauche = new MotorControllerGroup(avantgauche, arrieregauche);
|
|
|
|
final DifferentialDrive drive = new DifferentialDrive(gauche, droit);
|
2023-02-13 19:26:19 -05:00
|
|
|
//piston
|
2023-02-13 19:19:52 -05:00
|
|
|
private DoubleSolenoid brakedroit = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakedroit, Constants.brakedroit);
|
|
|
|
private DoubleSolenoid brakegauche = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakegauche, Constants.brakegauche);
|
2023-02-13 19:26:19 -05:00
|
|
|
//gyro
|
|
|
|
private AHRS gyroscope = new AHRS();public double getangle() {return gyroscope.getAngle();}
|
|
|
|
public double getpitch() {
|
2023-02-13 19:07:40 -05:00
|
|
|
return gyroscope.getPitch();
|
|
|
|
}
|
|
|
|
|
2023-02-15 19:20:58 -05:00
|
|
|
public void drive(double xSpeed, double zRotation, double d){
|
2023-02-08 18:20:19 -05:00
|
|
|
drive.arcadeDrive(xSpeed, zRotation);
|
|
|
|
}
|
|
|
|
public double distance(){
|
|
|
|
return (-avantdroit.getEncoder().getPosition()
|
|
|
|
+avantgauche.getEncoder().getPosition()
|
|
|
|
-arrieredroit.getEncoder().getPosition()
|
|
|
|
+arrieregauche.getEncoder().getPosition()) / 4;
|
|
|
|
}
|
|
|
|
public void Reset() {
|
|
|
|
avantdroit.getEncoder().setPosition(0);
|
|
|
|
avantgauche.getEncoder().setPosition(0);
|
|
|
|
arrieredroit.getEncoder().setPosition(0);
|
|
|
|
arrieregauche.getEncoder().setPosition(0);
|
|
|
|
}
|
2023-02-13 19:19:52 -05:00
|
|
|
public void BrakeOuvre(){
|
|
|
|
brakedroit.set(Value.kForward);
|
|
|
|
brakegauche.set(Value.kForward);
|
|
|
|
}
|
|
|
|
public void BrakeFerme(){
|
|
|
|
brakedroit.set(Value.kReverse);
|
|
|
|
brakegauche.set(Value.kReverse);
|
|
|
|
}
|
2023-02-06 19:02:17 -05:00
|
|
|
/** Creates a new BasePilotable. */
|
2023-02-06 19:37:01 -05:00
|
|
|
public BasePilotable() {
|
|
|
|
droit.setInverted(true);
|
|
|
|
}
|
2023-02-06 19:02:17 -05:00
|
|
|
|
|
|
|
@Override
|
|
|
|
public void periodic() {
|
|
|
|
// This method will be called once per scheduler run
|
2023-02-08 18:11:46 -05:00
|
|
|
|
2023-02-06 19:02:17 -05:00
|
|
|
}
|
|
|
|
}
|