Servi.java0

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.Range;

/*
### OUTLINE OF CONCEPT ###

#Originals
x = joyx
y = joyy
wheels = [angle1, angle2, angle3, ...]

#To polar coordinates
r = min(sqrt(x*x + y*y), 1)
theta = atan2(x, y)

#Counter for gyroscope
theta += gyro

#Wheel power
for angle in wheels:
    #closeness to front: sin()
    power = cos(theta + angle)
*/

@TeleOp(name="Servi", group="ser")
public class Servi extends OpMode {

    /* Declare OpMode members. */
    private ServiHardware robot = new ServiHardware();
    // Defines wheel positions
    double[] wheels = new double[]{ 0,((double)1/2)*3.1415,(double)3.1415,((double)3/2)*3.1415 };
    // Array for motor powers
    double[] powers = new double[]{ 0,0,0,0 };
    // Current angle of gyroscope
    float currentAngle = 0;
    // Field centric drive; toggled with X
    boolean fieldcentric = true;
    // Relative zero for field-centric drive
    float zero = 0;
    // Self-righting; disables manual turning; toggles with Y
    boolean righteous = false;
    // Currently turning; used for exact turns
    boolean turning = false;
    // Speed multiplier from 1-10(11); adjusted up/down with A/B
    int speed = 10;
    // For detecting presses of X
    boolean fcswitch = false;
    // For detecting presses of Y
    boolean rgswitch = false;
    // For detecting presses of B
    boolean slswitch = false;
    // For detecting presses of A
    boolean fsswitch = false;
    // For detecting presses of back
    boolean lsbswitch = false;
    // For detecting presses of start
    boolean rsbswitch = false;
    
    // Initialize variables
    double x,y,turn,r,theta,setangle,delta;

    /*
     * 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);

        telemetry.addData("Robot", "ready");
    }

    /*
     * 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() {
        robot.updateGyro(5);    // Obtain new gyro information
        currentAngle = robot.angles.thirdAngle;
        
        
        // --- GAMEPAD INPUT ---
        turn = gamepad1.right_stick_x;  // Use the right joy x for turning
        y = gamepad1.left_stick_y;      // Get left joy y
        x = -gamepad1.left_stick_x;     // Get left joy x
        
        
        // --- TO POLAR COORDINATES ---
        r = Math.min(Math.sqrt(x*x + y*y), 1);      // Convert x,y to speed
        theta = Math.atan2(x, y);                   // and direction
        
        
        // --- DRIVE MODES ---
        if(speed == 11 && r != 0.0) {
            r = 1.0;
        }
        if(fieldcentric) {                                              // If field-centric:
            theta += Math.toRadians(currentAngle - zero);    // Compensate direction for gyro
        }
        if(righteous || turning) {                      // If self-righting or turning:
            if(turn == 0.0) {                               // If not manually turning:
                delta = currentAngle - setangle;     // Get needed correction
                turn = wrap(delta) / 180;                       // Set turn to correction
            }
            else {                                          // If manually turning:
                setangle = currentAngle;             // Set return angle
            }
            if(Math.abs(turn) < 0.05) turn = 0;                 // don't bother if you're super close
            if(turn < 0.15 && turn > 0.05) turn = 0.15;         // if .05-.15, use .15 power
            if(turn > -0.15 && turn < -0.05) turn = -0.15;      // ditto for negative; avoids squeeking
        }
        if(turning && turn == 0) {      // If it was turning and finished:
            turning = false;                // Stop turning
        }
        

        // --- WHEEL POWERS ---
        r = r*r*r;                                  // Curve the speed exponentially
        r = r * speed/10.0;                         // Adjust the translation speed
        for(int i = 0; i != wheels.length; i++) {                   // For all the wheels:
            powers[i] = (Math.sin(wheels[i] - theta) * r ) - turn;  // calculate the desired speed
            // sin(wheel direction - desired direction finds speed), *r accounts
            // for joystick speed, +turn adds in turning with joystick/self
        }
        robot.one.setPower(powers[0]);     // Set motor powers
        robot.two.setPower(powers[1]);     // si(x, 1.5) applies curve, *(speed/10) applies speed
        robot.three.setPower(powers[2]);   // Range.clip() applies limit
        robot.four.setPower(powers[3]);
        
        
        // --- DRIVE MODE CONTROLS ---
        if(gamepad1.start) { // If "shift" is held and X is pushed:
            zero = currentAngle;             // Reset forwards for field-centric
        }
        else if(!fcswitch && gamepad1.x) {              // Otherwise, if X is pushed:
            fieldcentric = !fieldcentric;               // Toggle field-centric
        }
        if(!rgswitch && gamepad1.y) {       // If Y is pushed:
            righteous = !righteous;                 // Toggle self-righting
            setangle = currentAngle;     // Set return angle
        }
        if(!slswitch && gamepad1.b && speed > 1) {      // If B is pushed and speed over 1:
            speed -= 1;                                 // decrease speed
        }
        if(!fsswitch && gamepad1.a && speed < 10) {     // If A is pushed and speed under 10:
            speed += 1;                                 // increase speed
        }
        if(!fsswitch && gamepad1.b && gamepad1.a && speed == 10) {
            speed = 11;
        }
        
        if(!lsbswitch && gamepad1.left_stick_button) {                        // If back is pressed:
            setangle = wrap(currentAngle + 84);      // Set desired angle to current +90
            turning = true;                                     // Set to turning
        }
        if(!rsbswitch && gamepad1.right_stick_button) {                       // If start is pressed:
            setangle = wrap(currentAngle - 84);      // Set desired angle to current -90
            turning = true;                                     // Set to turning
        }
        
        fcswitch = gamepad1.x;  // Store button states for next round
        rgswitch = gamepad1.y;
        slswitch = gamepad1.b;
        fsswitch = gamepad1.a;
        lsbswitch = gamepad1.left_stick_button;
        rsbswitch = gamepad1.right_stick_button;
        
        
        // --- TELEMETRY ---
        telemetry.addData("fieldcentric (X to toggle)", fieldcentric);
        telemetry.addData("righteous (Y to toggle)", righteous);
        telemetry.addData("speed (A/B +/-)", speed);
        
        telemetry.addData("", null);
        
        telemetry.addData("r", r);
        telemetry.addData("theta", theta);
        for(int i = 0; i != wheels.length; i++) {
            telemetry.addData("wheel " + Integer.toString(i + 1), powers[i]);
        }
        if(fieldcentric || righteous || turning) {
            telemetry.addData("", null);
            telemetry.addData("angle", currentAngle);
        }
        if(fieldcentric) {
            telemetry.addData("zero", zero);
        }
        if(righteous || turning) {
            telemetry.addData("delta", delta);              
            telemetry.addData("delta%180", delta%180);
            telemetry.addData("turn", turn*180);
        }
        
        // Lift and latch
        if(gamepad2.dpad_up) {
            robot.lift.setPower(-1);
        }
        else if(gamepad2.dpad_down) {
            robot.lift.setPower(1);
        }
        else {
            robot.lift.setPower(0);
        }
        if(gamepad2.dpad_left) {
            robot.latch.setPower(-1);
        }
        else if(gamepad2.dpad_right) {
            robot.latch.setPower(1);
        }
        else {
            robot.latch.setPower(0);
        }
        
        if(gamepad1.left_bumper) {
            robot.grab.setPower(1);
        } else if(gamepad1.right_bumper) {
            robot.grab.setPower(-1);
        } else {
            robot.grab.setPower(0);
        }
        
        /*if(gamepad1.right_trigger > 0) {
            robot.arm.setPower(gamepad1.right_trigger * 0.75);
        } else if(gamepad1.left_trigger > 0) {
            robot.arm.setPower(gamepad1.left_trigger * -0.75);
        } else {
            robot.arm.setPower(0);
        }
        
        if(gamepad1.x) {
            robot.spool.setPower(1);
        } else if(gamepad1.y) {
            robot.spool.setPower(-1);
        } else {
            robot.spool.setPower(0);
        }*/  //we're already using x and y for driving modes and it's better to use the other gamepad for auxilary functions anyway
        
        robot.arm.setPower(gamepad2.left_stick_y);      //
        robot.spool.setPower(gamepad2.right_stick_y);
        
        //robot.grab.setPower(gamepad1.left_trigger - gamepad1.right_trigger);
        
        updateTelemetry(telemetry);
    }

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

    private double wrap(double input) {
        while(Math.abs(input) > 180) {
            if(input < -180) {
                input += 360;
            }
            else {
                input -= 360;
            }
        }
        return input;
    }
}