FtcRobotController/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AbsoluteEncoderTest.java at master · FTC-19743/FtcRobotController · 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
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.RobotLog;

import org.firstinspires.ftc.teamcode.libs.TeamGamepad;
import org.firstinspires.ftc.teamcode.libs.teamUtil;

@TeleOp(name="AbsoluteEncoderTest", group="Linear Opmode")
public class AbsoluteEncoderTest extends LinearOpMode {
    public static void log(String logString) {
        RobotLog.d("19743LOG:" + Thread.currentThread().getStackTrace()[3].getMethodName() + ": " + logString);
    }
    @Override
    public void runOpMode(){
        teamUtil.init(this);
        double p1 = .05;
        double p2 = .8;
        TeamGamepad gamepad;
        gamepad = new TeamGamepad();
        gamepad.initilize(true);
        AnalogInput absoluteEncoder;
        Servo servo;
        absoluteEncoder = hardwareMap.analogInput.get("absEn");
        servo = hardwareMap.servo.get("servo");
        waitForStart();
        while(opModeIsActive()){
            double voltage = absoluteEncoder.getVoltage();
            telemetry.addData("raw val", "voltage:  " + Double.toString(voltage));
            telemetry.update();
            gamepad.loop();
            if (gamepad.wasAPressed()){
                if (servo.getPosition() > .7){
                    servo.setPosition(p2);
                    long loopStartTime = System.currentTimeMillis();
                    while(absoluteEncoder.getVoltage() < 2){
                        log("Voltage: "+absoluteEncoder.getVoltage());
                    }
                    log("done in "+(System.currentTimeMillis()-loopStartTime)+" milliseconds");
                }else{
                    servo.setPosition(p1);
                    long loopStartTime = System.currentTimeMillis();
                    while(absoluteEncoder.getVoltage() > 4.4){
                        log("Voltage: "+absoluteEncoder.getVoltage());
                    }
                    log("done in "+(System.currentTimeMillis()-loopStartTime)+" milliseconds");
                }
            }
        }
    }
}