robot.py

#!/usr/bin/env python3
#
# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.
#

import wpilib
import wpilib.drive
from wpilib import PowerDistribution
from wpilib import SmartDashboard
from wpilib import DigitalInput, Servo
from wpilib import Timer
from phoenix5 import TalonSRX, ControlMode
from phoenix6.hardware import TalonFX
from math import floor
from enum import Enum

import logging

logger = logging.getLogger(__name__)
logging.basicConfig(level=logging.DEBUG)

class DemoState(Enum):
    IDLE = 0
    CUBE_SLURP = 1
    TARGET_LOCK = 2
    VECTOR_SHIFT = 3
    CRISIS_MODE = 4

class MyRobot(wpilib.TimedRobot):
    def robotInit(self):
        """
        This function is called upon program startup and
        should be used for any initialization code.
        """
        # self.leftDrive = wpilib.PWMSparkMax(0)
        # self.rightDrive = wpilib.PWMSparkMax(1)
        # self.robotDrive = wpilib.drive.DifferentialDrive(
        #     self.leftDrive, self.rightDrive
        # )
        # self.controller = wpilib.XboxController(0)
        #self.timer = wpilib.Timer()

        # # We need to invert one side of the drivetrain so that positive voltages
        # # result in both sides moving forward. Depending on how your robot's
        # # gearbox is constructed, you might have to invert the left side instead.
        # self.rightDrive.setInverted(True)

    def autonomousInit(self):
        """This function is run once each time the robot enters autonomous mode."""
        #self.timer.restart()

    def autonomousPeriodic(self):
        """This function is called periodically during autonomous."""

        # # Drive for two seconds
        # if self.timer.get() < 2.0:
        #     # Drive forwards half speed, make sure to turn input squaring off
        #     self.robotDrive.arcadeDrive(0.5, 0, squareInputs=False)
        # else:
        #     self.robotDrive.stopMotor()  # Stop robot

    def teleopInit(self):
        """This function is called once each time the robot enters teleoperated mode."""

    state = DemoState.IDLE

    def teleopPeriodic(self):
        """This function is called periodically during teleoperated mode."""
        bb_blocked = not self.bb1.get()
        wpilib.SmartDashboard.putBoolean("Break Beam Broken", bb_blocked)

        switch1_is_pressed = not self.switch1.get()

        switch1_pressed_event = False
        switch1_unpressed_event = False

        if self.switch_1_last_state == False and switch1_is_pressed == True:
            switch1_pressed_event = True
        if self.switch_1_last_state == True and switch1_is_pressed == False:
            switch1_unpressed_event = True

        bb_blocked_event = False
        bb_unblocked_event = False

        if self.bb_last_state == False and bb_blocked == True:
            bb_blocked_event = True
        if self.bb_last_state == True and bb_blocked == False:
            bb_unblocked_event = True

        encoder_rotation = self.talon_enc.getSelectedSensorPosition(0)
        encoder_rotation_deg = ((encoder_rotation % 4096) / 4096) * 360
        wpilib.SmartDashboard.putNumber("Encoder Position", floor(encoder_rotation_deg))

        match(self.state):
            case DemoState.IDLE:
                if switch1_pressed_event:
                    self.state = DemoState.CUBE_SLURP
            case DemoState.CUBE_SLURP:
                self.talon_cim.set(ControlMode.PercentOutput, 0.1)
                if bb_blocked_event:
                    self.talon_cim.set(ControlMode.PercentOutput, 0)
                    self.state = DemoState.TARGET_LOCK
            case DemoState.TARGET_LOCK:
                self.falcon.setVoltage(0.5)
                #if switch1_pressed_event:
                if 300 < encoder_rotation_deg < 310:
                    self.falcon.setVoltage(0)
                    self.timer.reset()
                    self.timer.start()
                    self.state = DemoState.VECTOR_SHIFT
            case DemoState.VECTOR_SHIFT:
                self.small_servo.setAngle(float(next(self.triangle_gen)))
                if self.timer.get() > 5.0:
                    self.timer.stop()
                    self.state = DemoState.IDLE
            # case DemoState.CRISIS_MODE:
            #     self.talon_cim.set(ControlMode.PercentOutput, 0.1)
            #     self.falcon.setVoltage(0.5)
            #     self.small_servo.setAngle(float(next(self.triangle_gen)))
            #     if switch1_pressed_event:
            #         self.talon_cim.set(ControlMode.PercentOutput, 0)
            #         self.falcon.setVoltage(0)
            #         self.state = DemoState.IDLE

        wpilib.SmartDashboard.putString("Current State", self.state.name)
        wpilib.SmartDashboard.putBoolean("Switch 1 Pressed", switch1_is_pressed)
        # if switch1_pressed_event:
        #     self.talon_cim.set(ControlMode.PercentOutput, 0.1)
        #     self.falcon.setVoltage(0.5)
        # elif switch1_unpressed_event:
        #     self.talon_cim.set(ControlMode.PercentOutput, 0)
        #     self.falcon.setVoltage(0)
        
        # if switch1_is_pressed:
        #     pass
        #     self.small_servo.setAngle(float(next(self.triangle_gen)))
        


        self.switch_1_last_state = switch1_is_pressed
        self.bb_last_state = bb_blocked
    
    talon_cim = TalonSRX(3)
    talon_enc = TalonSRX(2)
    falcon = TalonFX(11)

    switch1 = DigitalInput(0)
    bb1 = DigitalInput(1)

    small_servo = Servo(0)

    timer = Timer()

    
    def testInit(self):
        """This function is called once each time the robot enters test mode."""
        
    

    pdp = PowerDistribution()
    switch_1_last_state = False
    bb_last_state = False
    def testPeriodic(self):
        """This function is called periodically during test mode."""
        # voltage = self.pdp.getVoltage()
        # wpilib.SmartDashboard.putNumber("Voltage", voltage)
        
        bb_blocked = not self.bb1.get()
        wpilib.SmartDashboard.putBoolean("Break Beam Broken", bb_blocked)

        switch1_is_pressed = not self.switch1.get()

        switch1_pressed_event = False
        switch1_unpressed_event = False

        if self.switch_1_last_state == False and switch1_is_pressed == True:
            switch1_pressed_event = True

        if self.switch_1_last_state == True and switch1_is_pressed == False:
            switch1_unpressed_event = True
        
        wpilib.SmartDashboard.putBoolean("Switch 1 Pressed", switch1_is_pressed)
        if switch1_pressed_event:
            self.talon_cim.set(ControlMode.PercentOutput, 0.1)
            self.falcon.setVoltage(0.5)
        elif switch1_unpressed_event:
            self.talon_cim.set(ControlMode.PercentOutput, 0)
            self.falcon.setVoltage(0)
        
        if switch1_is_pressed:
            pass
            self.small_servo.setAngle(float(next(self.triangle_gen)))
        
        encoder_rotation = self.talon_enc.getSelectedSensorPosition(0)
        encoder_rotation_deg = ((encoder_rotation % 4096) / 4096) * 360
        wpilib.SmartDashboard.putNumber("Encoder Position", floor(encoder_rotation_deg))

        self.switch_1_last_state = switch1_is_pressed


    def triangle_wave(step):
        current_value = 0
        direction = step

        while True:
            yield current_value

            next_value = current_value + direction

            if next_value > 180:
                direction = -step
                current_value = 180 + direction
            elif next_value < 0:
                direction = step
                current_value = 0 + direction
            else:
                current_value = next_value

    triangle_gen = triangle_wave(3)

if __name__ == "__main__":
    wpilib.run(MyRobot)