[frasnow]

Originally Posted by dmitch

How did you code this? We use C++ on our bot and it seems like what you would have to do is set the setpoint of a PID loop to the value of the analog inputs on the model. Could you post the code that you wrote for this? And does it require any programming on the Cypress module?

In the spirit of coopertition and code reviews, here you go. The code could use a few more comments. This didn't require any Cypress module programming other than the standard setup in the FIRST documentation.

Originally Posted by apalrd

Take the inputs from the Cypress board (in Teleop.vi) and scale them to some sane units

We scaled them to a percentage.

public class Arm {
    private CANJaguar m_shoulder;
    private CANJaguar m_elbow;
    private DriverStationEnhancedIO m_enhancedIO;
    private int m_shoulderWaldoChannel;
    private int m_elbowWaldoChannel;
    private double m_shoulderValue = 0;
    private double m_elbowValue = 0;
    
    // This is correct. The down value is greater than the up value on the Waldo.
    private static final double kDownShoulderVoltage = 2.7;
    private static final double kUpShoulderVoltage = 1.3;
    private static final double kRetractedElbowVoltage = 2.8;
    private static final double kExtendedElbowVoltage = 0.9;
    public static final double kDownShoulderPosition = 0.09;
    public static final double kUpShoulderPosition = 0.55;
    public static final double kRetractedElbowPosition = 0.084;
    public static final double kExtendedElbowPosition = 0.665;

    public static final double kElbowMinToBeCollecting = .4;
    public static final double kShoulderMinWhenElbowInCollecting = 0.155;

    private DriverStation m_ds = DriverStation.getInstance();

    public Arm(int shoulderCANDevice, int elbowCANDevice, int shoulderWaldoChannel, int elbowWaldoChannel) {
        try {
            m_shoulder = new CANJaguar(shoulderCANDevice, CANJaguar.ControlMode.kPosition);
            m_shoulder.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
            m_shoulder.setPID(1000, 0, 100);
            m_shoulder.configPotentiometerTurns(1);
            m_shoulder.configSoftPositionLimits(kUpShoulderPosition, kDownShoulderPosition);
            m_shoulder.enableControl();

            m_elbow = new CANJaguar(elbowCANDevice, CANJaguar.ControlMode.kPosition);
            m_elbow.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
            m_elbow.setPID(700, 0, 50);
            m_elbow.configPotentiometerTurns(1);
            m_elbow.configSoftPositionLimits(kExtendedElbowPosition, kRetractedElbowPosition);
            m_elbow.enableControl();
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Arm Init Error!");
        }
        m_enhancedIO = DriverStation.getInstance().getEnhancedIO();
        m_shoulderWaldoChannel = shoulderWaldoChannel;
        m_elbowWaldoChannel = elbowWaldoChannel;
    }

    private void getValues() {
        try {
            m_shoulderValue = m_enhancedIO.getAnalogIn(m_shoulderWaldoChannel);
            m_shoulderValue -= kDownShoulderVoltage;
            m_shoulderValue /= (kUpShoulderVoltage - kDownShoulderVoltage);
            if (m_shoulderValue < 0) {
                m_shoulderValue = 0;
            }
            if (m_shoulderValue > 1) {
                m_shoulderValue = 1;
            }

            m_elbowValue = m_enhancedIO.getAnalogIn(m_elbowWaldoChannel);
            m_elbowValue -= kRetractedElbowVoltage;
            m_elbowValue /= (kExtendedElbowVoltage - kRetractedElbowVoltage);
            if (m_elbowValue < 0) {
                m_elbowValue = 0;
            }
            if (m_elbowValue > 1) {
                m_elbowValue = 1;
            }
        } catch (DriverStationEnhancedIO.EnhancedIOException e) {
            DriverStationPrinter.getInstance().println("Waldo Pos Read Error!");
        }
    }

    public void update() {
        getValues();
        double dMotorValueShoulder = m_shoulderValue;
        dMotorValueShoulder *= (kUpShoulderPosition - kDownShoulderPosition);
        dMotorValueShoulder += kDownShoulderPosition;

        double dMotorValueElbow = m_elbowValue;
        dMotorValueElbow *= (kExtendedElbowPosition - kRetractedElbowPosition);
        dMotorValueElbow += kRetractedElbowPosition;

        // Make sure that when the elbow is extended to collect that the shoulder stays
        // up enough to prevent the forarm from hitting the front of the robot.
        if ((dMotorValueElbow > kElbowMinToBeCollecting) && (dMotorValueShoulder < kShoulderMinWhenElbowInCollecting)){
            dMotorValueShoulder = kShoulderMinWhenElbowInCollecting;
        }

        try {
            m_shoulder.setX(dMotorValueShoulder);
            m_elbow.setX(dMotorValueElbow);
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Arm Pos Error!");
        }
    }
    
    /*
     *
     * @param shoulderPosition The position value for the shoulder in motor counts.
     * @param elbowPosition The position value for the elbow in motor counts.
     */
    public void setAutonomousArmPositions(double shoulderPosition, double elbowPosition) {
        if (m_ds.isAutonomous()) {
            // Make sure the shoulder position is in range.
            if (shoulderPosition > kUpShoulderPosition) {
                shoulderPosition = kUpShoulderPosition;
            } else if (shoulderPosition < kDownShoulderPosition) {
                shoulderPosition = kDownShoulderPosition;
            }
            // Make sure the elbow position is in range.
            if (elbowPosition > kExtendedElbowPosition) {
                elbowPosition = kExtendedElbowPosition;
            } else if (elbowPosition < kRetractedElbowPosition) {
                elbowPosition = kRetractedElbowPosition;
            }

            // Set the positions.
            try {
                m_shoulder.setX(shoulderPosition);
                m_elbow.setX(elbowPosition);
            } catch (CANTimeoutException e) {
                DriverStationPrinter.getInstance().println("CAN Auto Arm Pos Error!");
            }
        }
    }

    /**
     * Updates the information on the SmartDashboard.
     */
    public void updateSmartDashboard() {
        try {
            SmartDashboard.log(m_shoulder.getPosition(), "Shoulder Position");
            SmartDashboard.log(m_elbow.getPosition(), "Elbow Position");
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Get Arm Pos Error!");
        }
    }
}