4598Stronghold2016/RobotMap.java at main · AltaHighRobotics/4598Stronghold2016 · 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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
package org.usfirst.frc.team4915.stronghold;

import org.usfirst.frc.team4915.stronghold.utils.BNO055;

import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CANTalon.FeedbackDevice;
import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;

public class RobotMap {

    // Define channels for the drive train motors
    private static final int DRIVE_TRAIN_RIGHT_BACK_MOTOR_ID = 13;
    private static final int DRIVE_TRAIN_RIGHT_FRONT_MOTOR_ID = 12;
    private static final int DRIVE_TRAIN_LEFT_BACK_MOTOR_ID = 11;
    private static final int DRIVE_TRAIN_LEFT_FRONT_MOTOR_ID = 10;

    // Define channels for two speed gear system for the drive train
    private static final int SOLENOID_CHANNEL_PRIMARY = 0;
    private static final int SOLENOID_CHANNEL_SECONDARY = 1;

    // Define port for the gyro
    public final static int GYRO_PORT = 0;

    // Define channels for IntakeLauncher motors
    private static final int INTAKE_LEFT_MOTOR_ID = 14;
    private static final int INTAKE_RIGHT_MOTOR_ID = 15;
    private static final int AIM_MOTOR_ID = 16;

    // Define port for the boulder switch
    private static final int BOULDER_SWITCH_PORT = 2;

    // Define port for the launcher pneumatic
    private static final int LAUNCHER_SOLENOID_PORT = 2; //TODO

    // Define channels for scaling motors
    private static final int SCALING_MOTOR_ID = 18; // TODO
    private static final int SCALING_WINCH_ID = 19; // TODO

    private static final int AIMER_P = 0;
    private static final int AIMER_I = 0;
    private static final int AIMER_D = 0;


    // Create motor controllers for the driveTrain
    public static CANTalon leftBackMotor;
    public static CANTalon rightBackMotor;
    public static CANTalon leftFrontMotor;
    public static CANTalon rightFrontMotor;

    // Create solenoid for the drivetrain
    public static DoubleSolenoid doubleSolenoid;

    // Create the gyro
    public static AnalogGyro gyro;

    //Create IMU
    public static BNO055 imuEuler;
    //public static BNO055 imuLinAcc;

    // Create the motor controllers for the IntakeLauncher
    public static CANTalon intakeLeftMotor;
    public static CANTalon intakeRightMotor;
    public static CANTalon aimMotor;

    // Create the boulder switch
    public static DigitalInput boulderSwitch;

    // Create the launcher solenoid
    public static Solenoid launcherSolenoid;

    // Create the motor controllers for the Scaler
    public static CANTalon scalingMotor;
    public static CANTalon scalingWinch;

    // Initialize the various robot modules
    public static void init() {
        // conditionally initialize the modules
        if (ModuleManager.DRIVE_MODULE_ON) {
            leftBackMotor = new CANTalon(DRIVE_TRAIN_LEFT_BACK_MOTOR_ID);
            rightBackMotor = new CANTalon(DRIVE_TRAIN_RIGHT_BACK_MOTOR_ID);
            leftFrontMotor = new CANTalon(DRIVE_TRAIN_LEFT_FRONT_MOTOR_ID);
            rightFrontMotor = new CANTalon(DRIVE_TRAIN_RIGHT_FRONT_MOTOR_ID);
            // TODO: Invert motors here if needed: someMotor.setInverted(true)
            /*
             * TODO: Initialize the Talon drive motors 1. establish follower
             * mode: we have 4 motor controls, but need to give commands to two
             * of them 2. establish speed control mode 3. on motors w/ encoders
             * set feedbackdevice to quadEncoder 4. optional: if driving jerky,
             * set PID values
             */
            // THe front motors are the follower motors
            // follower mode for right side
            rightFrontMotor.changeControlMode(CANTalon.TalonControlMode.Follower);
            rightFrontMotor.set(rightBackMotor.getDeviceID());
            // follow mode for left side
            leftFrontMotor.changeControlMode(CANTalon.TalonControlMode.Follower);
            leftFrontMotor.set(leftBackMotor.getDeviceID());

            System.out.println("ModuleManager RobotMap Initialize: DriveTrain Nothing to initalize... Moving on!");
        }
        if (ModuleManager.GEARSHIFT_MODULE_ON) {
            doubleSolenoid = new DoubleSolenoid(SOLENOID_CHANNEL_PRIMARY, SOLENOID_CHANNEL_SECONDARY);
        }

        if (ModuleManager.INTAKELAUNCHER_MODULE_ON) {
            intakeLeftMotor = new CANTalon(INTAKE_LEFT_MOTOR_ID);
            intakeRightMotor = new CANTalon(INTAKE_RIGHT_MOTOR_ID);
            aimMotor = new CANTalon(AIM_MOTOR_ID);
            intakeLeftMotor.changeControlMode(TalonControlMode.PercentVbus);
            intakeRightMotor.changeControlMode(TalonControlMode.PercentVbus);
            aimMotor.changeControlMode(TalonControlMode.Position);
            boulderSwitch = new DigitalInput(BOULDER_SWITCH_PORT);
            launcherSolenoid = new Solenoid(LAUNCHER_SOLENOID_PORT);
            System.out.println("ModuleManager RobotMap initialized: IntakeLauncher");

            // setup the motor
            if (aimMotor.isSensorPresent(FeedbackDevice.QuadEncoder) != null) {
                aimMotor.setFeedbackDevice(FeedbackDevice.QuadEncoder);
                aimMotor.enableLimitSwitch(true, true);
                aimMotor.enableBrakeMode(true);
                aimMotor.setPID(AIMER_P, AIMER_I, AIMER_D);
            }
            LiveWindow.addActuator("IntakeLauncher", "AimMotor", aimMotor);
        }

        if (ModuleManager.GYRO_MODULE_ON) {
            System.out.println("ModuleManager RobotMap initalize. Initialize Gyro!");
            gyro = new AnalogGyro(GYRO_PORT);
        }

        if (ModuleManager.SCALING_MODULE_ON) {
            System.out.println("ModuleManager RobotMap Initialize: Scaling");
            scalingMotor = new CANTalon(SCALING_MOTOR_ID);
            scalingWinch = new CANTalon(SCALING_WINCH_ID);
        }
        if (ModuleManager.IMU_MODULE_ON) {
            System.out.println("ModuleManager RobotMap Initialize: IMU");
            imuEuler = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, BNO055.vector_type_t.VECTOR_EULER);
            //imuLinAcc = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, BNO055.vector_type_t.VECTOR_LINEARACCEL);
        }
    }
}