RobotCoreExtensions/TeleopDriver.java at master · FTC3486/RobotCoreExtensions · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
package org.firstinspires.ftc.teamcode.RobotCoreExtensions;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.Gamepad;

/**
 * Created by Jacob on 7/12/2015.
 */
public class TeleopDriver
{
    private OpMode opMode;
    private Drivetrain drivetrain;
    private float maxSpeed = 1.0f;
    private float minSpeed = 0.1f;
    public enum Direction
    {
        FORWARD,
        BACKWARD
    }

    public TeleopDriver(OpMode opMode, Drivetrain drivetrain)
    {
        this.opMode = opMode;
        this.drivetrain = drivetrain;
    }

    public float getMaxSpeed()
    {
        return maxSpeed;
    }

    public void setMaxSpeed(float maxSpeed)
    {
        this.maxSpeed = maxSpeed;
    }

    public float getMinSpeed()
    {
        return minSpeed;
    }

    public void setMinSpeed(float minSpeed)
    {
        this.minSpeed = minSpeed;
    }

    public void tank_drive(Gamepad gamepad, Direction direction)
    {
        double left = get_scaled_power_from_gamepad_stick(gamepad.left_stick_y);
        double right = get_scaled_power_from_gamepad_stick(gamepad.right_stick_y);

        if(direction == Direction.FORWARD)
        {
            drivetrain.setPowers(left, right);
        }
        else
        {
            drivetrain.setPowers(-right, -left);
        }
    }
    public void half_speed_tank_drive(Gamepad gamepad, Direction direction)
    {
        double left = get_scaled_power_from_gamepad_stick(gamepad.left_stick_y)/2;
        double right = get_scaled_power_from_gamepad_stick(gamepad.right_stick_y)/2;

        if(direction == Direction.FORWARD)
        {
            drivetrain.setPowers(left, right);
        }
        else
        {
            drivetrain.setPowers(-right, -left);
        }
    }

    private double get_scaled_power_from_gamepad_stick(float stick)
    {
        return -(Math.signum(stick) * ((Math.pow(stick, 2) * (maxSpeed - minSpeed)) + minSpeed));
    }

    @Override
    public String toString() {
        return "max speed: " + String.format("%.2f", maxSpeed) + "\n" + drivetrain;
    }
}