Automated Drive

TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java

@@ -1,11 +1,14 @@
 package org.timecrafters.testing.states;
 
+import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
 
 public class PrototypeBot1 {
 
@@ -16,6 +19,8 @@ public class PrototypeBot1 {
 
         public CRServo collectorLeft, collectorRight;
 
+         public BNO055IMU imu;
+
 //        public Servo collectorWrist;
 
         public PrototypeBot1(CyberarmEngine engine) {
@@ -25,12 +30,25 @@ public class PrototypeBot1 {
         }
 
         private void setupRobot () {
+            BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+
+            parameters.mode = BNO055IMU.SensorMode.IMU;
+            parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+            parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+            parameters.loggingEnabled = false;
+
+            this.imu = engine.hardwareMap.get(BNO055IMU.class,"imu");
+            imu.initialize(parameters);
+
+            imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
+
+
 
             //motors configuration
             frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left");
             frontRightDrive = engine.hardwareMap.dcMotor.get("Front Right");
-            backRightDrive = engine.hardwareMap.dcMotor.get("Back Left");
+            backRightDrive = engine.hardwareMap.dcMotor.get("Back Right");
-            backLeftDrive = engine.hardwareMap.dcMotor.get("Back Right");
+            backLeftDrive = engine.hardwareMap.dcMotor.get("Back Left");
 
             // servo configuration
 
@@ -46,16 +64,16 @@ public class PrototypeBot1 {
 
             //motors direction and encoders
 
-            frontLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD);
+            frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
             frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
-            frontRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
+            frontRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
             frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
-            backLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD);
+            backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
             backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
-            backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
+            backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
             backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
         }

TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java

@@ -2,14 +2,10 @@ package org.timecrafters.testing.states;
 
 import android.annotation.SuppressLint;
 
-import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.cyberarm.engine.V2.CyberarmState;
 
-import java.lang.annotation.Target;
-import java.util.Objects;
-
 public class PrototypeTeleOPState extends CyberarmState {
 
     private final PrototypeBot1 robot;
@@ -27,7 +23,8 @@ public class PrototypeTeleOPState extends CyberarmState {
     private double speed = 1; // used for the normal speed while driving
     private double slowSpeed = 0.5; // used for slow mode speed while driving
     private int CyclingArmUpAndDown = 0;
-    private int DriveFowardAndBack;
+    private int DriveForwardAndBack;
+    private int RobotPosition, RobotStartingPosition;
 
     public PrototypeTeleOPState(PrototypeBot1 robot) {
         this.robot = robot;
@@ -94,26 +91,25 @@ public class PrototypeTeleOPState extends CyberarmState {
 //            bprev = false;
 //        }
 
-        //Drivetrain Variables
+//        //Drivetrain Variables
-        double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
+//        double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
-        double x = engine.gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
+//        double x = engine.gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
-        double rx = engine.gamepad1.left_stick_x;
+//        double rx = engine.gamepad1.left_stick_x;
-
+//
-        // Denominator is the largest motor power (absolute value) or 1
+//        // Denominator is the largest motor power (absolute value) or 1
-        // This ensures all the powers maintain the same ratio, but only when
+//        // This ensures all the powers maintain the same ratio, but only when
-        // at least one is out of the range [-1, 1]
+//        // at least one is out of the range [-1, 1]
-
+//
-        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+//        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
-        double frontLeftPower = (y + x + rx) / denominator;
+//        double frontLeftPower = (y + x + rx) / denominator;
-        double backLeftPower = (y - x + rx) / denominator;
+//        double backLeftPower = (y - x + rx) / denominator;
-        double frontRightPower = (y - x - rx) / denominator;
+//        double frontRightPower = (y - x - rx) / denominator;
-        double backRightPower = (y + x - rx) / denominator;
+//        double backRightPower = (y + x - rx) / denominator;
-
+//
-        robot.frontLeftDrive.setPower(-frontLeftPower * speed);
+//        robot.frontLeftDrive.setPower(-frontLeftPower * speed);
-        robot.backLeftDrive.setPower(backLeftPower * speed);
+//        robot.backLeftDrive.setPower(backLeftPower * speed);
-        robot.frontRightDrive.setPower(-frontRightPower * speed);
+//        robot.frontRightDrive.setPower(-frontRightPower * speed);
-        robot.backRightDrive.setPower(backRightPower * speed);
+//        robot.backRightDrive.setPower(backRightPower * speed);
-
 
 
         if (engine.gamepad2.y) {
@@ -232,39 +228,61 @@ public class PrototypeTeleOPState extends CyberarmState {
 //            }
 //        }
         // SPENCER____________________________________________________________________
-//        if (engine.gamepad1.start){
+        if (engine.gamepad1.start) {
-//
+            RobotPosition = robot.backRightDrive.getCurrentPosition();
-//                switch (DriveFowardAndBack) {
-//
-//                    // Drive Foward
-//                    case 0:
-//                        if (robot.backLeftDrive.getCurrentPosition() == 0) {
-//                            robot.backLeftDrive.setTargetPosition(4000);
-//                            robot.backRightDrive.setTargetPosition(4000);
-//                            robot.frontLeftDrive.setTargetPosition(4000);
-//                            robot.backRightDrive.setTargetPosition(4000);
-//                            robot.backLeftDrive.setPower(1);
-//                            robot.backRightDrive.setPower(1);
-//                            robot.frontLeftDrive.setPower(1);
-//                            robot.backRightDrive.setPower(1);
-//
-//                        }
-//                        } else {
-//
-//            }
-//                        }
-//                        break;
-//
-//                    // lower arm up
-//                    case 1:
-//                        if (robot.LowRiserLeft.getPosition() < 1) {
-//                            robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
-//                            robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
-//                        } else {
-//                            CyclingArmUpAndDown = CyclingArmUpAndDown +1;
-//                        }
-//                        break;
 
+            switch (DriveForwardAndBack) {
+
+                case 0:
+                    RobotStartingPosition = RobotPosition;
+                  drivePower = 1;
+
+                  DriveForwardAndBack += 1;
+                    break;
+
+                case 1:
+                    if (RobotPosition - RobotStartingPosition < 6250){
+                        robot.backLeftDrive.setPower(drivePower);
+                        robot.backRightDrive.setPower(drivePower);
+                        robot.frontLeftDrive.setPower(drivePower);
+                        robot.frontRightDrive.setPower(drivePower);
+                    } else
+                    {
+                        DriveForwardAndBack += 1;
+                        drivePower = -1;
+                    }
+                    break;
+                case 2:
+                    if (RobotPosition - RobotStartingPosition >= 0) {
+                        robot.backLeftDrive.setPower(drivePower);
+                        robot.backRightDrive.setPower(drivePower);
+                        robot.frontLeftDrive.setPower(drivePower);
+                        robot.frontRightDrive.setPower(drivePower);
+                    } else
+                    {
+                        DriveForwardAndBack += 1;
+                    }
+                    break;
+                case 3:
+                    robot.backLeftDrive.setPower(0);
+                    robot.backRightDrive.setPower(0);
+                    robot.frontLeftDrive.setPower(0);
+                    robot.frontRightDrive.setPower(0);
+                    DriveForwardAndBack = 1;
+                    drivePower = 1;
+                    break;
+
+
+            } // switch ending
+            } // if gamepad 1 start ending
+        else {
+
+            robot.backLeftDrive.setPower(0);
+            robot.backRightDrive.setPower(0);
+            robot.frontLeftDrive.setPower(0);
+            robot.frontRightDrive.setPower(0);
+
+        }
 
      if (engine.gamepad2.start){