Clean_2018-19/shellFish.java at master · nathan-ahn/Clean_2018-19 · 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
111
112
113
114
115
116
117
118
119
120
121
122
123
124
package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.ServoControllerEx;
import com.qualcomm.robotcore.hardware.TouchSensor;
import com.qualcomm.robotcore.util.ElapsedTime;

public class shellFish{
    //////////////////
    /* DECLARATIONS */
    //////////////////

    //public ColorSensor colorSensor = null;


    //DRIVE//
    public DcMotor front_left_motor   = null;
    public DcMotor front_right_motor  = null;
    public DcMotor back_left_motor    = null;
    public DcMotor back_right_motor   = null;

    public DcMotor chadArm1 = null;
    public DcMotor chadArm2 = null;

    public DcMotor chadExtend = null;

    public DcMotor hook = null;

    //public DcMotor chungoid = null;
    public Servo chungoid1 = null;
    public Servo chungoid2 = null;

    public CRServo intake1 = null;
    public CRServo intake2 = null;

    public AnalogInput encoderX = null;
    public AnalogInput encoderY = null;

    //IMU//
    BNO055IMU imu;

    HardwareMap hwMap           =  null;

    /* Constructor */
    public shellFish(){
    }

    /* Initialize standard Hardware interfaces */
    public void init(HardwareMap hwMap) {
        //////////////////////////////////
        /* RETRIEVING STUFF FROM PHONES */
        //////////////////////////////////

        //colorSensor = hwMap.get(ColorSensor.class, "colorSensor");

        //DRIVE//
        front_left_motor   = hwMap.dcMotor.get("fl");
        back_left_motor    = hwMap.dcMotor.get("bl");
        back_right_motor   = hwMap.dcMotor.get("br");
        front_right_motor  = hwMap.dcMotor.get("fr");
        front_left_motor.setDirection(DcMotor.Direction.REVERSE);
        back_left_motor.setDirection(DcMotor.Direction.FORWARD);
        back_right_motor.setDirection(DcMotor.Direction.FORWARD);
        front_right_motor.setDirection(DcMotor.Direction.REVERSE);
        front_left_motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        back_left_motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        back_right_motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        front_right_motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        chadArm1 = hwMap.dcMotor.get("chadArm1");
        chadArm2 = hwMap.dcMotor.get("chadArm2");
        chadArm1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        chadArm2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        chadArm1.setDirection(DcMotor.Direction.REVERSE);
        chadArm2.setDirection(DcMotor.Direction.REVERSE);
        // chadArm1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        // chadArm2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        chadArm1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        chadArm2.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        chadExtend = hwMap.dcMotor.get("chadExtend");
        chadExtend.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        hook = hwMap.dcMotor.get("hook");
        hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        //chungoid = hwMap.dcMotor.get("chungoid");
        chungoid1 = hwMap.servo.get("chungoid1");
        chungoid2 = hwMap.servo.get("chungoid2");

        intake1 = hwMap.crservo.get("intake1");
        intake2 = hwMap.crservo.get("intake2");

        encoderX = hwMap.analogInput.get("encoderX");
        encoderY = hwMap.analogInput.get("encoderY");

        //IMU//
        imu = hwMap.get(BNO055IMU.class, "imu");
        //////////////////
        /* STUFFY STUFF */
        //////////////////

    }
    public void imu(){
        /* IMU STUFF */
        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();

        parameters.mode                = BNO055IMU.SensorMode.IMU;
        parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
        parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
        parameters.loggingEnabled      = false;
        imu.initialize(parameters);
    }
}