Superstructure

Superstructure

[ Insert Documentation Here]

Sudo code for state machine

enum ArmState{
    Home,
    Lowscore,
    Midscore,
    Highscore,
    lowPortal,
    highPortal,
    ground,
    pickup,
  }

// define armstate

enum IntakeState{
    Cone,
    Cube,
    Empty,
}

enum PieceToPickup{
    Cone,
    Cube, 
}

pieceToPicup toggle = cube

def setPiece(){

    if toggle = cone:
        toggle = cube
        return
    toggle = cone

}

def pickup(){

}

manualMode = Codriver Control 

@periodic{

if maunal mode:
    Armstate set to home
    Manual control
else: 
    switch ArmState:
        if home:
            Go to home postion
            Wait codriver command to go to other states
            if Lowscore:
                next state = Lowscore state
                setTimeout for 5000ms
            elif midscore:
                next state = Midscore state
                set timeout for 5000ms
            elif Highscore:
                next state = Highscore state
                set timeout for 5000ms
            elif ground:
                next state = ground
                set timeout for 2000
            elif lowportal:
                next state = low portal
                set timeout for 2000
            elif high portal:
                next state = high portal
                set timeout for 2000

        if Lowscore:
            Go to Lowscore position
            if IntakeState.Empty then go ArmState.Home
            if codriver button press to home, go to ArmState.Home
            if timeout go to home
        if Midscore:
            Go to midscore position
            if IntakeState.Empty then go ArmState.Home
            If codriver button press to home, go to ArmState.Home
            if timeout go to home
        if Highscore:
            Go to Highscore position
            If IntakeState.Empty then go ArmState.home
            If codriver button press to home, go to ArmState.Home
            if timeout go to home
        if ground:
            Go to Ground position
            go to pickuup
            if timout go to home
            set time out for 10000
        if Low portal
            Go to low portal
            go to pickup
            if time out go to home
            set time out for 8000
        if High portal
            go to High porltal
            got to pickup
            if time out go to home
            set time out for 8000
        if pickup:
            if toggle = cone;
                spin cone pickup
            elif toggle = cube;
                spin cube pickup;
            // Check for current on the SRX if that does not work use the pdp
            if IntakeState != Empty: 
                stop rollers
                Go home
            if time out go to home
    smartdashboard put state

    Countdown
}

    [set timeout code]

timout actual java code

private int TimerCounter = 0; 
private boolean expired = true; 

public void setTimeOut(double ms){
    TimerCounter = (int) ms/20; 
  }
  public void countDown(){
    if(TimerCounter != 0){
      TimerCounter -= 1;
      expired = false;
    }
    else{
      expired = true;
    }
  }
  public boolean isTimedOut(){
    return expired;
  }
```java