diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java new file mode 100644 index 00000000000..709cf3cc7da --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java @@ -0,0 +1,108 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore; + +import org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems.MovementSubSystem; + +public class Drivetraintypes { + + public Robot bot; + public MovementSubSystem movSys; + + public enum Drivetrains{ + TANKDRIVE, + FOURWHEELTANKDRIVE, + FIVEWHEELHDRIVE, + THREEWHEELHDRIVE, + MECHANUMDRIVE, + KIWIDRIVE + } + + public void initDrivetrain(Robot _bot){ + Drivetrains Train; + bot = _bot; + Train = Robotconfig.DriveTrain; + + + switch (Train){ + case MECHANUMDRIVE: + bot.MotorBackLeft = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); + bot.MotorFrontLeft = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontLeft); + bot.MotorFrontRight = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontRight); + bot.MotorBackRight = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); + break; + + case TANKDRIVE: + bot.MotorBackLeft = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); + bot.MotorBackRight = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); + break; + + case FOURWHEELTANKDRIVE: + bot.MotorBackLeft = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); + bot.MotorFrontLeft = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontLeft); + bot.MotorFrontRight = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontRight); + bot.MotorBackRight = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); + break; + + case KIWIDRIVE: + bot.MotorBackLeft = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); + bot.MotorFrontLeft = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontLeft); + bot.MotorFrontRight = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontRight); + bot.MotorBackRight = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); + break; + + case FIVEWHEELHDRIVE: + bot.MotorBackLeft = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); + bot.MotorFrontLeft = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontLeft); + bot.MotorFrontRight = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontRight); + bot.MotorBackRight = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); + bot.MotorMiddle = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorMiddle); + break; + + case THREEWHEELHDRIVE: + bot.MotorBackLeft = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); + bot.MotorBackRight = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); + bot.MotorMiddle = bot.opMode.hardwareMap.dcMotor.get(Robotconfig.MotorMiddle); + break; + + default: + System.out.println("PLEASE SELECT A DRIVETRAIN"); + break; + } + + } + + public void DriveChecks(){ + Drivetrains Train; + + Train = Robotconfig.DriveTrain; + + switch (Train){ + case MECHANUMDRIVE: + movSys.instance.DriveChecksMechanum(); + break; + + case TANKDRIVE: + movSys.instance.DriveChecksTankDrive(); + break; + + case FOURWHEELTANKDRIVE: + movSys.instance.DriveChecks4WheelTankDrive(); + break; + + case KIWIDRIVE: + movSys.instance.DriveChecksKiwiDrive(); + break; + + case FIVEWHEELHDRIVE: + movSys.instance.DriveChecksHDrive5Motors(); + break; + + case THREEWHEELHDRIVE: + movSys.instance.DriveChecksHDrive3Motors(); + break; + + default: + //System.out.println("PLEASE SELECT A DRIVETRAIN"); + break; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Function_config_file.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Function_config_file.java new file mode 100644 index 00000000000..a2a42401461 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Function_config_file.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore; + +public class Function_config_file { + + //Drivedirection speed + public static boolean drivedirectionspeed = true; + + //function which the drivedir. follows + public static boolean linear = true; + public static boolean quadratic = false; + public static boolean cubic = false; + + //the value with which the x value increments on the function + public static float dx = 0.05f; + + //initial value of drivedirectionspeed + public static double drivespeed = 1; + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Math/Mathfunc.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Math/Mathfunc.java new file mode 100644 index 00000000000..8ddb55c83e7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Math/Mathfunc.java @@ -0,0 +1,59 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore.Math; + +public class Mathfunc { + + public static double clamp(double min, double max, double var){ + if (var < min){ + var = min; + } else if (var > max) { + var = max; + } + + return var; + } + + public static double[] clampProportion(double min, double max, double var0, double var1){ + double[] vars = new double[2]; + if(var0 > var1){ + if (var0 > max){ + var0 = max; + var1 = (var1 * max)/var0; + } + if (var1 < min){ + var1 = max; + var0 = (var0 * max)/var1; + } + }else if(var0 < var1){ + if (var1 > max){ + var0 = max; + var1 = (var1 * max)/var0; + } + if (var0 < min){ + var1 = max; + var0 = (var0 * max)/var1; + } + } + + vars[0] = var0; + vars[1] = var1; + return vars; + } + + public static boolean range(double offset, double var, double goal){ + return var > goal - offset && var < goal + offset; + } + + public static double FixAngle (double angle){ + while(angle < -180){ + angle += 360; + } + while (angle > 180) { + angle -= 360; + } + return angle; + } + + + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Math/Vector2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Math/Vector2.java new file mode 100644 index 00000000000..c0a0cafe5b2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Math/Vector2.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore.Math; + +public class Vector2 { + public double X; + public double Y; + + public double DistanceToVector2(Vector2 b) { + return Math.sqrt((this.X - b.X)*(this.X - b.X) + (this.Y - b.Y)*(this.Y - b.Y)); + } + + public Vector2 subtract (Vector2 b) { + Vector2 a = new Vector2(); + a.X = this.X - b.X; + a.Y = this.Y - b.Y; + return a; + } + + public Vector2 add (Vector2 b) { + Vector2 a = new Vector2(); + a.X = this.X + b.X; + a.Y = this.Y +b.Y; + return a; + } + public static Vector2 add (Vector2 a, Vector2 b){ + Vector2 c = new Vector2( + a.X + b.X, + a.Y + b.Y + ); + return c; + } + + public Vector2 (double x, double y) { + X = x; + Y = y; + } + public Vector2() { + X = 0; + Y = 0; + } + + + + public double Slope (){ + return (Y/X); + } + + public double Angle(){ + return Math.toDegrees(Math.atan(Slope())); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Optional_functions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Optional_functions.java new file mode 100644 index 00000000000..37eddb5ceb1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Optional_functions.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore; + + +//this class contains all function from the configfile +//go to that file for any changes +public class Optional_functions { + //Drivedirection speed + public void drivedirectionspeed(){ + + if (Function_config_file.drivedirectionspeed){ + double dds = Function_config_file.drivespeed; + if(Function_config_file.linear){ + dds = dds + Function_config_file.dx; + }else if(Function_config_file.quadratic){ + dds = Math.pow(Math.sqrt(dds) + Function_config_file.dx, 2); + }else if(Function_config_file.cubic){ + dds = Math.pow(Math.cbrt(dds) + Function_config_file.dx, 3); + }else{ + //message that says to select a function + } + + } + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java new file mode 100644 index 00000000000..6c0a5f23344 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -0,0 +1,83 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore; + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems.GuidanceSubSystem; +import org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems.IMUSubSystem; +import org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems.SubSystem; +import org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems.MovementSubSystem; + +import java.util.ArrayList; +import java.util.List; + +public class Robot { + //instance + public static Robot instance; + + //current opmode, set by constructor + public OpMode opMode; + + //drivetrain + public DcMotor MotorBackLeft; + public DcMotor MotorFrontLeft; + public DcMotor MotorFrontRight; + public DcMotor MotorBackRight; + public DcMotor MotorMiddle; + public BNO055IMU imu; + + public Drivetraintypes drivetrains; + + //list of subsystems + public List subSystems; + + + public Robot (OpMode _opmode) { + instance = this; + opMode = _opmode; + drivetrains = new Drivetraintypes(); + drivetrains.initDrivetrain(this); + imu = opMode.hardwareMap.get(BNO055IMU.class,"imu"); + subSystems = new ArrayList<>(); + } + + public void Update (){ + for (SubSystem system : subSystems) + { + system.Update(); + } + } + + /** + * calls the calibration functions on all sensors + */ + public void Calibrate(){ + if (IMUSubSystem.instance != null){ + IMUSubSystem.instance.Calibrate(); + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Create start functions for all subsystems here // + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + + public void StartIMUSubSystem () { + IMUSubSystem a = new IMUSubSystem(); + a.Start(); + subSystems.add(a); + } + + public void StartMovementSubSystem(){ + MovementSubSystem a = new MovementSubSystem(); + a.Start(); + subSystems.add(a); + } + + public void StartGuidanceSubSystem () { + GuidanceSubSystem a = new GuidanceSubSystem(); + a.Start(); + subSystems.add(a); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java new file mode 100644 index 00000000000..f48219c96ba --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore; + +public class Robotconfig { + + //drivetrain motors + public static String MotorBackLeft = "MotorBackLeft"; + public static String MotorFrontLeft = "MotorFrontLeft"; + public static String MotorFrontRight = "MotorFrontRight"; + public static String MotorBackRight = "MotorBackRight"; + public static String MotorMiddle = "MotorMiddle"; + + //TeleOp variables; + public static double ShortCutSpeed = 0.3; + + //servo ports + public static String Servo0 = "Servo0"; + public static String Servo1 = "Servo1"; + public static String Servo2 = "Servo2"; + public static String Servo3 = "Servo3"; + public static String Servo4 = "Servo4"; + public static String Servo5 = "Servo5"; + + //drivetrains + public static Drivetraintypes.Drivetrains DriveTrain = Drivetraintypes.Drivetrains.MECHANUMDRIVE; + + //logging + public static String teamName = "FTCunits"; + public static boolean isLoggingUsed; + + //guidance subsystem + public static boolean posControl; + public static boolean angleControl = true; + public static double angleKp = 1/20; + public static double angleKi = 1/100; + public static double angleKd = 1/25; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/GuidanceSubSystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/GuidanceSubSystem.java new file mode 100644 index 00000000000..279dbb4095e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/GuidanceSubSystem.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems; + +import org.firstinspires.ftc.teamcode.DutchFTCCore.Robotconfig; + +public class GuidanceSubSystem extends SubSystem { + + public static GuidanceSubSystem instance; + + //targets, set by external subsystems + public static double targetAngle; + public static double targetxPos; + public static double targetyPos; + + //output, read by external subsystems + public static double angleMovement; + + //angle data, don't change + double angleErrorPrev; + double angleIntegral; + double lastAngleTime; + @Override + public void Start () { + instance = this; + } + + @Override + public void Update(){ + super.Update(); + if (Robotconfig.angleControl){ + angleControlPID(); + if(MovementSubSystem.instance != null){ + MovementSubSystem.rotation = angleMovement; + } + } + } + + void angleControlPID () { + double currAngleTime = System.currentTimeMillis(); + double error = targetAngle - IMUSubSystem.currHeading; + double dt = currAngleTime - lastAngleTime; + angleIntegral = angleIntegral + error*dt; + double angleDeriv = (error-angleErrorPrev)/dt; + angleMovement = Robotconfig.angleKp*error + Robotconfig.angleKi*angleIntegral + Robotconfig.angleKd*angleDeriv; + angleErrorPrev = error; + lastAngleTime = currAngleTime; + + } + + //TODO: Add position controlq + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/IMUSubSystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/IMUSubSystem.java new file mode 100644 index 00000000000..1af536b207a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/IMUSubSystem.java @@ -0,0 +1,56 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems; + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.robotcore.util.ReadWriteFile; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; +import org.firstinspires.ftc.teamcode.DutchFTCCore.Robot; + +import java.io.File; +import java.nio.ByteBuffer; + +public class IMUSubSystem extends SubSystem { + + public static IMUSubSystem instance; + public static double currHeading; + + @Override + public void Start() { + instance = this; + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + parameters.mode = BNO055IMU.SensorMode.IMU; + parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; + parameters.pitchMode = BNO055IMU.PitchMode.ANDROID; + Robot.instance.imu.initialize(parameters); + } + + /** + * Call this to calibrate the robot; copied from calibration opmode + */ + public void Calibrate () { + BNO055IMU.CalibrationData calibrationData = Robot.instance.imu.readCalibrationData(); + + // Save the calibration data to a file. You can choose whatever file + // name you wish here, but you'll want to indicate the same file name + // when you initialize the IMU in an opmode in which it is used. If you + // have more than one IMU on your robot, you'll of course want to use + // different configuration file names for each. + String filename = "AdafruitIMUCalibration.json"; + File file = AppUtil.getInstance().getSettingsFile(filename); + ReadWriteFile.writeFile(file, calibrationData.serialize()); + } + + @Override + public void Update() { + super.Update(); + currHeading = Robot.instance.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).firstAngle; + //polls the sensor directly, converts from byte array to double + //byte[] a = new byte[2]; + //a[1] = Robot.instance.imu.read8(BNO055IMU.Register.EUL_H_LSB); + //a[0] = Robot.instance.imu.read8(BNO055IMU.Register.EUL_H_MSB); + //currHeading = ByteBuffer.wrap(a).getFloat(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/LoggingSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/LoggingSubsystem.java new file mode 100644 index 00000000000..9623c7c885b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/LoggingSubsystem.java @@ -0,0 +1,9 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems; + +public class LoggingSubsystem extends SubSystem { + + @Override + public void Start() { + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/MovementSubSystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/MovementSubSystem.java new file mode 100644 index 00000000000..12c2e4da84e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/MovementSubSystem.java @@ -0,0 +1,135 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems; + +import org.firstinspires.ftc.teamcode.DutchFTCCore.Drivetraintypes; +import org.firstinspires.ftc.teamcode.DutchFTCCore.Robot; +import org.firstinspires.ftc.teamcode.DutchFTCCore.Robotconfig; + +public class MovementSubSystem extends SubSystem { + public static MovementSubSystem instance; + Robot bot; + Drivetraintypes traintypes; + /** + * Movement of the robot on the x axis in a value between -1 and 1; + */ + public static double xMov; + /** + * Movement of the robot on the y axis in a value between -1 and 1; + */ + public static double yMov; + /** + * Rotational input of the robot in a value between -1 and 1; + */ + public static double rotation; + + @Override + public void Start() { + super.Start(); + bot = Robot.instance; + instance = this; + traintypes = bot.drivetrains; + } + + @Override + public void Update() { + super.Update(); + traintypes.DriveChecks(); + } + + public void DriveChecksKiwiDrive(){ + //the variables for the motor speeds + double right = -yMov + rotation; + double left = yMov + rotation; + double front = xMov + rotation; + double back = -xMov + rotation; + + //setting the motor speeds + bot.MotorBackLeft.setPower(left); + bot.MotorFrontLeft.setPower(back); + bot.MotorBackRight.setPower(front); + bot.MotorFrontRight.setPower(right); + } + + public void DriveChecks4WheelTankDrive(){ + //the variables for the motor speeds + double right = -yMov + rotation; + double left = yMov + rotation; + + //setting the motor speeds + bot.MotorBackLeft.setPower(left); + bot.MotorFrontLeft.setPower(left); + bot.MotorBackRight.setPower(right); + bot.MotorFrontRight.setPower(right); + + } + + public void DriveChecksTankDrive(){ + //the variables for the motor speeds + double right = -yMov + rotation; + double left = yMov + rotation; + + //setting the motor speeds + bot.MotorBackLeft.setPower(left); + bot.MotorBackRight.setPower(right); + + } + + public void DriveChecksHDrive5Motors(){ + + //the variables for the motor speeds + double right = -yMov + rotation; + double left = yMov + rotation; + double middle = xMov; + + //setting the motor speeds + bot.MotorBackLeft.setPower(left); + bot.MotorBackRight.setPower(right); + bot.MotorFrontLeft.setPower(left); + bot.MotorFrontRight.setPower(right); + bot.MotorMiddle.setPower(middle); + + } + + public void DriveChecksHDrive3Motors(){ + + //the variables for the motor speeds + double right = -yMov + rotation; + double left = yMov + rotation; + double middle = xMov; + + //setting the motor speeds + bot.MotorBackLeft.setPower(left); + bot.MotorBackRight.setPower(right); + bot.MotorMiddle.setPower(middle); + + } + + public void DriveChecksMechanum(){ + + double stickangle; + double speed; + double frontLeftPower; + double frontRightPower; + double backLeftPower; + double backRightPower; + + //radius of circle the stick is in + speed = (Math.hypot(xMov, yMov)); + + //inverse tangent calculate angle of stick + stickangle = Math.atan2(xMov, yMov); + + //twisting the circle of units 45 degrees + stickangle -= Math.PI / 4; + + frontLeftPower = (speed * Math.cos(stickangle) + rotation); + frontRightPower = (speed * Math.sin(stickangle) - rotation); + backLeftPower = (speed * Math.sin(stickangle) + rotation); + backRightPower = (speed * Math.cos(stickangle) - rotation); + + bot.MotorFrontLeft.setPower(frontLeftPower); + bot.MotorFrontRight.setPower(frontRightPower); + bot.MotorBackLeft.setPower(backLeftPower); + bot.MotorBackRight.setPower(backRightPower); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/SubSystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/SubSystem.java new file mode 100644 index 00000000000..51dca088fe9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/SubSystem.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems; + +/* +Base class for all subsystems + */ +public class SubSystem { + + public void Start() { + + } + + + public void Update(){ + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java new file mode 100644 index 00000000000..4b9bea7cb13 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.firstinspires.ftc.teamcode.DutchFTCCore.Robot; +import org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems.GuidanceSubSystem; +import org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems.MovementSubSystem; + +@TeleOp( name = "TestOpmode") +public class TestOpMode extends OpMode { + float eenGetal; + Robot bot; + + @Override + public void init() { + bot = new Robot(this); + bot.StartIMUSubSystem(); + bot.StartGuidanceSubSystem(); + bot.StartMovementSubSystem(); + bot.MotorBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + bot.MotorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + bot.Calibrate(); + } + + @Override + public void loop() { + bot.Update(); + //GuidanceSubSystem.targetAngle = 90; + MovementSubSystem.yMov = 1; + } + + +}