TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/SquirrelyDrive1.java
@Override
	public void loop() {

		// turning drive is on the left stick
		float tx = gamepad1.left_stick_x;
		float ty = -gamepad1.left_stick_y;	// y is reversed :(
		float left = (ty + tx/2);
		float right = (ty - tx/2);

		// clip the right/left values so that the values never exceed +/- 1
		left = Range.clip(left, -1, 1);
		right = Range.clip(right, -1, 1);

		// scale the joystick values to make it easier to control
		// the robot more precisely at slower speeds.
		//left =  (float)scaleInput(left);
		//right = (float)scaleInput(right);

		// squirrely drive is on the right stick
		float x = gamepad1.right_stick_x;
		float y = -gamepad1.right_stick_y;	// y is reversed :(

		// clip the values so that the values never exceed +/- 1
		x = Range.clip(x, -1, 1);
		y = Range.clip(y, -1, 1);

		// direction angle of stick >> the relative direction we want to move
		double theta = Math.atan2(-x, y);	// stick angle: zero = +y, positive CCW, range +-pi
		double heading = theta * 180.0/Math.PI;		// radians to degrees

		// compute front and back wheel relative speeds needed to go in desired direction
		SquirrelyLib.MotorPowers mp = SquirrelyLib.GetSquirrelyWheelMotorPowers(heading);
		double front = mp.Front();
		double back = mp.Back();

		// power is the magnitude of the stick vector
		double power = Math.sqrt(x*x + y*y);

		// scale the values by the desired power
		front *= power;
		back *= power;

		// combine turning and squirrely drive inputs and
		double fr = Range.clip(front+right, -1, 1);
		double br = Range.clip(back+right, -1, 1);
		double fl = Range.clip(front+left, -1, 1);
		double bl = Range.clip(back+left, -1, 1);

		// write the values to the motors
		if (!bDebug) {
			motorFrontRight.setPower(fr);
			motorBackRight.setPower(br);
			motorFrontLeft.setPower(fl);
			motorBackLeft.setPower(bl);
		}

		/*
		 * Send telemetry data back to driver station.
		 */
		telemetry.addData("Text", "*** v1.1 ***");
		telemetry.addData("front left/right power:", "%.2f %.2f", fl, fr);
		telemetry.addData("back left/right power:", "%.2f %.2f", bl, br);
		telemetry.addData("heading:", "%.2f", heading);
		telemetry.addData("gamepad1:", gamepad1);
		//telemetry.addData("gamepad2", gamepad2);
	}
/*
	 * This method will be called repeatedly in a loop
	 * 
	 * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
	 */
This method will be called repeatedly in a loop


This method will be called repeatedly in a loop
@Override
	public void loop() {
		float tx = gamepad1.left_stick_x;
		float ty = -gamepad1.left_stick_y;	
		float left = (ty + tx/2);
		float right = (ty - tx/2);
		left = Range.clip(left, -1, 1);
		right = Range.clip(right, -1, 1);
		float x = gamepad1.right_stick_x;
		float y = -gamepad1.right_stick_y;	
		x = Range.clip(x, -1, 1);
		y = Range.clip(y, -1, 1);
		double theta = Math.atan2(-x, y);	
		double heading = theta * 180.0/Math.PI;		
		SquirrelyLib.MotorPowers mp = SquirrelyLib.GetSquirrelyWheelMotorPowers(heading);
		double front = mp.Front();
		double back = mp.Back();
		double power = Math.sqrt(x*x + y*y);
		front *= power;
		back *= power;
		double fr = Range.clip(front+right, -1, 1);
		double br = Range.clip(back+right, -1, 1);
		double fl = Range.clip(front+left, -1, 1);
		double bl = Range.clip(back+left, -1, 1);
		if (!bDebug) {
			motorFrontRight.setPower(fr);
			motorBackRight.setPower(br);
			motorFrontLeft.setPower(fl);
			motorBackLeft.setPower(bl);
		}
		/*
		 * Send telemetry data back to driver station.
		 */
		telemetry.addData("Text", "*** v1.1 ***");
		telemetry.addData("front left/right power:", "%.2f %.2f", fl, fr);
		telemetry.addData("back left/right power:", "%.2f %.2f", bl, br);
		telemetry.addData("heading:", "%.2f", heading);
		telemetry.addData("gamepad1:", gamepad1);
	}
@ Override public void loop ( ) { float tx = gamepad1 . left_stick_x ; float ty = - gamepad1 . left_stick_y ; float left = ( ty + tx / 2 ) ; float right = ( ty - tx / 2 ) ; left = Range . clip ( left , - 1 , 1 ) ; right = Range . clip ( right , - 1 , 1 ) ; float x = gamepad1 . right_stick_x ; float y = - gamepad1 . right_stick_y ; x = Range . clip ( x , - 1 , 1 ) ; y = Range . clip ( y , - 1 , 1 ) ; double theta = Math . atan2 ( - x , y ) ; double heading = theta * 180.0 / Math . PI ; SquirrelyLib . MotorPowers mp = SquirrelyLib . GetSquirrelyWheelMotorPowers ( heading ) ; double front = mp . Front ( ) ; double back = mp . Back ( ) ; double power = Math . sqrt ( x * x + y * y ) ; front *= power ; back *= power ; double fr = Range . clip ( front + right , - 1 , 1 ) ; double br = Range . clip ( back + right , - 1 , 1 ) ; double fl = Range . clip ( front + left , - 1 , 1 ) ; double bl = Range . clip ( back + left , - 1 , 1 ) ; if ( ! bDebug ) { motorFrontRight . setPower ( fr ) ; motorBackRight . setPower ( br ) ; motorFrontLeft . setPower ( fl ) ; motorBackLeft . setPower ( bl ) ; } /*
		 * Send telemetry data back to driver station.
		 */ telemetry . addData ( "Text" , "*** v1.1 ***" ) ; telemetry . addData ( "front left/right power:" , "%.2f %.2f" , fl , fr ) ; telemetry . addData ( "back left/right power:" , "%.2f %.2f" , bl , br ) ; telemetry . addData ( "heading:" , "%.2f" , heading ) ; telemetry . addData ( "gamepad1:" , gamepad1 ) ; }
This method will be called repeatedly in a loop
This method will be called repeatedly in a loop
// turning drive is on the left stick // y is reversed :( // clip the right/left values so that the values never exceed +/- 1 // scale the joystick values to make it easier to control // the robot more precisely at slower speeds. //left =  (float)scaleInput(left); //right = (float)scaleInput(right); // squirrely drive is on the right stick // y is reversed :( // clip the values so that the values never exceed +/- 1 // direction angle of stick >> the relative direction we want to move // stick angle: zero = +y, positive CCW, range +-pi // radians to degrees // compute front and back wheel relative speeds needed to go in desired direction // power is the magnitude of the stick vector // scale the values by the desired power // combine turning and squirrely drive inputs and // write the values to the motors //telemetry.addData("gamepad2", gamepad2);