robot-2023/src/main/java/frc/robot/subsystems/BasePilotable.java

81 lines
3.2 KiB
Java
Raw Normal View History

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;
2023-02-20 18:53:50 -05:00
import edu.wpi.first.wpilibj.DriverStation;
2023-02-13 18:34:10 -05:00
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-20 19:57:40 -05:00
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
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-20 19:57:40 -05:00
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
2023-02-20 19:57:40 -05:00
ShuffleboardTab teb = Shuffleboard.getTab("teb");
2023-02-13 19:26:19 -05:00
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:38:45 -05:00
public void drive(double xSpeed, double zRotation){
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-20 18:53:50 -05:00
public void resetGyro(){
try {gyroscope.reset();} catch(Exception e){DriverStation.reportError("bye bye",true);
}
}
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() {
2023-02-20 19:57:40 -05:00
teb .add("encodeuravantdroit",0.1);
teb .add("encodeurarrieregauche",0.1);
teb .add("encodeurarrieredroit",0.1);
teb .add("encodeuravantgauche",0.1);
teb .add("distance",0.1);
2023-02-06 19:02:17 -05:00
}
}