# 20220228~20220306 程式組
###### tags: `工作筆記 程式組`

# 20220228

## 王鴻霖 Aaron Wang

### Progress Summary

Today I mainly worked on the auto climb command and the turret auto seek functionality. I've made progress on both parts, but currently they are both unfinished. I'll describe the problem in the following section

### Progress Details

**Auto Climb**

The problems with auto climb are mainly surrounding the two fundamental issue with the hanger hardware:
1. Encoder inaccuracy: the encoder for the hanger motor is only 52 CPR, which is very inaccurate and unreliable to say the least. This make hanger control very difficult as resetting the hanger position to its minimum often result in over retracting or extending the hanger, harming the motor and causing the hanger to arrive at incorrect position.
2. Hanger motor difference: the motors seems to move at slightly different speed at the same output level. Moreover, the string that the motors use to pull the hanger are perhaps different in length, causing the problem that different amount of force is needed on the two sides.

I thought about solving this using PID, but I'm not really sure how to do it when the encoder isn't even accurate.

**Turret Auto Target Seeking**
Basically let the turret turn around if the target is not detected. It's so simple that the code is just as following:

public class TurretSeek extends CommandBase{
    
    Turret m_turret;

    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
    NetworkTableEntry tyEntry = table.getEntry("ty");
    
    double ty;
    double speed=TurretConstants.turretSpeedMultiplier/2;
    boolean targetFound = false;
    public TurretSeek(Turret m_robotTurret){
        m_turret = m_robotTurret;
        addRequirements(m_robotTurret);
    }
    public void initialize(){
        ty = tyEntry.getDouble(0.0);
    }
    public void execute(){
        ty = tyEntry.getDouble(0.0);
        if (ty == 0.0d){
            if (m_turret.atLeftLimit()) speed = TurretConstants.turretSpeedMultiplier/2;
            if (m_turret.atRightLimit()) speed = -TurretConstants.turretSpeedMultiplier/2;
            m_turret.spinnerRun(speed);
        } else {
            m_turret.spinnerRun(0);
            targetFound = true;
        }
    }
    public boolean isFinished(){
        if(targetFound) return true;
        return false;
    }
    public void end(boolean interrupted){
        m_turret.spinnerRun(0);
    }
    public void interrupted(){
        m_turret.spinnerRun(0);
    }
}

The command itself is very simple.
The hard part is how to put it into function

To do this I tried to use the ConditionalCommand class
[Wpilib documentation of ConditionalCommand](https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html#conditionalcommand)

It's basically a command that has an ifelse statement to decide which of the two commands inputted to execute. Its constructor is as follows:
> ConditionalCommand(Command commandOnTrue, Command commandOnFalse, BooleanSupplier condition)

in our case, it is:
> ConditionalCommand(new TurretSeek(m_robotTurret), new LimelightAim(m_robotTurret, m_vision), targetNotIn)

where targetNotIn is a BooleanSupplier:

BooleanSupplier targetNotIn = new BooleanSupplier() {
        @Override
        public boolean getAsBoolean(){
            if (m_vision.getY() == 0.0d) return true;
            return false; 
        }
    };

So when there is a target and yError isn't 0, it should execute LimelightAim. Else, it should execute TurretSeek.

At this point all the logic seems correct. However, once I bind the function to a button, problems start to occur.

new JoystickButton(m_driverController, OIConstants.Btn_Y)
    .whenPressed(new ConditionalCommand(new TurretSeek(m_robotTurret), new LimelightAim(m_robotTurret, m_vision), targetNotIn));

If I bind is like this, no matter I use whenHeld, whileHeld, or whenPressed, the command will only work one time and then refuse to work. I think this is either because the ConditionalCommand is not repeatable, or that the BooleanSupplier was only used once. I have yet to find a solution for this.

### Conclusion
I made some progress today. Since Albert is in charge of the trajectory tracking part now, I am more comfortable working with these two as now I can focus on them without distraction. However, I'll always be ready to help Albert as his part is perhaps the more urgent one.