robocode_ftc5060/BlockManipulation.java at master · MiltonStatic5060/robocode_ftc5060 · 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
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.hardware.*;
/**
 * DcMotor
 * Servo
 * DcMotorController
 * ServoController
 * AccelerationSensor
 * LightSensor
 * ColorSensor
 * CRServo is Continuous Rotation Servo
 * CompassSensor
 * DeviceInterfaceModule
 * DistanceSensor
 * Gyroscope
 * GyroSensor
 * I2cController
 * I2cDevice
 * IrSeekerSensor
 * LegacyModule
 * TouchSensor
 * UltrasonicSensor
 */
/**
 * gamepad1 or gamepad2
 * true/false:
 * a,b,x,y,left_bumper,right_bumper
 * dpad_up,dpad_down,dpad_right,dpad_left
 * left_stick_button,right_stick_button
 * guide,start,back
 *
 * -1/1 (up is -1, and left is -1)
 * left_stick_y,left_stick_x
 * right_stick_y,right_stick_x
 *
 * 0/1
 * left_trigger,right_trigger
 *
 */

@TeleOp(name = "Concept: NullOp", group = "Concept")
//@Autonomous(name = "Concept: NullOp", group = "Concept")
//@Disabled
public class TeleOpTemplate extends OpMode {
    DcMotor blockLift;
    Servo blockGrabL;
    Servo blockGrabR;
    TouchSensor blockTouch;

    @Override
    public void init(){

        blockLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    }

    @Override
    public void start(){
        //Calibrate BlockLift encoder
        while(!blockTouch.isPressed()){
            blockLift.setPower(-0.3);
            if(blockTouch.isPressed()){
                break;
            }
        }
        blockLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    }

    @Override
    public void loop(){

    }
    public void r_grabber(){
        blockGrabL.setPosition(Range.clip(right_trigger,0,1));
        blockGrabR.setPosition(Range.clip((1.0-right_trigger),0,1));
        blockLift.setPower(0.5);
        blockLift.setTargetPosition((1-gamepad1.left_trigger)*-1800);
        // blockLift.setMode(DcMotor.RunMode);
        //STOP_AND_RESET_ENCODER - current encoder position zero
        //RUN_TO_POSITION - go to target encoder
        //RUN_USING_ENCODER - constant velocity
        //RUN_WITHOUt_ENCODER - just normal power based

        if(gamepad1.back){
            while(!blockTouch.isPressed()){  //move to top position
                blockLift.setPower(-0.3);
                if(blockTouch.isPressed()){
                    break;
                }
            }
            blockLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            blockLift.setPower(0.5);
            blockLift.setTargetPosition(-1800);
            blockLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        }
        /**
         * set power to -0.3 until touchsensor==true
         * setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER)
         * blockLift.setPower(0.5);
         * blockLift.setTargetPosition(-1800);
         * setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER)
         *
         */
    }
}