initial commit

TeamCode/build.gradle

@@ -22,7 +22,12 @@ android {
         jniLibs.useLegacyPackaging true
     }
 }
+repositories {
+    maven { url = 'https://jitpack.io' }
+}
 
 dependencies {
     implementation project(':FtcRobotController')
+    implementation 'org.ftclib.ftclib:core:2.1.1'
+    implementation 'com.github.Eeshwar-Krishnan:PhotonFTC:v3.0.3-ALPHA'
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveConstants.java

@@ -0,0 +1,13 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class DriveConstants {
+
+    public static double kP = 0;
+    public static double kI = 0;
+    public static double kD = 0;
+    public static double kS = 0;
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/IntakeConstants.java

@@ -0,0 +1,10 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class IntakeConstants {
+    public static double foldedPos = 0.6;
+    public static double openPos = 0.4;
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PivotConstants.java

@@ -0,0 +1,20 @@
+
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class PivotConstants {
+    public static double kP = 0.4;
+    public static double kI = 0;
+    public static double kD = 0;
+    public static double kS = 0;
+    public static double bottomLimit;
+    public static double topLimit;
+    public static double tolerance = 0.05;
+    public static double direction = 1;
+    public static boolean encoderInvert = false;
+    public static double offset = 0;
+    public static double bottomPos = 1.92;
+    public static double topPos = 0.384;
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SlideConstants.java

@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+
+public class SlideConstants {
+    public static double kP = 0.006;
+    public static double kI = 0;
+    public static double kD = 0;
+    public static double kS = 0;
+    public static double ticksPerInch = 31.6;
+    public static double maxExtension = 27;
+    public static double direction = 1;
+    public static double tolerance = 0.5;
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/DefaultDrive.java

@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.OTOSSubsystem;
+
+import java.util.function.DoubleSupplier;
+
+public class DefaultDrive extends CommandBase {
+    MecanumDriveSubsystem drive;
+    DoubleSupplier x, y, rx, heading;
+    public DefaultDrive(MecanumDriveSubsystem driveSubsystem, DoubleSupplier inputX, DoubleSupplier inputY, DoubleSupplier inputRx) {
+        this.drive = driveSubsystem;
+        this.x = inputX;
+        this.y = inputY;
+        this.rx = inputRx;
+        addRequirements(drive);
+    }
+    @Override
+    public void execute() {
+        drive.driveFieldCentric(-x.getAsDouble(),y.getAsDouble(),rx.getAsDouble(),0);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/AnalogEncoder.java

@@ -0,0 +1,67 @@
+package org.firstinspires.ftc.teamcode.lib;
+
+
+
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+public class AnalogEncoder {
+
+    private AnalogInput sensor;
+    private double offset;
+    private double maxVoltage;
+    private boolean inverted;
+    private double maxAngle;
+
+    {
+        // default to 3.3
+        maxVoltage = 3.3;
+
+        // default to 2pi
+        maxAngle = Math.PI * 2;
+
+        // default to 0
+        offset = 0;
+
+        // default to false
+        inverted = false;
+    }
+
+    public AnalogEncoder(String key, HardwareMap hMap){
+        this.sensor = hMap.analogInput.get(key);
+    }
+
+    public void setInverted(boolean inverted){
+        this.inverted = inverted;
+    }
+
+    /**
+     * Sets offset of the encoder
+     */
+    public void setPositionOffset(double offset){
+        this.offset = offset;
+    }
+
+    public void setMaxVoltage(double maxVoltage){
+        this.maxVoltage = maxVoltage;
+    }
+
+    /**
+     * Sets the maximum angle of the analog encoder. This means that 3.3V = max angle.
+     * @param maxAngle Angle at 3.3V.
+     */
+    public void setMaxAngle(double maxAngle) {
+        this.maxAngle = maxAngle;
+    }
+
+    /**
+     * @return Radians.
+     */
+    public double getAngle(){
+        return AngleUnit.normalizeRadians((inverted ? -1 : 1) * (sensor.getVoltage() * maxAngle/maxVoltage) + offset);
+    }
+
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Localizer.java

@@ -0,0 +1,7 @@
+package org.firstinspires.ftc.teamcode.lib;
+
+import com.arcrobotics.ftclib.geometry.Pose2d;
+
+public interface Localizer {
+    public Pose2d getPose();
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/SquIDController.java

@@ -0,0 +1,18 @@
+package org.firstinspires.ftc.teamcode.lib;
+
+import com.arcrobotics.ftclib.controller.PIDController;
+
+public class SquIDController {
+    double p, i, d;
+    public SquIDController() {
+        p=0;
+        i=0;
+        d=0;
+    }
+    public void setPID(double p) {
+        this.p = p;
+    }
+    public double calculate(double setpoint, double current) {
+        return Math.sqrt(Math.abs((setpoint-current)*p))*Math.signum(setpoint-current);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Util.java

@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.lib;
+
+public class Util {
+    public static boolean inRange(double a, double b, double thres) {
+        return Math.abs(a-b)<thres;
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/ExtensionPIDTest.java

@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.subsystems.ExtensionSubsystem;
+@Config
+@TeleOp
+public class ExtensionPIDTest extends LinearOpMode {
+    public static double targetInches = 0;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        ExtensionSubsystem extensionSubsystem = new ExtensionSubsystem(hardwareMap);
+        waitForStart();
+        while (!isStopRequested()) {
+            extensionSubsystem.periodic();
+            extensionSubsystem.extendInches(targetInches);
+
+            telemetry.addData("current pos", extensionSubsystem.getCurrentPosition());
+            telemetry.addData("target", targetInches);
+            telemetry.addData("current inches", extensionSubsystem.getCurrentInches());
+            telemetry.addData("is there", extensionSubsystem.isClose(targetInches));
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PivotPIDTest.java

@@ -0,0 +1,31 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.subsystems.ExtensionSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.PivotSubsystem;
+
+@Config
+@TeleOp
+public class PivotPIDTest extends LinearOpMode {
+    public static double target = 0;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        PivotSubsystem pivot = new PivotSubsystem(hardwareMap);
+        waitForStart();
+        while (!isStopRequested()) {
+            pivot.periodic();
+            pivot.tiltToPos(Math.toRadians(target));
+
+            telemetry.addData("current pos", Math.toDegrees(pivot.getCurrentPosition()));
+            telemetry.addData("target", (Math.toRadians(target)));
+            telemetry.addData("is there", pivot.isClose((Math.toRadians(target))));
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOpTest.java

@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.opmode;
+
+import com.arcrobotics.ftclib.command.CommandScheduler;
+import com.arcrobotics.ftclib.command.InstantCommand;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.commands.DefaultDrive;
+import org.firstinspires.ftc.teamcode.subsystems.Robot;
+@TeleOp
+public class TeleOpTest extends Robot {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        initialize();
+        GamepadEx driverPad = new GamepadEx(gamepad1);
+        DefaultDrive drive = new DefaultDrive(mecanum, driverPad::getLeftY, driverPad::getLeftX, driverPad::getRightX);
+        CommandScheduler.getInstance().setDefaultCommand(mecanum,drive);
+        while (!isStopRequested()) {
+            update();
+            telemetry.addData("motorpos", extension.getCurrentPosition());
+            extension.setPower(driverPad.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) - driverPad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER));
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ExtensionSubsystem.java

@@ -0,0 +1,67 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.arcrobotics.ftclib.controller.PIDController;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.SlideConstants;
+import org.firstinspires.ftc.teamcode.lib.SquIDController;
+import org.firstinspires.ftc.teamcode.lib.Util;
+
+public class ExtensionSubsystem extends SubsystemBase {
+    private DcMotor motor0, motor1;
+    private int currentPos = 0;
+    private PIDController pid = new PIDController(SlideConstants.kP, SlideConstants.kI, SlideConstants.kD);
+    private SquIDController squid = new SquIDController();
+
+
+    public ExtensionSubsystem(HardwareMap hMap) {
+        motor0 = hMap.dcMotor.get("slide0");
+        motor0.setDirection(DcMotorSimple.Direction.REVERSE);
+        motor1 = hMap.dcMotor.get("slide1");
+        motor1.setDirection(DcMotorSimple.Direction.REVERSE);
+    }
+    @Override
+    public void periodic() {
+        currentPos = -motor0.getCurrentPosition();
+    }
+    public void setPower(double power) {
+        motor0.setPower(power*SlideConstants.direction);
+        motor1.setPower(-power*SlideConstants.direction);
+    }
+    public void extendInches(double inches) {
+        extendToPosition((int) (inches*SlideConstants.ticksPerInch));
+    }
+    public void extendToPosition(int ticks) {
+        //pid.setPID(SlideConstants.kP, SlideConstants.kI, SlideConstants.kD);
+        squid.setPID(SlideConstants.kP);
+        //setPower(Math.sqrt(pid.calculate(getCurrentPosition(),ticks)));
+        double power = squid.calculate(ticks, getCurrentPosition());
+        if ((currentPos < 0 && power < 0)||currentPos > 27 && power > 0) {
+            power = 0;
+        }
+        setPower(power);
+
+    }
+    /**
+    * @param target in inches, use the same one as the pid target
+    */
+    public boolean isClose(double target) {
+        return Util.inRange(target, getCurrentInches(), SlideConstants.tolerance);
+    }
+
+
+    public int getCurrentPosition() {
+        return currentPos;
+    }
+    public double getCurrentInches() {
+        return getCurrentPosition()/SlideConstants.ticksPerInch;
+    }
+
+    public void stop() {
+        motor0.setPower(0);
+        motor1.setPower(0);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IMULocalizer.java

@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.arcrobotics.ftclib.geometry.Pose2d;
+import com.arcrobotics.ftclib.geometry.Rotation2d;
+
+import org.firstinspires.ftc.teamcode.lib.Localizer;
+
+public class IMULocalizer extends SubsystemBase implements Localizer {
+    public IMULocalizer() {
+
+    }
+    @Override
+    public Pose2d getPose() {
+        return new Pose2d(0,0, new Rotation2d(0));
+    }
+}