package org.usfirst.frc.team4828;

import edu.wpi.first.wpilibj.Servo;

public class ServoGroup {
    private static final double STEP_SIZE = 0.01;

    private Servo master;
    private Servo slave;
    private double[] masterRange;
    private double[] slaveRange;
    private double position;

    /**
     * Creates a ServoGroup with the given ports
     *
     * @param masterPort port of the master servo
     * @param slavePort port of the slave servo
     */
    ServoGroup(int masterPort, int slavePort) {
        master = new Servo(masterPort);
        slave = new Servo(slavePort);
        masterRange = new double[2];
        slaveRange = new double[2];
    }

    /**
     * Creates a ServoGroup with the given ports and range values
     *
     * @param masterPort port of the master servo
     * @param slavePort port of the slave servo
     * @param masterMin minimum position of the master servo
     * @param masterMax maximum position of the master servo
     * @param slaveMin minimum position of the slave servo
     * @param slaveMax maximum position of the slave servo
     */
    ServoGroup(int masterPort, int slavePort, double masterMin, double masterMax, double slaveMin, double slaveMax) {
        master = new Servo(masterPort);
        slave = new Servo(slavePort);
        masterRange = new double[2];
        slaveRange = new double[2];
        calibrate(1, masterMin, masterMax);
        calibrate(2, slaveMin, slaveMax);
        position = 0;
    }

    /**
     * Raises the servos by a given step size
     *
     * @param step double step size
     */
    public void raise(double step) {
        set(position + step);
    }

    /**
     * Raises the servos by the default step size
     */
    public void raise() {
        raise(STEP_SIZE);
    }

    /**
     * Lowers the servos by a given step size
     *
     * @param step double step size
     */
    public void lower(double step) {
        set(position - step);
    }

    /**
     * Lowers the servos by the default step size
     */
    public void lower() {
        lower(STEP_SIZE);
    }

    /**
     * Set the servos' position
     *
     * @param pos double 0-1 relative position within range
     */
    public void set(double pos) {
        position = pos;
        master.set((masterRange[1] - masterRange[0]) * position + masterRange[0]);
        slave.set((slaveRange[1] - slaveRange[0]) * position + slaveRange[0]);
    }

    /**
     * Returns the current position
     *
     * @return double position
     */
    public double get() {
        return position;
    }

    /**
     * Configure the servos' range of motion.
     *
     * @param sel 1 = master, 2 = slave
     * @param min double lowest angle
     * @param max double highest angle
     */
    public void calibrate(int sel, double min, double max) {
        if (sel == 1) {
            masterRange[0] = min;
            masterRange[1] = max;
        } else {
            slaveRange[0] = min;
            slaveRange[1] = max;
        }
    }

    /**
     * Returns a String stating the position of each servo
     *
     * @return String
     */
    public String toString() {
        return "Master: " + master.get() + " Slave: " + slave.get();
    }

}

@@ -303,33 +303,32 @@
 **%0A     * Create
-s
  a ServoGroup wi
@@ -345,16 +345,17 @@
 en ports
+.
 %0A     *%0A
@@ -739,16 +739,17 @@
 e values
+.
 %0A     *%0A
@@ -1523,32 +1523,33 @@
  given step size
+.
 %0A     *%0A     * @
@@ -1670,33 +1670,32 @@
 /**%0A     * Raise
-s
  the servos by t
@@ -1706,32 +1706,33 @@
 efault step size
+.
 %0A     */%0A    pub
@@ -1795,33 +1795,32 @@
 /**%0A     * Lower
-s
  the servos by a
@@ -1831,24 +1831,25 @@
 en step size
+.
 %0A     *%0A    
@@ -1982,17 +1982,16 @@
  * Lower
-s
  the ser
@@ -2010,32 +2010,33 @@
 efault step size
+.
 %0A     */%0A    pub
@@ -2118,32 +2118,33 @@
 servos' position
+.
 %0A     *%0A     * @
@@ -2340,24 +2340,24 @@
 rRange%5B0%5D);%0A
-
         slav
@@ -2422,32 +2422,324 @@
 nge%5B0%5D);%0A    %7D%0A%0A
+    /**%0A     * Set a single servo's position.%0A     *%0A     * @param sel 1 = master, 2 = slave%0A     * @param pos -1-1%0A     */%0A    public void setAbsolute(int sel, double pos)%7B%0A        if(sel==1)%7B%0A            master.set(pos);%0A        %7D%0A        else%7B%0A            slave.set(pos);%0A        %7D%0A    %7D%0A%0A
     /**%0A     * R

Add calibration function