1. package com.qualcomm.ftcrobotcontroller.opmodes;
  2.  
  3. import com.qualcomm.robotcore.eventloop.opmode.OpMode;
  4. import com.qualcomm.robotcore.hardware.DcMotor;
  5. import com.qualcomm.robotcore.util.Range;
  6.  
  7. /**
  8.  * Created by Adam Li on 10/16/2015.
  9.  * The class is structured to be easy to follow and is heavily annotated. It's designed to
  10.  * help people with enough programming knowledge easily pick up how to setup a basic mode
  11.  * on the robot.
  12.  *
  13.  * After creating an OpMode (Operational Mode) class, you have to add it to
  14.  * FtcOpModeRegister within the package com.qualcomm.ftcrobotcontroller.opmodes.
  15.  * Basic syntax is: manager.register ("MenuItemName", Classname.class);
  16.  * For this OpMode: manager.register ("Adam Drive Example", AdamBasicDriveExample.class);
  17.  *
  18.  * This class will allow manual control of a simple drive robot with a joystick controlling
  19.  * a side (Tank style), and with the press of a button (left trigger) it will cycle to Arcade
  20.  * Drive (left joystick only), and three other modes that are uncommon and impractical because
  21.  * they are made up (Racing game style and digital joysticks for arcade and tank style). With
  22.  * the press of another button (right trigger), the robot will make a complete circle or square
  23.  * to the left or right, depending on which horizontal direction the left joystick (circle) or
  24.  * right joystick (square) is moved to within a second. If both joysticks are activated at the
  25.  * same moment, the action is canceled and the robot will move back a bit.
  26.  */
  27. public class AdamBasicDriveExample extends OpMode{
  28.  
  29.     // Empty class constructor
  30.     public AdamBasicDriveExample ()
  31.     {
  32.         // Needed to create the class object (Java requires it), but since there isn't
  33.         // anything being passed to it and there's no needed setup on creation (at
  34.         // least in this case), it's empty.
  35.     }
  36.  
  37.  
  38.     // Below I'm declaring variables in the scope of the entire class. The reason I'm not
  39.     // declaring them in the init is because those variables would only be local to the
  40.     // init function.
  41.     // Normally I declare these right in the beginning, before the constructor or anything.
  42.  
  43.     // The SDK includes a few new object types, one is the DcMotor object (which should
  44.     // be fairly straightforward). Below I'm declaring the two drive motors typical of
  45.     // most robots.
  46.     private DcMotor leftMotor;  // Notice how I named it. There are many ways to name variables.
  47.     private DcMotor rightMotor; // The way I name them is a mix of naming schemes, which you'll see sooner or later.
  48.     private int driveMode;      // Refer to top. Drive modes are in the order written.
  49.  
  50.     private double currentVel;  // Current Velocity. Used for calculations with acceleration.
  51.     private double leftVel;     // Velocity of left wheel. Used for calculations with acceleration.
  52.     private double rightVel;    // Velocity of right wheel. Used for calculations with acceleration.
  53.     private double maxVel;      // Max Velocity. Calculated in the program.
  54.  
  55.     private long lastTime;      // The time at the last time check (using System.currentTimeMillis())
  56.     private long changeTime;    // The change in time (in millis) from last time check
  57.  
  58.     private boolean LTpressed;  // Flags to avoid the pressing of a button twice.
  59.     private boolean RTpressed;  // TODO: 10/26/2015 Find a better method, if possible
  60.  
  61.  
  62.     // The below are "flags" that I like to use to make code easier to change in many areas
  63.     // Normally I would set these before I would even declare the variables.
  64.     private static final boolean motorLeftReverse = false;  // All the "gibberish" before the "flag" name
  65.     private static final boolean motorRightReverse = true;  // is to make sure the variable can't change
  66.  
  67.     private static final boolean curvedJoystickMap = true;  // This flag changes the joystick mapping between linear and squared
  68.  
  69.  
  70.     // Variables that affect the behavior of the program (can be static final or not, doesn't matter too much, but it should be done for optimization)
  71.     private static final float wheelDiameter = 4;       // Wheel size in inches (I know it's annoying, but we live in the US)
  72.     private static final float motorPowerRotation = .8f;// This is about how much power the motor needs to make one rotation per second.
  73.         // Differs per robot. I could have implemented the use of a rotary encoder and a calibration
  74.         // function, but I would need to do some experimenting first.
  75.     private static final float buttonFAccel = 1.2f;     // Set in m/s^2. For use with the three digital drives. Forwards Acceleration
  76.     private static final float buttonFDecel = 1.2f;     // Set in m/s^2. For use with the three digital drives. Forwards Deceleration
  77.     private static final float buttonBAccel = 1.2f;     // Set in m/s^2. For use with the three digital drives. Backwards Acceleration
  78.     private static final float buttonBDecel = 1.2f;     // Set in m/s^2. For use with the three digital drives. Backwards Deceleration
  79.     private static final float steeringPower = .8f;     // Multiplied by the current velocity of the robot while in digital drive to adjust steering
  80.         // Notice the f in 1.2f, f is used to show that it's a float, not a double. Java automatically
  81.         // interprets a.b as a double (64-bit floating point number), which is more precise than a
  82.         // float (32-bit floating point number). To denote a float, write add an f.
  83.  
  84.     private static final float trigThreshold = .7f;     // When to say that the analogue triggers are pressed
  85.     private static final float stickThreshold = .3f;    // When to say that a joystick is one way or the other for autoCircle()
  86.  
  87.     //This one might change, so it's not static final.
  88.     private float maxMotorSpeed = 1;    // Set on a floating scale of 0 < x <= 1. If out of range, will default to .8/
  89.  
  90.     private static final float circleRadius = 1;        // Radius of circle and square for automatic circle/square in meters
  91.     private static final float maxCircleSpeed = .8f;    // This*maxMotorSpeed = maximum speed the robot will go when doing the circle
  92.     private static final float robotWidth = 50;         // Distance between the centers of the wheels across the robot.
  93.  
  94.  
  95.     // Have you ever worked with Arduino before?
  96.     // This is designed similarly, there's two functions that are required, init() and loop(),
  97.     // and because they are already setup, they should be overridden with @Override a line
  98.     // before. It isn't necessary because the compiler is capable of figuring out that you
  99.     // want to override it, but it's a good habit to do so and makes code easier to understand.
  100.     //
  101.     // The init function below setups up the robot; it runs once on startup or reset
  102.     // IT IS NOTE DESIGNED TO BE CALLED ON WITHIN ANY OTHER FUNCTION THAT YOU WILL
  103.     // PROBABLY WRITE. I'VE SEEN PEOPLE DO THIS BEFORE, AND IT IS A TERRIBLE TRICK.
  104.     //
  105.     // The loop function does as it says, it loops when the program is running. Also never
  106.     // call this function anywhere within this function. It will results in a stack overflow
  107.     // then a crash, and nobody likes it when programs crash.
  108.     @Override
  109.     public void init(){
  110.         // Assign the motor mapped to "left_drive" and "right_drive" to the appropriate
  111.         // objects.
  112.         leftMotor = hardwareMap.dcMotor.get("left_drive");
  113.         rightMotor = hardwareMap.dcMotor.get("right_drive");
  114.  
  115.         // If statements with brackets or without brackets? I say with. It's easier to edit,
  116.         // even if it's just one line of code, and easier to understand.
  117.         if (motorLeftReverse){                                      // These two if statements
  118.             leftMotor.setDirection(DcMotor.Direction.REVERSE);      // read the flag set above
  119.         }                                                           // and set reverse the motor
  120.         if (motorRightReverse){                                     // direction if needed.
  121.             rightMotor.setDirection(DcMotor.Direction.REVERSE);     // They aren't reversed by
  122.         }                                                           // default.
  123.  
  124.         // Check for out of range variables
  125.         if (!(0 < maxMotorSpeed && maxMotorSpeed <= 1)){            // Just to show negation.
  126.             maxMotorSpeed = .8f;                                    // Default Motor Speed
  127.         }
  128.  
  129.         driveMode = 0;                          // Default Mode: Tank Drive
  130.         currentVel = leftVel = rightVel = 0;    // Initial Velocity (obviously!)
  131.         LTpressed = RTpressed = false;          // Neither of the triggers are started pressed.
  132.         // TODO: 10/22/2015 Explain comment
  133.         maxVel = ((double)maxMotorSpeed/(double)motorPowerRotation)*wheelDiameter*0.0254*Math.PI;
  134.         lastTime = System.currentTimeMillis();
  135.     }
  136.  
  137.     @Override
  138.     public void loop(){
  139.         changeTime = System.currentTimeMillis()-lastTime;
  140.         lastTime += changeTime;
  141.  
  142.         // Read the values from the first gamepad joysticks.
  143.         float leftStickY = -gamepad1.left_stick_y;      // The SDK already maps the gamepads to
  144.         float leftStickX = gamepad1.left_stick_x;       // two variables, gamepad1 and gamepad2.
  145.         float rightStickY = -gamepad1.right_stick_y;    // The reason for the negative sign in
  146.         float rightStickX = gamepad1.right_stick_x;     // front is because the y-values are inverted.
  147.  
  148.         // Read the curvedJoystickMap flag, and if it's true, curve the joystick values.
  149.         if (curvedJoystickMap){
  150.             // The *= operator: a *= b is the same as a = a*b.
  151.             // To get a simple curve, all you need to do is square the joystick value. It won't go
  152.             // above 1, the max, because 1^2 = 1. To make sure negatives stay, the values is
  153.             // multiplied by the absolute value of itself.
  154.             leftStickY *= Math.abs(leftStickY);
  155.             leftStickX *= Math.abs(leftStickX);
  156.             rightStickY *= Math.abs(rightStickY);
  157.             rightStickX *= Math.abs(rightStickX);
  158.         }
  159.  
  160.         // Check the driveMode to see how to drive the robot. Could use if statements, or switch cases.
  161.         switch(driveMode) {
  162.             // Check for Tank Drive
  163.             case 0:
  164.                 mapToMotor(leftStickY, leftMotor);      // Call to the one line function that maps
  165.                 mapToMotor(rightStickY, rightMotor);    // the joystick to the motor.
  166.                 break;              // Break breaks out of the switch.
  167.  
  168.             // Check for Arcade Drive (although my drive is a modified arcade drive to work better...)
  169.             case 1:
  170.                 // I don't feel like explaining the math :(
  171.                 mapToMotor(Math.copySign(Math.abs(leftStickY) leftStickX, leftStickY), leftMotor);
  172.                 mapToMotor(Math.copySign(Math.abs(leftStickY) - leftStickX, leftStickY), rightMotor);
  173.                 // First I take the absolute value of the y-value and I add or subtract the x-value.
  174.                 // Because it removes the sign, the robot can only move forwards, so I use
  175.                 // Math.copySign to copy the sign from the Y stick the the result and pass that to
  176.                 // the mapToMotor(float, DcMotor) function.
  177.                 // That makes the drive make sense, where going to the bottom left makes the robot
  178.                 // move to the left while going backwards, and vice versa. Their version makes the
  179.                 // robot move to the right when the joystick is at the bottom left.
  180.                 break;
  181.  
  182.             // YAY Racing Game Style! (not very good...) // TODO: 10/24/2015 Comments for whole digital section
  183.             case 2:
  184.                 // TODO: 10/26/2015 See if the if statements can be optimized
  185.                 if (gamepad1.a && currentVel buttonFAccel <= maxVel) {
  186.                     currentVel += buttonFAccel * (1000 / changeTime);
  187.                 }
  188.                 if (gamepad1.b && currentVel - buttonBAccel >= -maxVel) {
  189.                     currentVel -= buttonBAccel * (1000 / changeTime);
  190.                 }
  191.                 if (!gamepad1.a && !gamepad1.b && currentVel > 0) {
  192.                     currentVel -= buttonFDecel * (1000 / changeTime);
  193.                     if (currentVel < 0) {
  194.                         currentVel = 0;
  195.                     }
  196.                 }
  197.                 if (!gamepad1.a && !gamepad1.b && currentVel < 0) {
  198.                     currentVel += buttonBDecel * (1000 / changeTime);
  199.                     if (currentVel > 0) {
  200.                         currentVel = 0;
  201.                     }
  202.                 }
  203.                 leftVel = Math.copySign(Math.abs(currentVel) leftStickX * steeringPower, currentVel);
  204.                 rightVel = Math.copySign(Math.abs(currentVel) - leftStickX * steeringPower, currentVel);
  205.                 velToMotor(leftVel, leftMotor);
  206.                 velToMotor(rightVel, rightMotor);
  207.                 // TODO: 10/24/2015 Explain flaws in this code section and how to improve them
  208.                 break;
  209.  
  210.             // Digital Tank...
  211.             case 3:
  212.                 //Right side
  213.                 if (gamepad1.y && rightVel buttonFAccel <= maxVel) {
  214.                     rightVel += buttonFAccel * (1000 / changeTime);
  215.                 }
  216.                 if (gamepad1.a && rightVel - buttonBAccel >= -maxVel) {
  217.                     rightVel -= buttonBAccel * (1000 / changeTime);
  218.                 }
  219.                 if (!gamepad1.y && !gamepad1.a && rightVel > 0) {
  220.                     rightVel -= buttonFDecel * (1000 / changeTime);
  221.                     if (rightVel < 0) {
  222.                         rightVel = 0;
  223.                     }
  224.                 }
  225.                 if (!gamepad1.y && !gamepad1.a && rightVel < 0) {
  226.                     rightVel += buttonBDecel * (1000 / changeTime);
  227.                     if (rightVel > 0) {
  228.                         rightVel = 0;
  229.                     }
  230.                 }
  231.                 //Left Side
  232.                 if (gamepad1.dpad_up && leftVel buttonFAccel <= maxVel) {
  233.                     leftVel += buttonFAccel * (1000 / changeTime);
  234.                 }
  235.                 if (gamepad1.dpad_down && leftVel - buttonBAccel >= -maxVel) {
  236.                     leftVel -= buttonBAccel * (1000 / changeTime);
  237.                 }
  238.                 if (!gamepad1.dpad_up && !gamepad1.dpad_down && leftVel > 0) {
  239.                     leftVel -= buttonFDecel * (1000 / changeTime);
  240.                     if (leftVel < 0) {
  241.                         leftVel = 0;
  242.                     }
  243.                 }
  244.                 if (!gamepad1.dpad_up && !gamepad1.dpad_down && leftVel < 0) {
  245.                     leftVel += buttonBDecel * (1000 / changeTime);
  246.                     if (leftVel > 0) {
  247.                         leftVel = 0;
  248.                     }
  249.                 }
  250.                 currentVel = (leftVel rightVel) / 2;
  251.                 velToMotor(leftVel, leftMotor);
  252.                 velToMotor(rightVel, rightMotor);
  253.                 break;
  254.  
  255.             // Digital Arcade... (YOU WILL HATE DRIVING THIS) (And it may not work perfectly... My head isn't always 100% accurate)
  256.             case 4:
  257.                 // using the dpad
  258.                 if (gamepad1.dpad_up && currentVel buttonFAccel <= maxVel) {
  259.                     currentVel += buttonFAccel * (1000 / changeTime);
  260.                 }
  261.                 if (gamepad1.dpad_down && currentVel - buttonBAccel >= -maxVel) {
  262.                     currentVel -= buttonBAccel * (1000 / changeTime);
  263.                 }
  264.                 if (!gamepad1.dpad_up && !gamepad1.dpad_down && currentVel > 0) {
  265.                     currentVel -= buttonFDecel * (1000 / changeTime);
  266.                     if (currentVel < 0) {
  267.                         currentVel = 0;
  268.                     }
  269.                 }
  270.                 if (!gamepad1.dpad_up && !gamepad1.dpad_down && currentVel < 0) {
  271.                     currentVel += buttonBDecel * (1000 / changeTime);
  272.                     if (currentVel > 0) {
  273.                         currentVel = 0;
  274.                     }
  275.                 }
  276.                 if (gamepad1.dpad_left && leftVel buttonFAccel <= maxVel && currentVel > 0) {
  277.                     rightVel += buttonFAccel * (1000 / changeTime);
  278.                     leftVel -= buttonFAccel * (1000 / changeTime);
  279.                 }
  280.                 if (gamepad1.dpad_right && rightVel buttonFAccel <= maxVel && currentVel > 0) {
  281.                     leftVel += buttonFAccel * (1000 / changeTime);
  282.                     rightVel -= buttonFAccel * (1000 / changeTime);
  283.                 }
  284.                 if (!gamepad1.dpad_left && !gamepad1.dpad_right && leftVel < rightVel && currentVel > 0) {
  285.                     leftVel += buttonFAccel * (1000 / changeTime);
  286.                     rightVel -= buttonFAccel * (1000 / changeTime);
  287.                     if (leftVel > rightVel) {
  288.                         leftVel = rightVel = (leftVel rightVel) / 2;
  289.                     }
  290.                 }
  291.                 if (!gamepad1.dpad_left && !gamepad1.dpad_right && leftVel > rightVel && currentVel > 0) {
  292.                     rightVel += buttonFAccel * (1000 / changeTime);
  293.                     leftVel -= buttonFAccel * (1000 / changeTime);
  294.                     if (leftVel < rightVel) {
  295.                         leftVel = rightVel = (leftVel rightVel) / 2;
  296.                     }
  297.                 }
  298.                 if (gamepad1.dpad_left && leftVel buttonFAccel <= maxVel && currentVel < 0) {
  299.                     rightVel -= buttonFAccel * (1000 / changeTime);
  300.                     leftVel += buttonFAccel * (1000 / changeTime);
  301.                 }
  302.                 if (gamepad1.dpad_right && rightVel buttonFAccel <= maxVel && currentVel < 0) {
  303.                     leftVel -= buttonFAccel * (1000 / changeTime);
  304.                     rightVel += buttonFAccel * (1000 / changeTime);
  305.                 }
  306.                 if (!gamepad1.dpad_left && !gamepad1.dpad_right && leftVel < rightVel && currentVel < 0) {
  307.                     leftVel -= buttonFAccel * (1000 / changeTime);
  308.                     rightVel += buttonFAccel * (1000 / changeTime);
  309.                     if (leftVel > rightVel) {
  310.                         leftVel = rightVel = (leftVel rightVel) / 2;
  311.                     }
  312.                 }
  313.                 if (!gamepad1.dpad_left && !gamepad1.dpad_right && leftVel > rightVel && currentVel < 0) {
  314.                     rightVel -= buttonFAccel * (1000 / changeTime);
  315.                     leftVel += buttonFAccel * (1000 / changeTime);
  316.                     if (leftVel < rightVel) {
  317.                         leftVel = rightVel = (leftVel rightVel) / 2;
  318.                     }
  319.                 }
  320.                 double difference = currentVel - ((leftVel rightVel) / 2);
  321.                 leftVel += difference;
  322.                 rightVel += difference;
  323.                 velToMotor(leftVel, leftMotor);
  324.                 velToMotor(rightVel, rightMotor);
  325.                 break;
  326.             default:
  327.                 driveMode = 0;
  328.                 break;
  329.         }
  330.         if (driveMode != 2 && driveMode != 3 && driveMode != 4){
  331.             currentVel = leftVel = rightVel = 0;
  332.         }
  333.  
  334.         // TODO: 10/26/2015 COMMENTS COMMENTS COMMENTS
  335.         if (gamepad1.left_trigger > trigThreshold && !LTpressed) {
  336.             LTpressed = true;
  337.             driveMode++;
  338.         }
  339.         else if (gamepad1.left_trigger < (trigThreshold-.1f) && LTpressed){
  340.             LTpressed = false;
  341.         }
  342.  
  343.         if (gamepad1.right_trigger > trigThreshold && !RTpressed) {
  344.             if (currentVel != 0){
  345.                 currentVel = leftVel = rightVel = 0;
  346.                 mapToMotor(0, leftMotor);
  347.                 mapToMotor(0, rightMotor);
  348.             }
  349.             RTpressed = true;
  350.             while (lastTime > System.currentTimeMillis()-1000){
  351.                 if (Math.abs(leftStickX) > stickThreshold && Math.abs(rightStickX) > stickThreshold){
  352.                     mapToMotor(.5f, leftMotor);
  353.                     mapToMotor(.5f,rightMotor);
  354.                     break;
  355.                 } else if (Math.abs(leftStickX) > stickThreshold){
  356.                     if (leftStickX < 0) autoCircle(true,true);
  357.                     if (leftStickX > 0) autoCircle(true,false);
  358.                     break;
  359.                 } else if (Math.abs(rightStickX) > stickThreshold){
  360.                     if (rightStickX < 0) autoCircle(false,true);
  361.                     if (rightStickX > 0) autoCircle(false,false);
  362.                     break;
  363.                 }
  364.             }
  365.         }
  366.         else if (gamepad1.right_trigger < (trigThreshold-.1f) && RTpressed){
  367.             RTpressed = false;
  368.         }
  369.     }
  370.  
  371.     // It's private so that it isn't accessible outside of this class.
  372.     // circle tells the class if it's making a circle of a square, and left tells
  373.     // it to turn left or right. Remember that Java is case-sensitive.
  374.     // TODO: 11/1/2015 Add acceleration abilities and comments
  375.     private void autoCircle(boolean circle, boolean left){
  376.         double speed = maxCircleSpeed*maxMotorSpeed*maxVel;
  377.         double distance, distanceTraveled = 0;
  378.         if (circle){
  379.             distance = 2 * Math.PI * circleRadius;
  380.             changeTime = System.currentTimeMillis()-lastTime;
  381.             lastTime += changeTime;
  382.             if (left) {
  383.                 velToMotor(speed, leftMotor);
  384.                 velToMotor((2 * Math.PI * (circleRadius robotWidth / 100)) / (distance / speed), rightMotor);
  385.             }else {
  386.                 velToMotor((2 * Math.PI * (circleRadius robotWidth / 100)) / (distance / speed), leftMotor);
  387.                 velToMotor(speed, rightMotor);
  388.             }
  389.             while (distance > distanceTraveled){
  390.                 changeTime = System.currentTimeMillis()-lastTime;
  391.                 lastTime += changeTime;
  392.                 distanceTraveled += speed*(1000/changeTime);
  393.             }
  394.             mapToMotor(0, leftMotor);
  395.             mapToMotor(0, rightMotor);
  396.         }
  397.         else {
  398.             distance = Math.PI * robotWidth / 4;        //distance for the outer wheel to make for a 90 degree turn in this case
  399.             for (int x = 0; x < 8; x++){
  400.                 changeTime = System.currentTimeMillis()-lastTime;
  401.                 lastTime += changeTime;
  402.                 velToMotor(speed, leftMotor);
  403.                 velToMotor(speed, rightMotor);
  404.                 while (circleRadius > distanceTraveled) {
  405.                     changeTime = System.currentTimeMillis() - lastTime;
  406.                     lastTime += changeTime;
  407.                     distanceTraveled += speed * (1000 / changeTime);
  408.                 }
  409.                 distanceTraveled = 0;
  410.                 if (x % 2 == 0){
  411.                     changeTime = System.currentTimeMillis() - lastTime;
  412.                     lastTime += changeTime;
  413.                     if (left) {
  414.                         mapToMotor(0, leftMotor);
  415.                     }else {
  416.                         mapToMotor(0, rightMotor);
  417.                     }
  418.                     while (distance > distanceTraveled) {
  419.                         changeTime = System.currentTimeMillis() - lastTime;
  420.                         lastTime += changeTime;
  421.                         distanceTraveled += speed * (1000 / changeTime);
  422.                     }
  423.                     distanceTraveled = 0;
  424.                 }
  425.             }
  426.             mapToMotor(0, leftMotor);
  427.             mapToMotor(0, rightMotor);
  428.         }
  429.     }
  430.  
  431.     // A simple function to map values on a scale of -1 to 1 to whatever the maxMotorSpeed was set to.
  432.     // Throttle is the power value you want to map to the max, and motor is the motor to actually
  433.     // change. In C++ and stuff, you'd need a pointer to affect the original object, but Java doesn't
  434.     // have pointers. Instead everything is a reference, and the JVM makes the decisions for us.
  435.     private void mapToMotor(float throttle, DcMotor motor){
  436.         // This makes sure to avoid an out of range error by limiting it.
  437.         throttle = Range.clip(throttle, -1, 1);
  438.         // Simple mathematical formula for mapping. Expanded to be easier to read. Didn't feel like
  439.         // looking for a native Java function. Edit: DANG IT THERE IS A SCALE FUNCTION, don't care.
  440.         motor.setPower(((double)throttle - (-1) ) / ( (1) - (-1) ) * ( (maxMotorSpeed) - (-maxMotorSpeed) ) -maxMotorSpeed);
  441.     }
  442.  
  443.     // Another function to convert the meters per second calculated in the main function to the
  444.     // power that the motor requires to move to that speed as defined around the flags. Reason
  445.     // behind doing this is because we have the processing power and working with more standard
  446.     // units is soo much easier. Do not use this on the final robot as we will probably have
  447.     // rotary encoders and that allows for a much more consistent measurements.
  448.     // At this point, it might have been more effective to just create a map function or look for
  449.     // one because it's basically the same as mapToMotor, but OH WELL.
  450.     // TODO: 10/22/2015 Elaborate
  451.     private void velToMotor(double velocity, DcMotor motor){
  452.         velocity = Range.clip(velocity, -maxVel, maxVel);
  453.         motor.setPower((velocity - (-maxVel)) / ((maxVel) - (-maxVel)) * ((maxVel) - (-maxVel)) -maxVel);
  454.     }
  455. }