From 7f1fd3d4bbe292e0b05d965acf50d120182b0612 Mon Sep 17 00:00:00 2001 From: Guus Dohmen Date: Sat, 18 Jan 2020 15:39:21 +0100 Subject: [PATCH 01/25] start of dutchftccore --- .../ftc/teamcode/DutchFTCCore/Robot.java | 44 +++++++++++++++++++ .../teamcode/DutchFTCCore/Robotconfig.java | 16 +++++++ .../SubSystems/LoggingSubsystem.java | 9 ++++ .../DutchFTCCore/SubSystems/SubSystem.java | 16 +++++++ 4 files changed, 85 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/LoggingSubsystem.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/SubSystem.java 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..db38ba5a5bc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -0,0 +1,44 @@ +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.SubSystem; + +import java.util.List; + +public class Robot { + //current opmode, set by constructor + public OpMode opMode; + + //drivetrain + public DcMotor MotorBackLeft; + public DcMotor MotorFrontLeft; + public DcMotor MotorFrontRight; + public DcMotor MotorBackRight; + public BNO055IMU imu; + + //list of subsystems + public List subSystems; + public Robot (OpMode _opmode) { + opMode = _opmode; + MotorBackLeft = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); + MotorFrontLeft = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontLeft); + MotorFrontRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontRight); + MotorBackRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); + imu = opMode.hardwareMap.get(BNO055IMU.class,"imu"); + } + + public void Update (){ + for (SubSystem system : subSystems) + { + system.Update(); + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Create start functions for all subsystems here // + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +} \ 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..2858bd64a83 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore; + +import android.util.Log; + +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"; + + //logging + public static String teamName = "FTCunits"; + public static boolean isUsed +} 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/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(){ + + } +} From d449b36f30b1b513bfd7a9727e88f9337c081b74 Mon Sep 17 00:00:00 2001 From: JanTheBeast Date: Sat, 18 Jan 2020 22:15:16 +0100 Subject: [PATCH 02/25] Added drivetrain types This is a first implementation of the different drivetrain types, perhaps later a system will be implemented so that you can only select one at a time. --- .../ftc/teamcode/DutchFTCCore/Robotconfig.java | 7 +++++++ 1 file changed, 7 insertions(+) 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 index 2858bd64a83..c786feec7fe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java @@ -10,6 +10,13 @@ public class Robotconfig { public static String MotorFrontRight = "MotorFrontRight"; public static String MotorBackRight = "MotorBackRight"; + //drivetrains + public static boolean TankDrive2Motors = false; + public static boolean TankDrive1Motor = true; + public static boolean MechanumDrive = false; + public static boolean H_Drive = false; + public static boolean CrossOmniDrive = false; + //logging public static String teamName = "FTCunits"; public static boolean isUsed From 7ca5e8592a8cd0776c3bea5dba57579be9ccde14 Mon Sep 17 00:00:00 2001 From: JanTheBeast Date: Sat, 18 Jan 2020 22:39:02 +0100 Subject: [PATCH 03/25] added more config added a file in which people can enable optional function and adjust those functions, also added the files that implement those function. The strings for the 6 servo slots were also added --- .../DutchFTCCore/Function_config_file.java | 17 ++++++++++++ .../DutchFTCCore/Optional_functions.java | 26 +++++++++++++++++++ .../teamcode/DutchFTCCore/Robotconfig.java | 8 ++++++ 3 files changed, 51 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Function_config_file.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Optional_functions.java 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..3ac88da16de --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Function_config_file.java @@ -0,0 +1,17 @@ +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; + + +} 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..f1df92d63da --- /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){ + + if(Function_config_file.linear){ + //linear function that take dx + }else if(Function_config_file.quadratic){ + //quadratic functions that takes dx + }else if(Function_config_file.cubic){ + //cubic function that take dx + }else{ + //message that says to select a function + } + + } + + } + +} 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 index c786feec7fe..1cc46a1484b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java @@ -10,6 +10,14 @@ public class Robotconfig { public static String MotorFrontRight = "MotorFrontRight"; public static String MotorBackRight = "MotorBackRight"; + //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 boolean TankDrive2Motors = false; public static boolean TankDrive1Motor = true; From be42c1482e5be8522dcfe46795c9cc6ffd5cf158 Mon Sep 17 00:00:00 2001 From: JanTheBeast Date: Sat, 18 Jan 2020 22:41:18 +0100 Subject: [PATCH 04/25] bug fixing the entire project Added a very essential semi-colon on line 30 of the robotconfig file. How can you even call yourself a programmer, GUUS? --- .../ftc/teamcode/DutchFTCCore/Function_config_file.java | 2 +- .../firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 index 3ac88da16de..8c002f5cc11 100644 --- 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 @@ -9,7 +9,7 @@ public class Function_config_file { 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; 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 index 1cc46a1484b..5bb74a2f399 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java @@ -27,5 +27,5 @@ public class Robotconfig { //logging public static String teamName = "FTCunits"; - public static boolean isUsed + public static boolean isUsed; } From c84653ddfd07a737d2f2cf63dd21ef1d064e58cd Mon Sep 17 00:00:00 2001 From: Guus Dohmen Date: Sun, 19 Jan 2020 00:03:06 +0100 Subject: [PATCH 05/25] Added IMUsystem currently only reads heading, needs some more work also added guidancesubsystem and testopmode --- .../ftc/teamcode/DutchFTCCore/Robot.java | 15 ++++++++ .../SubSystems/GuidanceSubSystem.java | 12 +++++++ .../DutchFTCCore/SubSystems/IMUSubSystem.java | 35 +++++++++++++++++++ .../ftc/teamcode/TestOpMode.java | 19 ++++++++++ 4 files changed, 81 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/GuidanceSubSystem.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/IMUSubSystem.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java 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 index db38ba5a5bc..01bf41acf04 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -4,11 +4,16 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; +import org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems.IMUSubSystem; import org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems.SubSystem; +import java.util.ArrayList; import java.util.List; public class Robot { + //instance + public static Robot instance; + //current opmode, set by constructor public OpMode opMode; @@ -21,13 +26,17 @@ public class Robot { //list of subsystems public List subSystems; + + public Robot (OpMode _opmode) { + instance = this; opMode = _opmode; MotorBackLeft = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); MotorFrontLeft = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontLeft); MotorFrontRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontRight); MotorBackRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); imu = opMode.hardwareMap.get(BNO055IMU.class,"imu"); + subSystems = new ArrayList(); } public void Update (){ @@ -41,4 +50,10 @@ public void Update (){ // Create start functions for all subsystems here // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + public void StartIMUSubSystem () { + IMUSubSystem a = new IMUSubSystem(); + a.Start(); + subSystems.add(a); + } } \ No newline at end of file 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..50fbfcef202 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/GuidanceSubSystem.java @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems; + +public class GuidanceSubSystem extends SubSystem { + + public static GuidanceSubSystem instance; + + @Override + public void Start () { + instance = this; + } + +} 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..5a13bcaaaf0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/IMUSubSystem.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems; + +import com.qualcomm.hardware.bosch.BNO055IMU; + +import org.firstinspires.ftc.teamcode.DutchFTCCore.Robot; +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); + } + + public void Calibrate () { + //TODO: Implement this instead of the seperate opmode for calibrating + } + + @Override + public void Update() { + super.Update(); + //polls the sensor directly, converts from byte array to double + byte[] a = Robot.instance.imu.read(BNO055IMU.Register.EUL_H_LSB, 2); + currHeading = ByteBuffer.wrap(a).getDouble(); + + } +} 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..1c7a0d01ddd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.DutchFTCCore.Robot; + +public class TestOpMode extends OpMode { + Robot bot; + @Override + public void init() { + bot = new Robot(this); + bot.StartIMUSubSystem(); + } + + @Override + public void loop() { + bot.Update(); + } +} From 5192574305cb8b5cefdc9d29fbe652490927c44e Mon Sep 17 00:00:00 2001 From: JanTheBeast Date: Sun, 19 Jan 2020 15:47:25 +0100 Subject: [PATCH 06/25] worked further on dds --- .../ftc/teamcode/DutchFTCCore/Function_config_file.java | 3 +++ .../ftc/teamcode/DutchFTCCore/Optional_functions.java | 8 ++++---- 2 files changed, 7 insertions(+), 4 deletions(-) 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 index 8c002f5cc11..a2a42401461 100644 --- 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 @@ -13,5 +13,8 @@ public class Function_config_file { //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/Optional_functions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Optional_functions.java index f1df92d63da..37eddb5ceb1 100644 --- 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 @@ -8,13 +8,13 @@ public class Optional_functions { public void drivedirectionspeed(){ if (Function_config_file.drivedirectionspeed){ - + double dds = Function_config_file.drivespeed; if(Function_config_file.linear){ - //linear function that take dx + dds = dds + Function_config_file.dx; }else if(Function_config_file.quadratic){ - //quadratic functions that takes dx + dds = Math.pow(Math.sqrt(dds) + Function_config_file.dx, 2); }else if(Function_config_file.cubic){ - //cubic function that take dx + dds = Math.pow(Math.cbrt(dds) + Function_config_file.dx, 3); }else{ //message that says to select a function } From 323ecf1873d52c28d63e49de02eaa11800855f32 Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Mon, 20 Jan 2020 09:48:34 +0100 Subject: [PATCH 07/25] Guus- Added imu calibration added the imu calibration --- .../DutchFTCCore/SubSystems/IMUSubSystem.java | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) 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 index 5a13bcaaaf0..15793d79407 100644 --- 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 @@ -1,8 +1,12 @@ package org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems; import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.robotcore.util.ReadWriteFile; +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 { @@ -21,7 +25,16 @@ public void Start() { } public void Calibrate () { - //TODO: Implement this instead of the seperate opmode for calibrating + 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 From a74be0faaaaf8ab0be3a018bc51a9e69df1f889c Mon Sep 17 00:00:00 2001 From: Guus Dohmen Date: Tue, 21 Jan 2020 16:53:50 +0100 Subject: [PATCH 08/25] Update IMUSubSystem.java --- .../ftc/teamcode/DutchFTCCore/SubSystems/IMUSubSystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 5a13bcaaaf0..847fd9a6f72 100644 --- 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 @@ -6,7 +6,7 @@ import java.nio.ByteBuffer; public class IMUSubSystem extends SubSystem { - + public static IMUSubSystem instance; public static double currHeading; From 5f44af9a9bad00e83290700ae11bd49271644a57 Mon Sep 17 00:00:00 2001 From: Guus Dohmen Date: Thu, 6 Feb 2020 11:03:34 +0100 Subject: [PATCH 09/25] Added angle PID control --- .../ftc/teamcode/DutchFTCCore/Robot.java | 14 ++++---- .../teamcode/DutchFTCCore/Robotconfig.java | 11 ++++-- .../SubSystems/GuidanceSubSystem.java | 34 +++++++++++++++++++ 3 files changed, 49 insertions(+), 10 deletions(-) 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 index 01bf41acf04..b40c9a23959 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -30,13 +30,13 @@ public class Robot { public Robot (OpMode _opmode) { instance = this; - opMode = _opmode; - MotorBackLeft = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); - MotorFrontLeft = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontLeft); - MotorFrontRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontRight); - MotorBackRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); - imu = opMode.hardwareMap.get(BNO055IMU.class,"imu"); - subSystems = new ArrayList(); + opMode = _opmode; + MotorBackLeft = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); + MotorFrontLeft = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontLeft); + MotorFrontRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontRight); + MotorBackRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); + imu = opMode.hardwareMap.get(BNO055IMU.class,"imu"); + subSystems = new ArrayList(); } public void Update (){ 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 index 5bb74a2f399..cc55599e7b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java @@ -1,7 +1,5 @@ package org.firstinspires.ftc.teamcode.DutchFTCCore; -import android.util.Log; - public class Robotconfig { //drivetrain motors @@ -27,5 +25,12 @@ public class Robotconfig { //logging public static String teamName = "FTCunits"; - public static boolean isUsed; + 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 index 50fbfcef202..204bcab1fc1 100644 --- 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 @@ -1,12 +1,46 @@ 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 + double angleErrorPrev; + double angleIntegral; + double lastAngleTime; @Override public void Start () { instance = this; } + @Override + public void Update(){ + super.Update(); + if (Robotconfig.angleControl){ + angleControlPID(); + } + } + + 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; + + } + } From 5bf4d3a607fc1368cf412cbdfb8b65e5ac545146 Mon Sep 17 00:00:00 2001 From: Guus Dohmen Date: Mon, 10 Feb 2020 10:04:04 +0100 Subject: [PATCH 10/25] Added angle guidance test Added the test to the testopmode. Will test this next saturday --- .../firstinspires/ftc/teamcode/DutchFTCCore/Robot.java | 9 ++++++++- .../DutchFTCCore/SubSystems/GuidanceSubSystem.java | 2 ++ .../java/org/firstinspires/ftc/teamcode/TestOpMode.java | 3 +++ 3 files changed, 13 insertions(+), 1 deletion(-) 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 index b40c9a23959..b188fb0acdb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -4,6 +4,7 @@ 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; @@ -36,7 +37,7 @@ public Robot (OpMode _opmode) { MotorFrontRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontRight); MotorBackRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); imu = opMode.hardwareMap.get(BNO055IMU.class,"imu"); - subSystems = new ArrayList(); + subSystems = new ArrayList<>(); } public void Update (){ @@ -56,4 +57,10 @@ public void StartIMUSubSystem () { 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/SubSystems/GuidanceSubSystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/GuidanceSubSystem.java index 204bcab1fc1..ac7f8a4fa2a 100644 --- 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 @@ -43,4 +43,6 @@ void angleControlPID () { } + //TODO: Add position control + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java index 1c7a0d01ddd..f99b4b04923 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java @@ -3,6 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.teamcode.DutchFTCCore.Robot; +import org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems.GuidanceSubSystem; public class TestOpMode extends OpMode { Robot bot; @@ -10,10 +11,12 @@ public class TestOpMode extends OpMode { public void init() { bot = new Robot(this); bot.StartIMUSubSystem(); + bot.StartGuidanceSubSystem(); } @Override public void loop() { bot.Update(); + GuidanceSubSystem.targetAngle = 90; } } From 5b0a2fa92d69bbe138484fb792fe79d9f2567821 Mon Sep 17 00:00:00 2001 From: Guus Dohmen Date: Mon, 10 Feb 2020 12:01:48 +0100 Subject: [PATCH 11/25] Welp, it's propably fucked up but I don't care. --- .../ftc/teamcode/DutchFTCCore/Robot.java | 4 ++ .../teamcode/DutchFTCCore/Robotconfig.java | 3 ++ .../SubSystems/MovementSubSystem.java | 50 +++++++++++++++++++ .../ftc/teamcode/TestOpMode.java | 4 ++ 4 files changed, 61 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/MovementSubSystem.java 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 index b188fb0acdb..632080133e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -58,6 +58,10 @@ public void StartIMUSubSystem () { subSystems.add(a); } + public void getCurrentHeading(){ + + } + public void StartGuidanceSubSystem () { GuidanceSubSystem a = new GuidanceSubSystem(); a.Start(); 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 index cc55599e7b4..999db90b66c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java @@ -8,6 +8,9 @@ public class Robotconfig { public static String MotorFrontRight = "MotorFrontRight"; public static String MotorBackRight = "MotorBackRight"; + //TeleOp variables; + public static double ShortCutSpeed = 0.3; + //servo ports public static String Servo0 = "Servo0"; public static String Servo1 = "Servo1"; 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..a6101698302 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/MovementSubSystem.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore.SubSystems; + +import org.firstinspires.ftc.teamcode.DutchFTCCore.Robot; +import org.firstinspires.ftc.teamcode.DutchFTCCore.Robotconfig; + +public class MovementSubSystem extends SubSystem { + + Robot bot; + + @Override + public void Start() { + super.Start(); + bot = Robot.instance; + } + + @Override + public void Update() { + super.Update(); + } + + public void DriveChecks(double speed, double xmov, double ymov, double rotation){ + + double stickangle; + + 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/TestOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java index f99b4b04923..a4e12341d12 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java @@ -7,6 +7,7 @@ public class TestOpMode extends OpMode { Robot bot; + @Override public void init() { bot = new Robot(this); @@ -18,5 +19,8 @@ public void init() { public void loop() { bot.Update(); GuidanceSubSystem.targetAngle = 90; + } + + } From 2c8c8e298a8a257672f2fdc3e2be44b922fc5cd9 Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Wed, 12 Feb 2020 11:02:32 +0100 Subject: [PATCH 12/25] aadded movement in robot class --- .../org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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 index 632080133e4..ca18c352ba0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -7,6 +7,7 @@ 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; @@ -58,8 +59,10 @@ public void StartIMUSubSystem () { subSystems.add(a); } - public void getCurrentHeading(){ - + public void StartMovementSubSystem(){ + MovementSubSystem a = new MovementSubSystem(); + a.Start(); + subSystems.add(a); } public void StartGuidanceSubSystem () { From e660fe50c5cc1ff63ef7de2e91da505e3c04d5c9 Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Wed, 12 Feb 2020 12:16:55 +0100 Subject: [PATCH 13/25] reworked movementSubSystem to get values from static variables -guus --- .../DutchFTCCore/SubSystems/IMUSubSystem.java | 3 +++ .../DutchFTCCore/SubSystems/MovementSubSystem.java | 11 +++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) 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 index 39a33272d6d..971fa1a1088 100644 --- 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 @@ -24,6 +24,9 @@ public void Start() { 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(); 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 index a6101698302..97a89cd8167 100644 --- 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 @@ -6,6 +6,9 @@ public class MovementSubSystem extends SubSystem { Robot bot; + public static double xMov; + public static double yMov; + public static double rotation; @Override public void Start() { @@ -18,20 +21,20 @@ public void Update() { super.Update(); } - public void DriveChecks(double speed, double xmov, double ymov, double rotation){ + public void DriveChecks(){ double stickangle; - + double speed; double frontLeftPower; double frontRightPower; double backLeftPower; double backRightPower; //radius of circle the stick is in - speed = (Math.hypot(xmov, ymov)); + speed = (Math.hypot(xMov, yMov)); //inverse tangent calculate angle of stick - stickangle = Math.atan2(xmov, ymov); + stickangle = Math.atan2(xMov, yMov); //twisting the circle of units 45 degrees stickangle -= Math.PI / 4; From 6fbf1662bab2d0c24454ba14a06393fde2a15e81 Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Wed, 12 Feb 2020 12:26:00 +0100 Subject: [PATCH 14/25] Added documentation, added rotation input --- .../DutchFTCCore/SubSystems/GuidanceSubSystem.java | 3 +++ .../DutchFTCCore/SubSystems/MovementSubSystem.java | 13 ++++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) 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 index ac7f8a4fa2a..8a10f57c5a4 100644 --- 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 @@ -28,6 +28,9 @@ public void Update(){ super.Update(); if (Robotconfig.angleControl){ angleControlPID(); + if(MovementSubSystem.instance != null){ + MovementSubSystem.rotation = angleMovement; + } } } 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 index 97a89cd8167..e7638d24205 100644 --- 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 @@ -4,21 +4,32 @@ import org.firstinspires.ftc.teamcode.DutchFTCCore.Robotconfig; public class MovementSubSystem extends SubSystem { - + public static MovementSubSystem instance; Robot bot; + /** + * 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; } @Override public void Update() { super.Update(); + DriveChecks(); } public void DriveChecks(){ From 67a7d1268cee564a37f611e0a2dec5ad9a560e9c Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Thu, 13 Feb 2020 10:52:12 +0100 Subject: [PATCH 15/25] Added calibration function to robot class --- .../firstinspires/ftc/teamcode/DutchFTCCore/Robot.java | 9 +++++++++ .../DutchFTCCore/SubSystems/GuidanceSubSystem.java | 2 +- .../java/org/firstinspires/ftc/teamcode/TestOpMode.java | 2 ++ 3 files changed, 12 insertions(+), 1 deletion(-) 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 index ca18c352ba0..fd0ac9cfbd7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -48,6 +48,15 @@ public void 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 // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 index 8a10f57c5a4..7ddba7cca59 100644 --- 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 @@ -14,7 +14,7 @@ public class GuidanceSubSystem extends SubSystem { //output, read by external subsystems public static double angleMovement; - //angle data + //angle data, don't change double angleErrorPrev; double angleIntegral; double lastAngleTime; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java index a4e12341d12..878c103c459 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java @@ -13,6 +13,8 @@ public void init() { bot = new Robot(this); bot.StartIMUSubSystem(); bot.StartGuidanceSubSystem(); + bot.StartMovementSubSystem(); + bot.Calibrate(); } @Override From 6186ab2de41ed27ab3b4b58b3a269b8863d61a1a Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Fri, 14 Feb 2020 11:32:32 +0100 Subject: [PATCH 16/25] added different movement functions --- .../teamcode/DutchFTCCore/Robotconfig.java | 2 +- .../SubSystems/MovementSubSystem.java | 19 +++++++++++++++++-- 2 files changed, 18 insertions(+), 3 deletions(-) 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 index 999db90b66c..466b6e9e44a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java @@ -24,7 +24,7 @@ public class Robotconfig { public static boolean TankDrive1Motor = true; public static boolean MechanumDrive = false; public static boolean H_Drive = false; - public static boolean CrossOmniDrive = false; + public static boolean KiwiDrive = false; //logging public static String teamName = "FTCunits"; 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 index e7638d24205..09f5ab3475c 100644 --- 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 @@ -29,10 +29,25 @@ public void Start() { @Override public void Update() { super.Update(); - DriveChecks(); } - public void DriveChecks(){ + public void DriveChecksKiwiDrive(){ + + } + + public void DriveChecks4WheelTankDrive(){ + + } + + public void DriveChecksTankDrive(){ + + } + + public void DriveChecksHDrive(){ + + } + + public void DriveChecksMechanum(){ double stickangle; double speed; From 7b310a4722d35e56bb9f60923db28593c0d9c9f2 Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Fri, 14 Feb 2020 11:54:51 +0100 Subject: [PATCH 17/25] mafs --- .../teamcode/DutchFTCCore/Math/Mathfunc.java | 49 +++++++++++++++++++ .../SubSystems/MovementSubSystem.java | 34 ++++++++++++- 2 files changed, 82 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Math/Mathfunc.java 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..07b9143dbf6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Math/Mathfunc.java @@ -0,0 +1,49 @@ +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; + } + + + + +} 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 index 09f5ab3475c..c2cd77991d2 100644 --- 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 @@ -36,14 +36,46 @@ public void DriveChecksKiwiDrive(){ } 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 DriveChecksHDrive(){ + public void DriveChecksHDrive5Motors(){ + double left; + double right; + double 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); } From ba41677042cc4aeb741d000f75f9bda90a44189a Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Fri, 14 Feb 2020 12:26:10 +0100 Subject: [PATCH 18/25] Added drivetrain enum with switch statement --- .../DutchFTCCore/Drivetraintypes.java | 47 +++++++++++++++++++ .../teamcode/DutchFTCCore/Robotconfig.java | 6 +-- 2 files changed, 48 insertions(+), 5 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java 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..3bcdf2b9d6d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode.DutchFTCCore; + +public class Drivetraintypes { + public enum Drivetrains{ + TANKDRIVE, + FOURWHEELTANKDRIVE, + FIVEWHEELHDRIVE, + THREEWHEELHDRIVE, + MECHANUMDRIVE, + KIWIDRIVE + } + + public void name(){ + Drivetrains Train; + + Train = Robotconfig.DriveTrain; + + switch (Train){ + case MECHANUMDRIVE: + break; + + case TANKDRIVE: + break; + + case KIWIDRIVE: + break; + + case FIVEWHEELHDRIVE: + break; + + case THREEWHEELHDRIVE: + break; + + case FOURWHEELTANKDRIVE: + break; + + default: + System.out.println("PLEASE SELECT A DRIVETRAIN"); + break; + } + + + + + + } +} 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 index 466b6e9e44a..cd9e3b49209 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java @@ -20,11 +20,7 @@ public class Robotconfig { public static String Servo5 = "Servo5"; //drivetrains - public static boolean TankDrive2Motors = false; - public static boolean TankDrive1Motor = true; - public static boolean MechanumDrive = false; - public static boolean H_Drive = false; - public static boolean KiwiDrive = false; + public static Drivetraintypes.Drivetrains DriveTrain = Drivetraintypes.Drivetrains.MECHANUMDRIVE; //logging public static String teamName = "FTCunits"; From 37c14b6195f322ea24b6e5865e330efc945c9336 Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Fri, 14 Feb 2020 12:34:14 +0100 Subject: [PATCH 19/25] added an drivetrain init function somehow have to fixed static problem --- .../DutchFTCCore/Drivetraintypes.java | 33 ++++++++++++++++--- .../ftc/teamcode/DutchFTCCore/Robot.java | 6 ++-- .../teamcode/DutchFTCCore/Robotconfig.java | 1 + 3 files changed, 32 insertions(+), 8 deletions(-) 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 index 3bcdf2b9d6d..4fc09d79b7a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java @@ -1,6 +1,9 @@ package org.firstinspires.ftc.teamcode.DutchFTCCore; public class Drivetraintypes { + + public Robot bot; + public enum Drivetrains{ TANKDRIVE, FOURWHEELTANKDRIVE, @@ -10,28 +13,50 @@ public enum Drivetrains{ KIWIDRIVE } - public void name(){ + public void initDrivetrain(){ Drivetrains Train; 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: - break; - - case FOURWHEELTANKDRIVE: + 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: 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 index fd0ac9cfbd7..80b37120f13 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -24,6 +24,7 @@ public class Robot { public DcMotor MotorFrontLeft; public DcMotor MotorFrontRight; public DcMotor MotorBackRight; + public DcMotor MotorMiddle; public BNO055IMU imu; //list of subsystems @@ -33,10 +34,7 @@ public class Robot { public Robot (OpMode _opmode) { instance = this; opMode = _opmode; - MotorBackLeft = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackLeft); - MotorFrontLeft = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontLeft); - MotorFrontRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorFrontRight); - MotorBackRight = opMode.hardwareMap.dcMotor.get(Robotconfig.MotorBackRight); + Drivetraintypes.initDrivetrain(); imu = opMode.hardwareMap.get(BNO055IMU.class,"imu"); subSystems = new ArrayList<>(); } 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 index cd9e3b49209..f48219c96ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robotconfig.java @@ -7,6 +7,7 @@ public class Robotconfig { 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; From d26e8cde8e1436a391009d018d8678c483e5d349 Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Fri, 14 Feb 2020 12:41:26 +0100 Subject: [PATCH 20/25] added all drivetrain movement --- .../SubSystems/MovementSubSystem.java | 26 ++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) 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 index c2cd77991d2..552e472c584 100644 --- 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 @@ -32,7 +32,17 @@ public void Update() { } 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(){ @@ -60,9 +70,18 @@ public void DriveChecksTankDrive(){ } public void DriveChecksHDrive5Motors(){ - double left; - double right; - double middle; + + //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); } @@ -76,6 +95,7 @@ public void DriveChecksHDrive3Motors(){ //setting the motor speeds bot.MotorBackLeft.setPower(left); bot.MotorBackRight.setPower(right); + bot.MotorMiddle.setPower(middle); } From 146cd6ab3aff26937179d68003070f169bf6ce0d Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Fri, 14 Feb 2020 13:00:51 +0100 Subject: [PATCH 21/25] added more mafs --- .../teamcode/DutchFTCCore/Math/Mathfunc.java | 10 ++++ .../teamcode/DutchFTCCore/Math/Vector2.java | 51 +++++++++++++++++++ 2 files changed, 61 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Math/Vector2.java 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 index 07b9143dbf6..8ddb55c83e7 100644 --- 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 @@ -43,6 +43,16 @@ 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..a8680c5297c --- /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())); + } + +} From d829e830aae284e28c7aaf458a2e6f310ec0f087 Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Fri, 14 Feb 2020 13:02:35 +0100 Subject: [PATCH 22/25] fixed stuff --- .../firstinspires/ftc/teamcode/DutchFTCCore/Math/Vector2.java | 2 +- .../org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) 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 index a8680c5297c..c0a0cafe5b2 100644 --- 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 @@ -47,5 +47,5 @@ public double Slope (){ public double Angle(){ return Math.toDegrees(Math.atan(Slope())); } - + } 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 index 80b37120f13..dc14cb79732 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -27,6 +27,8 @@ public class Robot { public DcMotor MotorMiddle; public BNO055IMU imu; + public Drivetraintypes drivetrains; + //list of subsystems public List subSystems; @@ -34,7 +36,7 @@ public class Robot { public Robot (OpMode _opmode) { instance = this; opMode = _opmode; - Drivetraintypes.initDrivetrain(); + drivetrains.initDrivetrain(); imu = opMode.hardwareMap.get(BNO055IMU.class,"imu"); subSystems = new ArrayList<>(); } From ec6b5c2745fccdd554b8857e138ead6355795024 Mon Sep 17 00:00:00 2001 From: JanTheBeast <44895646+JanTheBeast@users.noreply.github.com> Date: Fri, 14 Feb 2020 13:24:34 +0100 Subject: [PATCH 23/25] added movement function in update and in drivetrain types --- .../DutchFTCCore/Drivetraintypes.java | 35 +++++++++++++++++++ .../SubSystems/MovementSubSystem.java | 3 ++ 2 files changed, 38 insertions(+) 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 index 4fc09d79b7a..5d4bb272930 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java @@ -1,8 +1,11 @@ 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, @@ -64,9 +67,41 @@ public void initDrivetrain(){ break; } + } + + public void DriveChecks(){ + Drivetrains Train; + + Train = Robotconfig.DriveTrain; + + switch (Train){ + case MECHANUMDRIVE: + movSys.DriveChecksMechanum(); + break; + + case TANKDRIVE: + movSys.DriveChecksTankDrive(); + break; + + case FOURWHEELTANKDRIVE: + movSys.DriveChecks4WheelTankDrive(); + break; + case KIWIDRIVE: + movSys.DriveChecksKiwiDrive(); + break; + case FIVEWHEELHDRIVE: + movSys.DriveChecksHDrive5Motors(); + break; + case THREEWHEELHDRIVE: + movSys.DriveChecksHDrive3Motors(); + break; + default: + System.out.println("PLEASE SELECT A DRIVETRAIN"); + break; + } } } 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 index 552e472c584..65c986e1665 100644 --- 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 @@ -1,11 +1,13 @@ 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; */ @@ -29,6 +31,7 @@ public void Start() { @Override public void Update() { super.Update(); + traintypes.DriveChecks(); } public void DriveChecksKiwiDrive(){ From b25e0af7625fae2facd06070c5957bfcc6bc13af Mon Sep 17 00:00:00 2001 From: Guus Dohmen Date: Sat, 15 Feb 2020 15:33:00 +0100 Subject: [PATCH 24/25] See the code review --- .../DutchFTCCore/Drivetraintypes.java | 19 ++++++++++--------- .../ftc/teamcode/DutchFTCCore/Robot.java | 3 ++- .../SubSystems/GuidanceSubSystem.java | 2 +- .../DutchFTCCore/SubSystems/IMUSubSystem.java | 11 ++++++++--- .../SubSystems/MovementSubSystem.java | 1 + .../ftc/teamcode/TestOpMode.java | 10 ++++++++-- 6 files changed, 30 insertions(+), 16 deletions(-) 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 index 5d4bb272930..709cf3cc7da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Drivetraintypes.java @@ -16,11 +16,12 @@ public enum Drivetrains{ KIWIDRIVE } - public void initDrivetrain(){ + 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); @@ -76,31 +77,31 @@ public void DriveChecks(){ switch (Train){ case MECHANUMDRIVE: - movSys.DriveChecksMechanum(); + movSys.instance.DriveChecksMechanum(); break; case TANKDRIVE: - movSys.DriveChecksTankDrive(); + movSys.instance.DriveChecksTankDrive(); break; case FOURWHEELTANKDRIVE: - movSys.DriveChecks4WheelTankDrive(); + movSys.instance.DriveChecks4WheelTankDrive(); break; case KIWIDRIVE: - movSys.DriveChecksKiwiDrive(); + movSys.instance.DriveChecksKiwiDrive(); break; case FIVEWHEELHDRIVE: - movSys.DriveChecksHDrive5Motors(); + movSys.instance.DriveChecksHDrive5Motors(); break; case THREEWHEELHDRIVE: - movSys.DriveChecksHDrive3Motors(); + movSys.instance.DriveChecksHDrive3Motors(); break; default: - System.out.println("PLEASE SELECT A DRIVETRAIN"); + //System.out.println("PLEASE SELECT A DRIVETRAIN"); break; } } 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 index dc14cb79732..6c0a5f23344 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/Robot.java @@ -36,7 +36,8 @@ public class Robot { public Robot (OpMode _opmode) { instance = this; opMode = _opmode; - drivetrains.initDrivetrain(); + drivetrains = new Drivetraintypes(); + drivetrains.initDrivetrain(this); imu = opMode.hardwareMap.get(BNO055IMU.class,"imu"); subSystems = new ArrayList<>(); } 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 index 7ddba7cca59..279dbb4095e 100644 --- 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 @@ -46,6 +46,6 @@ void angleControlPID () { } - //TODO: Add position control + //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 index 971fa1a1088..1af536b207a 100644 --- 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 @@ -3,6 +3,9 @@ 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; @@ -43,9 +46,11 @@ public void Calibrate () { @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 = Robot.instance.imu.read(BNO055IMU.Register.EUL_H_LSB, 2); - currHeading = ByteBuffer.wrap(a).getDouble(); - + //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/MovementSubSystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DutchFTCCore/SubSystems/MovementSubSystem.java index 65c986e1665..12c2e4da84e 100644 --- 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 @@ -26,6 +26,7 @@ public void Start() { super.Start(); bot = Robot.instance; instance = this; + traintypes = bot.drivetrains; } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java index 878c103c459..203df0aa945 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java @@ -1,10 +1,14 @@ 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 { Robot bot; @@ -14,14 +18,16 @@ public void init() { 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; - + //GuidanceSubSystem.targetAngle = 90; + MovementSubSystem.yMov = 1; } From 1faf339da1e22a26aac005361d23eb2467183b30 Mon Sep 17 00:00:00 2001 From: Guus Dohmen Date: Sun, 29 Mar 2020 13:48:08 +0200 Subject: [PATCH 25/25] Update TestOpMode.java --- .../src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java index 203df0aa945..4b9bea7cb13 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestOpMode.java @@ -10,6 +10,7 @@ @TeleOp( name = "TestOpmode") public class TestOpMode extends OpMode { + float eenGetal; Robot bot; @Override