Tonight, I worked on two things: tensor-flow and autonomous mode. On both of these, I just went through the preliminary tasks that each requires, and have valuable information that will help me in the future, despite it looking like I got very little done. I worked on both because I was missing a part I thought we had that would have been used for a webcam, spending that time on an autonomous mode flowchart.

For tensor-flow, I started it last week and asked for the Vu-marks and a powered USB hub to begin testing. Since I obtained only the Vu-marks for this meeting, I worked on those. I grabbed four of the sample op-modes that were provided in the SDK: ConceptTensorFlowObjectDetection, ConceptTensorFlowObjectDetectionWebcam, ConceptVuforiaSkyStoneNavigation, and ConceptVuforiaSkyStoneNavigationWebcam. I copied them over, placed in our vuforia key, and tried them out. They worked fairly well and all did exactly what I expected, even when using a webcam with the unpowered USB hub. Overall, I would recommend using these as a base for any vision code, as that is what we have done and will do as this season progresses.

Once I finished that, I decided to create a basic autonomous mode flow chart. While looking up the field for reference, I stumbled upon this image (left or top) from Reddit, which I used to help me in developing my flow-chart (thank you u/FTC_16471):

Reddit Image - Safe Autonomous Paths for SkyStone20191008 200713

Using this image, I developed my own flowchart which is likely not the final version but will be used for a while whilst developing the autonomous op-modes (right or bottom). Sorry it is sideways.

Overall, I feel like I am in a good place for the year and am looking forward to what we will do with these two concepts throughout the rest of the season.

Tonight, very little was changed in the robot hardware and teleop modes, as the team's main goal for tonight was to have a drive-able base (which we succeeded at). These changes amounted to preemptively adding in the motors that will be used for the scissor lift as well as cleaning up any comments or code left over from importing some methods from a previous year. Despite the lack of changes, I highly recommend that anyone using any part of this code for any reason use the versions below, as they are heavily commented for easy comprehension and reading. Speaking of below, here they are with the hardware first and teleop last:

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.util.ElapsedTime;

/*
* NRG6762
* Northwestern Regional 7 Gearheads
* 2019-2020 Season
* Skystone
* Hardware Class
* Written by Aiden Maraia
* Version: 9/27/2019
* Feel free to make any changes and use at your disposal.
*/
public class BuilderHardware {

//System Activators
public Boolean driveMotorsTrue = true;
public Boolean scissorDriveTrue = false;

//Declare Drive Motors
public DcMotor leftFront = null;
public DcMotor rightFront = null;
public DcMotor leftBack = null;
public DcMotor rightBack = null;

//Declare Scissor Motors
public DcMotor scissorA = null;
public DcMotor scissorB = null;

//Local OpMode Members
HardwareMap hwMap = null;
private ElapsedTime period = new ElapsedTime();

//Constructor
public BuilderHardware(){
}

//Initialize standard Hardware interfaces
public void init(HardwareMap ahwMap){

//Save reference to Hardware map
hwMap = ahwMap;

//Drive Motor Initialization
if(driveMotorsTrue){

//Get Drive Motors
leftFront = ahwMap.dcMotor.get("left_front");
rightFront = ahwMap.dcMotor.get("right_front");
leftBack = ahwMap.dcMotor.get("left_back");
rightBack = ahwMap.dcMotor.get("right_back");

//Set Drive Motor Modes
leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

//Set Drive Motor Modes
leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

//Set Drive Motor Directions
leftFront.setDirection(DcMotorSimple.Direction.FORWARD);
rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftBack.setDirection(DcMotorSimple.Direction.FORWARD);
rightBack.setDirection(DcMotorSimple.Direction.REVERSE);

//Set Drive Motor Powers
leftFront.setPower(0.0);
rightFront.setPower(0.0);
leftBack.setPower(0.0);
rightBack.setPower(0.0);

}

if(scissorDriveTrue){

scissorA = ahwMap.dcMotor.get("scissor_a");
scissorB = ahwMap.dcMotor.get("scissor_b");

scissorA.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
scissorA.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

scissorA.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
scissorA.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

scissorA.setDirection(DcMotorSimple.Direction.FORWARD);
scissorB.setDirection(DcMotorSimple.Direction.REVERSE);

scissorA.setPower(0.0);
scissorB.setPower(0.0);

}
}
}

-------------------------------------------------------------------------------
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;

/*
* NRG6762
* Northwestern Regional 7 Gearheads
* 2019-2020 Season
* Skystone
* Hardware Class
* Written by Aiden Maraia
* Version: 9/27/2019
* Feel free to make any changes and use at your disposal.
*/
@TeleOp(name="Builder Tele-Op", group="Competition")
//@Disabled
public class BuilderTeleOp extends LinearOpMode {

//Declare OpMode members.
private ElapsedTime runtime = new ElapsedTime();

//Set the Hardware Map as the One Specified
private BuilderHardware robot = new BuilderHardware();

//Declare a joystick deadzone
private float deadzone = 0.05f;

//Declare the use of mecanum telemetry
private Boolean mecanumTelemetry = true;

/* Code to be run while the robot is running (from pressing play to stop) */
@Override
public void runOpMode() {

//Initialize the Hardware Map
robot.init(hardwareMap);

//Signify the Hardware Map has been initialized
telemetry.addData("Status", "Initialized");
telemetry.addData("Use Drive", robot.driveMotorsTrue);
telemetry.addData("Use Scissor", robot.scissorDriveTrue);

//Update the telemetry
telemetry.update();

//Wait for the Pressing of the Start Button
waitForStart();

//Reset the game clock to zero in Start()
runtime.reset();

//Run until the end of the match (driver presses STOP)
while (opModeIsActive()) {

//Create a deadzone for the main drive sticks
gamepad1.setJoystickDeadzone(deadzone);

//Turn joystick input into drive motor movement
if(robot.driveMotorsTrue) {
robot.leftFront.setPower(mecanum(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x)[0]);
robot.rightFront.setPower(mecanum(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x)[1]);
robot.leftBack.setPower(mecanum(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x)[2]);
robot.rightBack.setPower(mecanum(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x)[3]);
}

//Update the Telemetry
telemetry.update();

}

}

/* Mecanum Class used to find the power levels for the drive motors */
private double[] mecanum(double y1, double x1, double x2) {

//Initialize the variable that will be returned
double[] finalReturns = new double []{0.0, 0.0, 0.0, 0.0};

//Change joystick input into speed, angle, and spin
double polarSpeed = Math.sqrt((y1 * y1) + (x1 * x1));
double polarAngle = Math.atan2(x1, -y1);
double polarSpeen = -x2 / 3.33;

//Change speed, angle, and spin into power levels for the four drive motors
double leftFrontValue = polarSpeed * Math.sin(polarAngle + (Math.PI/4)) + polarSpeen;
double rightFrontValue = polarSpeed * Math.cos(polarAngle + (Math.PI/4)) - polarSpeen;
double leftBackValue = polarSpeed * Math.cos(polarAngle + (Math.PI/4)) + polarSpeen;
double rightBackValue = polarSpeed * Math.sin(polarAngle + (Math.PI/4)) - polarSpeen;

//Set maxValue to the absolute value of first power level
double maxValue = Math.abs(leftFrontValue);

//If the absolute value of the second power level is less than the maxValue, make it the new maxValue
if(Math.abs(rightFrontValue) > maxValue){maxValue = Math.abs(rightFrontValue);}

//If the absolute value of the third power level is less than the maxValue, make it the new maxValue
if(Math.abs(leftBackValue) > maxValue){maxValue = Math.abs(leftBackValue);}

//If the absolute value of the fourth power level is less than the maxValue, make it the new maxValue
if(Math.abs(rightBackValue) > maxValue){maxValue = Math.abs(rightBackValue);}

//Check if need to scale -- if not set maxValue to 1 to nullify scaling
if (maxValue < 1){maxValue = 1;}

//Send the scaled power levels to the returned variable
finalReturns[0] = leftFrontValue / maxValue;
finalReturns[1] = rightFrontValue / maxValue;
finalReturns[2] = leftBackValue / maxValue;
finalReturns[3] = rightBackValue / maxValue;

/* Mecanum Telemetry for troubleshooting */
if(mecanumTelemetry) {

//Show the joystick values
telemetry.addData("Y1", y1);
telemetry.addData("X1", x1);
telemetry.addData("X2", x2);

//Show the calculated vector values
telemetry.addData("Polar Speed", polarSpeed);
telemetry.addData("Polar Angle", polarAngle);
telemetry.addData("Polar Speen", polarSpeen);

//Show the calculated, unscaled values
telemetry.addData("Left Front Calculated Value", leftFrontValue);
telemetry.addData("Right Front Calculated Value", rightFrontValue);
telemetry.addData("Left Back Calculated Value", leftBackValue);
telemetry.addData("Right Back Calculated Value", rightBackValue);

//Show the scaling values
telemetry.addData("Scaling Value", maxValue);

//Show the calculated and scaled values
telemetry.addData("Left Front Actual Value", finalReturns[0]);
telemetry.addData("Right Front Actual Value", finalReturns[1]);
telemetry.addData("Left Back Actual Value", finalReturns[2]);
telemetry.addData("Right Back Actual Value", finalReturns[3]);

//Update telemetry
telemetry.update();

}

//Return the variable
return finalReturns;

}

}


The main thing I have been working on these past few meetings is autonomous mode. It has been based on the 

IMG 20170109 202048

This is the latest autonomous code which combines changes of default values as well as slight changes in flow.  One major change not shown in the original diagram was an additional crabbing step that was added after the white beacon line was located on the floor.  This straightens the robot out better to the absolute heading and it also makes better use of the optical distance sensor as well as the beacon limit switch.

package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.adafruit.BNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.Range;

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.external.navigation.Orientation;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;

import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Locale;

@Autonomous(name="Benji: Autonomous", group="Autonomous")
public class BenjiAutonomous extends LinearOpMode
{

   
//Declare OpMode members. Use the class created to define the robot's hardware
   
private HardwareRobot robot = new HardwareRobot();

   
private double startTime;
   
private double newStartTime;
   
private double  loopTime;
   
double ballLaunchTriggerCycles = 0;
   
int ballLaunchedNumber = 0;

   
//Set Run-Time Variables
   
private int shootPosition = 1280;                       //In Encoder Ticks
   
private double accelTimeMinPowerWheels = 0.500;         //In Seconds
   
private double decelMinTicks = 1200;                    //In Encoder Ticks
   
private double decelMinCrawlPwr = 0.05;                 //In Power (Using Encoders)
   
private double wallLightODS = 0.03;                     //In Detected and Scaled Light
   
private double linActuOut = 1.0;                        //In Sensor Position
   
private double linActuIn = 0.0;                         //In Sensor Position
   
public boolean IMUactive = true;                        //Changes to not Use IMU
   
private boolean redAliance = true;                      //Defaults to Red/Blue Alliance
   
private boolean useAllianceSwitch = true;               //Override Alliance Switch with above
   
String currMainTask = "moveToShoot";                    //Set Initial Task (Testing)
   
public double whiteLineAlphaThreshold = 125;             //Set threshold value for alpha
   
public double maxWhiteLineDistance = 800;               //Max encoder ticks looking for wh line

    //Activate different Autonomous first time in booleans
   
private boolean firstLoopMoveToShoot = true;
   
private boolean firstBallShooterMode = true;
   
private boolean firstRotationMode    = true;
   
private boolean firstBeaconPress = true;
   
private boolean firstAlignToBeacon = true;

   
//Ball Lift Positions
   
private double ballLiftUp   = 0.0;
   
private double ballLiftDown = 0.5;
   
private double ballWallUp   = 0.4;
   
private double ballWallDown = 0.8;


   
//Find the angles of the IMU
   
private Orientation angles;
   
private double       startHeading;
   
private double       lastHeading;

   
//Various Global Variables Needed By Routines
    
private double rotForwardCrabDirection;
   
private boolean blnForwardRotationSpinAround = false;
   
private double initialPOS = 0.0;

   
@Override public void runOpMode() {
        initialize();
        waitForStart();
       
startTime = getRuntime();
       
newStartTime = getRuntime();
       
while (opModeIsActive()) {
            newLoop();
        }
    }

   
public void initialize(){

       
//Initialize the hardware variables. The init() method of the hardware class does all the work here
       
telemetry.addData("Init", "Initializing Hardware Map");
       
telemetry.update();
       
robot.init(hardwareMap);
       
telemetry.addData("Init", "Initialized Hardware Map");
       
telemetry.update();

       
telemetry.addData("Init", "Searching For Existing File");
       
telemetry.update();

       
telemetry.addData("Init", "Reading Heading From Compass");
       
telemetry.update();

       
try
       
{
           
//robot.IMU.startAccelerationIntegration(new Position(), new Velocity(), 1000);
           
angles = robot.IMU.getAngularOrientation().toAxesReference(AxesReference.INTRINSIC).toAxesOrder(AxesOrder.ZXY);
           
startHeading = angles.firstAngle;
        }
       
catch (Exception e)
        {
           
telemetry.addData("Init", "IMU blew up, disabled");
           
telemetry.update();
           
startHeading = 0.0;
        }

        File headingText =
new File("/sdcard/FIRST/heading.txt");

       
//Create the file
       
try {
           
if (headingText.createNewFile()) {
               
telemetry.addLine("File was created!");
               
telemetry.update();
            }
else {
               
telemetry.addLine("File already exists.");
               
telemetry.update();
            }
            FileWriter writer =
new FileWriter(headingText);
            writer.write(Double.toString(
startHeading));
            writer.close();
           
telemetry.addLine("File was written.");
           
telemetry.update();

        }
catch (IOException e){
           
telemetry.addLine("File could not be created/written.");
           
telemetry.update();
        }


       
telemetry.addData("Heading: ", startHeading);
       
telemetry.update();
    }

   
public void newLoop(){

       
loopTime = getRuntime() - newStartTime;

       
switch (currMainTask) {

           
case "moveToShoot":
               
/* Move Robot to Shooting Position
                    Task:
                    Drives forward to the desired position using the accel and decel methods.
                 */
               
if (firstLoopMoveToShoot) {
                   
newStartTime = getRuntime();
                   
firstLoopMoveToShoot = false;
                }
else {
                   
telemetry.addData("POS", robot.DrivePos1.getCurrentPosition());
                   
telemetry.update();
                   
//Move to next task if shoot position achieved
                   
if (robot.DrivePos1.getCurrentPosition() >= shootPosition) {
                        drive(
0.0);
                       
currMainTask = "shootParticles";
                    }
                   
else
                       
//Set Motor Drive with Accel and Deccel Limiters in place to prevent jerking of bot
                       
drive(decelerationLimiter(robot.DrivePos1.getCurrentPosition(),shootPosition,accelerationLimiter(loopTime,1)));
                }
               
break;

           
case "shootParticles":
               
/* SHOOT PARTICLES:
                    Assumption:
                    Single Particle Loaded in Tube
                    Second Particle Waiting in Hopper
                    Task:
                    Shoot both balls as quickly and accurately as possible.
                 */

                   
if(firstBallShooterMode){
                       
//Reset loop timer for timing of sequence
                       
newStartTime = getRuntime();

                       
// Turn on shooter
                       
launch(1.0);

                       
//Process rest of loop going forward
                       
firstBallShooterMode = false;
                    }
                   
else {
                       
//Timed Sequence To Lunch Particles

                        //Do not start until 500 msec to give shooter time to get to speed.
                       
if (loopTime >= .65 && loopTime < 1.25) {
                           
robot.BallLift.setPosition(ballLiftDown);
                        }
                       
if (loopTime >= 1.25 && loopTime < 1.5) {
                           
robot.BallWall.setPosition(ballWallUp);
                        }
                       
if (loopTime >= 1.5 && loopTime < 2.00) {
                           
robot.BallLift.setPosition(ballLiftUp);
                        }
                        
if (loopTime >= 2.0 && loopTime < 2.5) {
                           
robot.BallWall.setPosition(ballWallDown);
                        }
                       
if (loopTime >= 2.50 && loopTime < 3.0) {
                           
robot.BallLift.setPosition(ballLiftDown);
                        }
                       
if (loopTime >= 3.00) {
                           
robot.BallWall.setPosition(ballWallUp);
                            launch(
0.0);
                           
currMainTask = "rotateForward";
                        }
                    }
               
break;

           
case "rotateForward":

               
/* Rotate Forward
                    Assumption:
                    This will be called once robot is in launch position and that alliance switch
                    or alliance override is set. IMU and Optical Distance Sensor required although
                    emergency mode for both is in place in case of error or sensor failure. The
                    emergency mode will use encoder and timing and a limit switch to determine pos.
                    Task:
                    Crabs left/right and uses IMU to maintain heading and
                    optical distance sensor to judge when wall is near.
                 */

               
if(firstRotationMode){
                   
//Reset Loop Timer
                   
newStartTime = getRuntime();

                   
// Set Values Based on Alliance Switch or Override
                   
if (redAliance){
                       
rotForwardCrabDirection = 62;
                    }
                   
else
                   
{
                       
rotForwardCrabDirection = 295;
                       
blnForwardRotationSpinAround = true;
                    }

                   
//Process remainder of code for additional loops
                   
firstRotationMode = false;
                }
               
else {

                   
if (IMUactive == true) {
                       
try {

                           
if (robot.IMU.getSystemStatus() == BNO055IMU.SystemStatus.RUNNING_FUSION) {
                               
angles = robot.IMU.getAngularOrientation().toAxesReference(AxesReference.INTRINSIC).toAxesOrder(AxesOrder.ZXY);
                                
lastHeading = angles.firstAngle;
                            }
                        }
catch (Exception e) {
                           
telemetry.addData("IMU Status", robot.IMU.getSystemStatus());
                           
telemetry.addData("IMU Error", robot.IMU.getSystemError());
                           
telemetry.addData("Error", e.toString());

                        }
                       
telemetry.addData("Headings", startHeading + " " + lastHeading);
                       
telemetry.addData("IMU Status", robot.IMU.getSystemStatus());

                    }

                   
if (blnForwardRotationSpinAround) {
                        mecanum(
0, 0, 0.3);
                       
if ((loopTime >= 6) || (IMUactive == true && lastHeading >= 180 ))  {
                           
//Reset Loop Timer
                           
newStartTime = getRuntime();
                           
blnForwardRotationSpinAround = false;
                        }
                    }
else {
                       
if ((loopTime >= 4.25)  || (robot.ODS.getLightDetected() >= wallLightODS)) {
                            drive(
0.0);
                           
currMainTask = "alignTo1stBeacon";
                           
telemetry.addData("ODS: ", robot.ODS.getLightDetected());
                        }
else {
                           
//Set Drive Speed Based on Alliance Switch/Override and IMU heading
                           
mecanum( 0.75, rotForwardCrabDirection, (lastHeading)*.05);
                           
telemetry.addData("ODS: ", robot.ODS.getLightDetected());

                        }
                    }
                }
               
telemetry.update();

               
break;

           
case "alignTo1stBeacon":
               
//Move Forward Until White Line Located
               
if (firstAlignToBeacon) {
                   
//Reset Loop Timer
                   
newStartTime = getRuntime();

                   
//Turn On RGB LED
                    //robot.RGBBOTTOMCENTER.enableLed(true);
                   
robot.cdim.setDigitalChannelState(robot.RGB_LED_BOTTOMSIDE, true);

                   
firstAlignToBeacon = false;
                }
               
else
               
{
                    
//Determine encoder ticks traveled
                   
double deltaPOSDiff = initialPOS - robot.DrivePos1.getCurrentPosition();

                   
if (IMUactive == true) {
                       
try {

                           
if (robot.IMU.getSystemStatus() == BNO055IMU.SystemStatus.RUNNING_FUSION) {
                               
angles = robot.IMU.getAngularOrientation().toAxesReference(AxesReference.INTRINSIC).toAxesOrder(AxesOrder.ZXY);
                               
lastHeading = angles.firstAngle;
                            }
                        }
catch (Exception e) {
                           
telemetry.addData("IMU Status", robot.IMU.getSystemStatus());
                           
telemetry.addData("IMU Error", robot.IMU.getSystemError());
                           
telemetry.addData("Error", e.toString());

                        }
                       
telemetry.addData("Headings", startHeading + " " + lastHeading);
                       
telemetry.addData("IMU Status", robot.IMU.getSystemStatus());

                    }

                   
//Check for encoder ticks (as well as time limit)
                   
if (loopTime <= 4 && deltaPOSDiff <= maxWhiteLineDistance){
                       
telemetry.addData("POS: ", robot.DrivePos1.getCurrentPosition());
                       
telemetry.addData("AlphaValue: ", robot.RGBBOTTOMSIDE.alpha());

                        mecanum(
0.20, 0, (lastHeading)*.05);

                       
if (robot.RGBBOTTOMSIDE.alpha() >= whiteLineAlphaThreshold) {
                           
//Stop robot!
                           
drive(0);

                           
//Turn Off LED to conserve battery
                           
robot.cdim.setDigitalChannelState(robot.RGB_LED_BOTTOMSIDE, true);

                           
//We are (hopefully) aligned -- go to beacon press task
                           
firstBeaconPress = true;
                           
firstAlignToBeacon = true; //RESET FOR SECOND BEACON
                           
currMainTask = "beaconpress";
                        }

                    } 
else {


                    }

                }
               
telemetry.update();

               
break;

           
case "alignTo2ndBeacon":

               
//Move Forward Until White Line Located
               
if (firstAlignToBeacon) {
                   
//Reset Loop Timer
                   
newStartTime = getRuntime();

                   
//Turn On RGB LED
                   
robot.RGBBOTTOMCENTER.enableLed(true);
                   
robot.cdim.setDigitalChannelState(robot.RGB_LED_BOTTOMCENTER, true);

                   
firstBeaconPress = false;
                }
               
else
               
{
                   
//Determine encoder ticks traveled
                   
double deltaPOSDiff = initialPOS - robot.DrivePos1.getCurrentPosition();

                   
//Check for encoder ticks (as well as time limit)
                   
if (loopTime <= 4 && deltaPOSDiff <= maxWhiteLineDistance){
                       
telemetry.addData("POS: ", robot.DrivePos1.getCurrentPosition());
                       
telemetry.addData("AlphaValue: ", robot.RGBBOTTOMCENTER.alpha());

                       
if (robot.RGBBOTTOMCENTER.alpha() >= whiteLineAlphaThreshold) {

                           
//Turn Off LED to conserve battery
                           
robot.cdim.setDigitalChannelState(robot.RGB_LED_BOTTOMCENTER, true);

                           
//We are (hopefully) aligned -- got to beacon press task
                           
firstBeaconPress = false;
                           
currMainTask = "beaconpress";
                        }

                    } 
else {


                    }

                }
                
telemetry.update();
               
break;

           
case "beaconPress":

               
if (firstBeaconPress) {
                   
currMainTask = "alignTo2ndBeacon";
                }
else {
                   
currMainTask = "driveToBall";
                }
               
break;

           
case "drivetoball":

               
break;

        }

    }

   
private void drive(double power){
       
robot.DrivePos1.setPower(power);
       
robot.DrivePos2.setPower(power);
       
robot.DrivePos3.setPower(power);
       
robot.DrivePos4.setPower(power);
    }

   
private void mecanum(double speed, double degrees, double spin){

       
//Change joypad values into useful polar values
       
double angle = (degrees * Math.PI)/ 180;
       
double dchange = -spin / 3.33;

       
double pos1, pos2, pos3, pos4, maxValue;

       
//Define unscaled voltage multipliers
       
pos1 = speed*Math.sin(angle+(Math.PI/4))+dchange;
        pos2 = speed*Math.cos(angle+(Math.
PI/4))-dchange;
        pos3 = speed*Math.cos(angle+(Math.
PI/4))+dchange;
        pos4 = speed*Math.sin(angle+(Math.
PI/4))-dchange;

       
//VOLTAGE MULTIPLIER SCALER

        //Set maxValue to pos1 absolute
       
maxValue = Math.abs(pos1);

       
//If pos2 absolute is greater than maxValue, then make maxValue equal to pos2 absolute
       
if(Math.abs(pos2) > maxValue){maxValue = Math.abs(pos2);}

       
//If pos3 absolute is greater than maxValue, then make maxValue equal to pos3 absolute
       
if(Math.abs(pos3) > maxValue){maxValue = Math.abs(pos3);}

       
//If pos4 absolute is greater than maxValue, then make maxValue equal to pos4 absolute
       
if(Math.abs(pos4) > maxValue){maxValue = Math.abs(pos4);}

       
//Check if need to scale -- if not set to 1 to nullify scale
       
if (maxValue <= 1){ maxValue = 1;}

       
//Power motors with scaled voltage multipliers
       
robot.DrivePos1.setPower(pos1/maxValue);
       
robot.DrivePos2.setPower(pos2/maxValue);
       
robot.DrivePos3.setPower(pos3/maxValue);
       
robot.DrivePos4.setPower(pos4/maxValue);
    }

   
private void launch(double speed){
       
robot.BallLaunch1.setPower(speed);
       
robot.BallLaunch2.setPower(speed);
    }

   
private double accelerationLimiter(double deltaTime, double power){
       
return Range.clip(deltaTime * (1 / (accelTimeMinPowerWheels - ((1 - power) * accelTimeMinPowerWheels))),0,power);
    }

   
private double decelerationLimiter(double currEnc, double endPos, double power){
       
double deltaEncoder = Math.abs(endPos - currEnc);
       
double newPower;
       
if (deltaEncoder <= decelMinTicks - (power * decelMinTicks)){
            newPower = Range.clip(((
decelMinTicks - deltaEncoder)/ decelMinTicks), decelMinCrawlPwr, power);
        }
else{
            newPower = power;
        }
       
return newPower;
    }

   
//Grab a specific angle
   
private String formatAngle(AngleUnit angleUnit, double angle) {
       
return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
    }

   
//Grab the angle needed in radians
   
private String formatRadians(double radians){
       
return String.format(Locale.getDefault(), "%.3f", AngleUnit.RADIANS.normalize(radians));
    }

   
private String formatDegrees(double degrees){
       
return String.format(Locale.getDefault(), "%.3f", AngleUnit.DEGREES.normalize(degrees));
    }

}

 

My first blog of the New Year!!!

These past few meetings, I have been doing many different things, some of which do not involve programming. First of all, I worked on using IMU and RGB sensors in our robot, a team first, and adding and modifying the code for the sensors, using the example provided by FIRST as my base and modifying it to my needs. Also, I have added support for many new servos. Finally, I have worked on using a text file to keep data from autonomous and use it in teleop. Here is all of what I have said above:

On the sensor front, I have done a number of different things. For starters, I worked on creating a "cheater cable" for the IMU sensor. What this was were a series of cables that went from the pin headers that were soldered on the IMU for the pins on the core distribution module. I have also done this for an RGB sensor as well as soldering 90 degree headers to the sensor for easy usage in our 3D-printed housings. Those cables will not be the final product, rather a way to easily fix problems and test the sensors. The real cables are currently being created according to the First RGB guide (link further down). For the IMU, a really great wiring guide for the IMU can be found at https://github.com/jcorcoran/BNO055_FRC#wiring and an official FTC guide for the RGB sensors can be found at http://www.firstinspires.org/sites/default/files/uploads/resource_library/ftc/adafruit-rgb-sensor-buildguide.pdf. The official FTC guide can be used to help wire both types of sensors, as the IMU guide I have placed only has a pin-out table that is relevant to our usage.

On the coding front, I have used all of the example classes as a base for my test modes. I have also modified all of them to see if they can be used well in our teleop mode and, so far, they work well enough for our needs. Here is my example code for the RGB sensor:

package org.firstinspires.ftc.teamcode;

import android.app.Activity;
import android.graphics.Color;
import android.view.View;

import com.qualcomm.ftccommon.DbgLog;
import com.qualcomm.ftcrobotcontroller.R;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DeviceInterfaceModule;
import com.qualcomm.robotcore.hardware.DigitalChannelController;

@Autonomous(name = "RGBTest", group = "Sensor")
//@Disabled // Comment this out to add to the opmode list
public class RGBTest extends LinearOpMode {

ColorSensor sensorRGB;
DeviceInterfaceModule cdim;

// we assume that the LED pin of the RGB sensor is connected to
// digital port 5 (zero indexed).
static final int LED_CHANNEL = 5;

@Override
public void runOpMode() {

// get a reference to the RelativeLayout so we can change the background
// color of the Robot Controller app to match the hue detected by the RGB sensor.
final View relativeLayout = ((Activity) hardwareMap.appContext).findViewById(R.id.RelativeLayout);

// bPrevState and bCurrState represent the previous and current state of the button.
boolean bPrevState = false;
boolean bCurrState = false;

// bLedOn represents the state of the LED.
boolean bLedOn = true;

// get a reference to our DeviceInterfaceModule object.
cdim = hardwareMap.deviceInterfaceModule.get("dim");

// set the digital channel to output mode.
// remember, the Adafruit sensor is actually two devices.
// It's an I2C sensor and it's also an LED that can be turned on or off.
cdim.setDigitalChannelMode(LED_CHANNEL, DigitalChannelController.Mode.OUTPUT);

// get a reference to our ColorSensor object.
sensorRGB = hardwareMap.colorSensor.get("sensor_color");

// turn the LED on in the beginning, just so user will know that the sensor is active.
cdim.setDigitalChannelState(LED_CHANNEL, bLedOn);

// wait for the start button to be pressed.
waitForStart();

// loop and read the RGB data.
// Note we use opModeIsActive() as our loop condition because it is an interruptible method.
while (opModeIsActive()) {

// check the status of the x button on gamepad.
bCurrState = gamepad1.x;

// check for button-press state transitions.
if ((bCurrState == true) && (bCurrState != bPrevState)) {

// button is transitioning to a pressed state. Toggle the LED.
bLedOn = !bLedOn;
cdim.setDigitalChannelState(LED_CHANNEL, bLedOn);
}

// update previous state variable.
bPrevState = bCurrState;

// send the info back to driver station using telemetry function.
telemetry.addData("LED", bLedOn ? "On" : "Off");
telemetry.addData("Hue", sensorRGB.argb());
telemetry.update();
}
}
}

In addition, here is my example code for the IMU, finding the angle heading in radians:

package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.adafruit.BNO055IMU;
import com.qualcomm.hardware.adafruit.JustLoggingAccelerationIntegrator;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.robotcore.external.Func;
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.external.navigation.Orientation;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;

import java.util.Locale;

@Autonomous(name = "Sensor: Adafruit IMU", group = "Sensor")
//@Disabled // Uncomment this to add to the opmode list
public class SensorAdafruitIMURadians extends LinearOpMode
{
//----------------------------------------------------------------------------------------------
// State
//----------------------------------------------------------------------------------------------

// The IMU sensor object
BNO055IMU imu;

// State used for updating telemetry
Orientation angles;

//----------------------------------------------------------------------------------------------
// Main logic
//----------------------------------------------------------------------------------------------

@Override public void runOpMode() {

// Set up the parameters with which we will use our IMU. Note that integration
// algorithm here just reports accelerations to the logcat log; it doesn't actually
// provide positional information.
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.calibrationDataFile = "AdafruitIMUCalibration.json"; // see the calibration sample opmode
parameters.loggingEnabled = true;
parameters.loggingTag = "IMU";
parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();

// Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
// on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
// and named "imu".
imu = hardwareMap.get(BNO055IMU.class, "imu");
imu.initialize(parameters);

// Set up our telemetry dashboard
composeTelemetry();

// Wait until we're told to go
waitForStart();

// Start the logging of measured acceleration
imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);

// Loop and update the dashboard
while (opModeIsActive()) {
telemetry.update();
}
}

//----------------------------------------------------------------------------------------------
// Telemetry Configuration
//----------------------------------------------------------------------------------------------

void composeTelemetry() {

// At the beginning of each telemetry update, grab a bunch of data
// from the IMU that we will then display in separate lines.
telemetry.addAction(new Runnable() { @Override public void run()
{
// Acquiring the angles is relatively expensive; we don't want
// to do that in each of the three items that need that info, as that's
// three times the necessary expense.
angles = imu.getAngularOrientation().toAxesReference(AxesReference.INTRINSIC).toAxesOrder(AxesOrder.ZXY).toAngleUnit(angles.angleUnit);
}
});

telemetry.addLine()
.addData("heading", new Func<String>() {
@Override public String value() {
return formatAngle(angles.angleUnit, angles.firstAngle);
}
});
}

//----------------------------------------------------------------------------------------------
// Formatting
//----------------------------------------------------------------------------------------------

String formatAngle(AngleUnit angleUnit, double angle) {
return formatRadians(AngleUnit.RADIANS.fromUnit(angleUnit, angle));
}

String formatRadians(double radians){
return String.format(Locale.getDefault(), "%.2f", AngleUnit.RADIANS.fromRadians(radians));
}
}

On the servo and file front, the robot has a few new servos and we are keeping the angle heading from autonomous, so here is the updated teleop code:

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.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;

import java.io.File;
import java.io.FileReader;
import java.io.LineNumberReader;
import java.util.Locale;


/*
* Team: FTC NRG6762
* School: Northwest Regional 7
* Author: Aiden Maraia
*/
@TeleOp(name="Benji: TeleOP", group="Test")
//@Disabled
public class BenjiTeleOP extends OpMode{

//Declare OpMode members. Use the class created to define the robot's hardware
private HardwareRobot robot = new HardwareRobot();

//Telemetry boolean (used to turn on any valid telemetry at the right time)
boolean joyPosTele = false;
boolean joyPolarCoordTele = false;
boolean wheelScalersTele = false;
boolean ballLauncherTele = false;

//Find the angles of the IMU
Orientation angles;
double startHeading;
double lastHeading;

//Code to run ONCE when the driver hits INIT
@Override
public void init() {

//Initialize the hardware variables. The init() method of the hardware class does all the work here
robot.init(hardwareMap);

File headingText = new File("/sdcard/FIRST/heading.txt");
if (headingText.exists() && headingText.isFile()){
try{
FileReader fr = new FileReader(headingText);
LineNumberReader ln = new LineNumberReader(fr);
while (ln.getLineNumber() == 0){
String number = ln.readLine();
startHeading = Double.parseDouble(number);
}
headingText.delete();
}catch (Exception e){

}
}else{
startHeading = Double.parseDouble(formatAngle(angles.angleUnit, angles.firstAngle));
}

// Send telemetry message to signify robot waiting
telemetry.addData("Say", "Hello Driver");
}

//Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY
@Override
public void init_loop() {
}

//Code to run ONCE when the driver hits PLAY
@Override
public void start() {
}

//Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
@Override
public void loop() {

//Create and set values to used variables
double speed = 0, angle = 0, dchange = 0;
double retValues [];

//Instance of cartesianToPolar method used for changing the cartesian values into polar values.
retValues = cartesianToPolar(gamepad1.right_stick_y, gamepad1.right_stick_x, gamepad1.left_stick_x);

//Set retValues array to shown values. retValues is used to return the variables used in multiple methods.
speed = retValues[0];
angle = retValues[1];
dchange = retValues[2];

//Instance of polarToWheelSpeed method used for powering wheels
polarToWheelSpeed(speed, angle, dchange);

//Instance of ballShooter method used for angling and using the ballshooter
ballShooter();

//Update the Telemetry
telemetry.update();
}

//Code to run ONCE after the driver hits STOP
@Override
public void stop() {
}

private double[] cartesianToPolar(double y1, double x1, double x2) {

//Reset retValues to 0 for later use
double[] retValues = new double []{0.0,0.0,0.0};
double speed = 0.0, angle=0.0, dchange=0.0;

gamepad1.setJoystickDeadzone((float) 0.05);

//Change joypad values into useful polar values
speed = Math.sqrt((y1 * y1) + (x1 * x1));
angle = Math.atan2(x1, -y1) - (startHeading - lastHeading);
dchange = -x2 / 3.33;

//Orient the robot controls from the driver's point of view
if(Math.abs(angle) >= 2 * Math.PI){
angle = Math.abs(angle) - (2 * Math.PI);
}else{
angle = Math.abs(angle);
}

lastHeading = Double.parseDouble(formatAngle(angles.angleUnit, angles.firstAngle));

//Joypad input Telemetry
if (joyPosTele) {
telemetry.addData("X1: ",x1);
telemetry.addData("Y1: ",y1);
telemetry.addData("X2: ",x2);
}

//Polar values Telemetry
if (joyPolarCoordTele) {
telemetry.addData("Speed: ", speed);
telemetry.addData("Angle: ", angle);
telemetry.addData("Rotational Change", dchange);
}

//Add polar values to retValues array
retValues[0] = speed;
retValues[1] = angle;
retValues[2] = dchange;
return retValues;
}

private void polarToWheelSpeed(double speed, double angle, double dchange){

//Create Variables used only in this method
double pos1, pos2, pos3, pos4, maxValue;

//Define unscaled voltage multipliers
pos1 = speed*Math.sin(angle+(Math.PI/4))+dchange;
pos2 = speed*Math.cos(angle+(Math.PI/4))-dchange;
pos3 = speed*Math.cos(angle+(Math.PI/4))+dchange;
pos4 = speed*Math.sin(angle+(Math.PI/4))-dchange;

//VOLTAGE MULTIPLIER SCALER

//Set maxValue to pos1 absolute
maxValue = Math.abs(pos1);

//If pos2 absolute is greater than maxValue, then make maxValue equal to pos2 absolute
if(Math.abs(pos2) > maxValue){maxValue = Math.abs(pos2);}

//If pos3 absolute is greater than maxValue, then make maxValue equal to pos3 absolute
if(Math.abs(pos3) > maxValue){maxValue = Math.abs(pos3);}

//If pos4 absolute is greater than maxValue, then make maxValue equal to pos4 absolute
if(Math.abs(pos4) > maxValue){maxValue = Math.abs(pos4);}

//Check if need to scale -- if not set to 1 to nullify scale
if (maxValue <= 1){ maxValue = 1;}

//Power motors with scaled voltage multipliers
robot.DrivePos1.setPower(pos1/maxValue);
robot.DrivePos2.setPower(pos2/maxValue);
robot.DrivePos3.setPower(pos3/maxValue);
robot.DrivePos4.setPower(pos4/maxValue);

//Scaled Voltage Multiplier Telemetry
if (wheelScalersTele) {
telemetry.addData("Wheel 1 W/ Scale: ",pos1/maxValue);
telemetry.addData("Wheel 2 W/ Scale: ",pos2/maxValue);
telemetry.addData("Wheel 3 W/ Scale: ",pos3/maxValue);
telemetry.addData("Wheel 4 W/ Scale: ",pos4/maxValue);
}
}

private void ballShooter(){

//Control the ball lift using your right index finger
if(gamepad2.right_trigger == 1){
robot.SweeperLift.setDirection(DcMotorSimple.Direction.REVERSE);
robot.SweeperLift.setPower(1.0);
}else if(gamepad2.right_bumper){
robot.SweeperLift.setDirection(DcMotorSimple.Direction.FORWARD);
robot.SweeperLift.setPower(1.0);
}else{
robot.SweeperLift.setPower(0.0);
}

//Control the ball launcher using your right thumb
if(gamepad2.a){
robot.BallLift1.setPosition(1.0);
robot.BallLift2.setPosition(0.0);
robot.BallLaunch1.setPower(1.0);
robot.BallLaunch2.setPower(1.0);
}
else{
robot.BallLift1.setPosition(0.0);
robot.BallLift2.setPosition(1.0);
robot.BallLaunch1.setPower(0.5);
robot.BallLaunch2.setPower(0.5);
}

//Control the sweeper using your right thumb
if(gamepad2.b){robot.Sweeper.setPower(1.0);}
else{robot.Sweeper.setPower(0.0);}

//Ball Launching Telemetry
if(ballLauncherTele){
if (robot.SweeperLift.getPower() != 0.0) {telemetry.addData("Ball Lift Direction: ", robot.SweeperLift.getDirection());}
if (robot.BallLift1.getPosition() != 0.0) {telemetry.addData("Launcher Lift Position: ", robot.BallLift1.getPosition());}
if (robot.BallLaunch1.getPower() != 0.0) {telemetry.addLine("Ball Launcher Is Running");}
if (robot.Sweeper.getPower() != 0.0) {telemetry.addLine("Sweeper Is Running");}
}
}

private String formatAngle(AngleUnit angleUnit, double angle) {
return formatDegrees(AngleUnit.RADIANS.fromUnit(angleUnit, angle));
}

private String formatDegrees(double radians){
return String.format(Locale.getDefault(), "%.3f", AngleUnit.RADIANS.normalize(radians));
}
}