oscf2019/macanum.java at master · sus-tenance/oscf2019 · 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


//THIS IS COMPLETE DOO DOO


package frc.robot;

import com.revrobotics.CANSparkMax;

import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;

public class macanum {

    private CANSparkMax frontRightMotor, rearRightMotor, frontLeftMotor, rearLeftMotor;
    private static int frontRightMotorID=1, rearRightMotorID=2, frontLeftMotorID=3, rearLeftMotorID=4;
    private OI oi;
    private DifferentialDrive driverType;

    public void macanumInit() {
        oi = new OI();
        frontRightMotor = new CANSparkMax(frontRightMotorID, CANSparkMax.MotorType.kBrushless);
        rearRightMotor = new CANSparkMax(rearRightMotorID, CANSparkMax.MotorType.kBrushless);
        frontLeftMotor = new CANSparkMax(frontLeftMotorID, CANSparkMax.MotorType.kBrushless);
        rearLeftMotor = new CANSparkMax(rearLeftMotorID, CANSparkMax.MotorType.kBrushless);

        // FORWARD BACKWARD
        SpeedControllerGroup forwardb = new SpeedControllerGroup(frontRightMotor, rearRightMotor);
        SpeedControllerGroup forwardba = new SpeedControllerGroup(frontLeftMotor, rearLeftMotor);
        SpeedControllerGroup forwardbFull = new SpeedControllerGroup(forwardb, forwardba);
        // ROTATE
        SpeedControllerGroup rotate = new SpeedControllerGroup(frontRightMotor, frontLeftMotor);
        SpeedControllerGroup rotatea = new SpeedControllerGroup(rearRightMotor, rearLeftMotor);
        SpeedControllerGroup rotateFull = new SpeedControllerGroup(rotate, rotatea);


        driverType = new DifferentialDrive(forwardbFull, rotateFull);
    }

    public void macanumMain() {
        driverType.arcadeDrive(oi.getDriveRightX(), oi.getDriveRightY());
		if (oi.getDriveLeftX()>0) {
            frontRightMotor.set(.3);
            frontLeftMotor.set(.3);
        }
        else {
            rearLeftMotor.set(.3);
            rearRightMotor.set(.3);
        }
    }
}