3507-IntoTheDeep/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java at master · fwprobotics/3507-IntoTheDeep · 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
package org.firstinspires.ftc.teamcode;

import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.InstantAction;
import com.acmerobotics.roadrunner.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.qualcomm.robotcore.hardware.HardwareMap;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.autonomous.FieldTrajectoryPlanner;
import org.firstinspires.ftc.teamcode.pipelines.HuskySampleDetect;
import org.firstinspires.ftc.teamcode.subsystems.Arm;
import org.firstinspires.ftc.teamcode.subsystems.Claw;
import org.firstinspires.ftc.teamcode.subsystems.Hang;
import org.firstinspires.ftc.teamcode.subsystems.Lift;
import org.firstinspires.ftc.teamcode.subsystems.Wrist;

public class Robot {

    public  enum AutoPos {
        REDHUMAN (1, -1),
        REDNET (-1, -1),
        BLUEHUMAN (-1, 1),
        BLUENET (1, 1);

        public int xMult;
        public int yMult;

        AutoPos(int xMult, int yMult) {
            this.xMult = xMult;
            this.yMult = yMult;
        }
    }

    public enum RobotStates {
        DEFAULT (Lift.LiftStates.FLOOR, Arm.ArmStates.STORED, Wrist.WristStates.OUT),
        INTAKE (Lift.LiftStates.FLOOR, Arm.ArmStates.INTAKE, Wrist.WristStates.DOWN),
        SPECIMEN (Lift.LiftStates.SPECIMEN, Arm.ArmStates.OUT, Wrist.WristStates.OUT),
        LOW_CHAMBER (Lift.LiftStates.LOW_CHAMBER, Arm.ArmStates.OUT, Wrist.WristStates.OUT),
        HIGH_CHAMBER (Lift.LiftStates.LOW_BASKET, Arm.ArmStates.OUT, Wrist.WristStates.OUT),
    //    LOW_BASKET,
        HIGH_BASKET (Lift.LiftStates.HIGH_BASKET, Arm.ArmStates.OUT, Wrist.WristStates.OUT),
        HANG (Lift.LiftStates.HANG, Arm.ArmStates.STORED, Wrist.WristStates.OUT);

        Lift.LiftStates liftState;
        Arm.ArmStates armState;
        Wrist.WristStates wristState;

        RobotStates(Lift.LiftStates liftState, Arm.ArmStates armState, Wrist.WristStates wristState) {
            this.liftState = liftState;
            this.armState = armState;
            this.wristState = wristState;
        }

    }

    public Lift lift;
    public Arm arm;
    public Wrist wrist;
    public Claw claw;

    public Hang hang;
    public HuskySampleDetect huskyLens;
    public MecanumDrive drive;

    public AutoPos autoPos;

    public RobotStates currentState = RobotStates.DEFAULT;

    public Pose2d startingPos;

    public Robot(HardwareMap hardwareMap, Telemetry telemetry, AutoPos autoPos, boolean teleop) {
        this.lift = new Lift(hardwareMap, telemetry, teleop);
        this.arm = new Arm(hardwareMap, telemetry);
        this.wrist = new Wrist(hardwareMap, telemetry);
        this.claw = new Claw(hardwareMap, telemetry);
        this.hang = new Hang(hardwareMap, telemetry);
        this.huskyLens = new HuskySampleDetect(hardwareMap, telemetry);
        this.startingPos = new Pose2d(10*autoPos.xMult, 63* autoPos.yMult, Math.toRadians(-90* autoPos.yMult));
        this.drive = new MecanumDrive(hardwareMap, startingPos);
        this.autoPos = autoPos;

    }

    public Action robotAction(RobotStates state) {
        currentState = state;
        return new SequentialAction(
                this.claw.clawAction(Claw.ClawStates.CLOSE),
                this.lift.liftAction(state.liftState),
                this.arm.armAction(state.armState),
                this.wrist.wristAction(state.wristState),
                state.armState.setPos == Arm.ArmStates.INTAKE.setPos ? new SequentialAction(new SleepAction(0.1), this.claw.clawAction(Claw.ClawStates.OPEN)) : new InstantAction(() -> {})
        );
    }

    public void setRobotState(RobotStates state) {
        currentState = state;
       claw.setPosition(Claw.ClawStates.CLOSE);
       arm.setState(state.armState);
       wrist.setWristState(state.wristState);
       lift.setState(state.liftState);
    }

    public FieldTrajectoryPlanner createTrajectoryPlanner() {
        return new FieldTrajectoryPlanner(this);
    }

    //TODO: husky lens/opencv centering script, combinbed lift, arm, wrist action
}