package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/*
 * This class is used to test out the AS5145B encodder for absolute input
 * via the pwm out put through a low-pass filter
 * See the datasheet for instructions for wiring:
 * http://www.ams.com/eng/content/download/50206/533867/34237
 * From:
 * http://www.ams.com/eng/Products/Magnetic-Position-Sensors/Magnetic-Rotary-Position-Sensors/AS5145B
 * Wiring:
 * one wire connected to a ground(GND pin) on the magnetic encoder to the analog breakout gnd(-)
 * one wire going from the analog breakout power(pwr) to the 5v pin on the encoder
 * one wire going from the PWM pin on the encoder to a low-pass filter to change the PWM output to analog and then from the low pass filter to the desired channel on the analog breakout on the cRIO
 * optional wiring:
 * one wire to each the Mag DECn pin and the Mag INCn for testing to a pin on the digital sidecar 
 * see section 4.1 of the manual for pin descriptions 
 * 
 * 
 */
public class RobotTemplate extends SimpleRobot {

    DigitalInput magdec = new DigitalInput(4);
    DigitalInput maginc = new DigitalInput(3);
    AnalogChannel absAngle = new AnalogChannel(1);
    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        while(true) {
            // output voltage is ~4.1v. Conversion
            double angle = absAngle.getVoltage() * 360/4.9; 

            SmartDashboard.putBoolean("maginc", maginc.get());
            SmartDashboard.putBoolean("magdec", magdec.get());
            SmartDashboard.putNumber("voltage", absAngle.getVoltage());
            SmartDashboard.putNumber("angle", angle);
            System.out.println("voltage:" + absAngle.getVoltage());
        }

    }
}

@@ -1200,16 +1200,252 @@
 obot %7B%0A%0A
+    // these inputs are to test weather the magnetic encoder is in the correct field.%0A    // These don't need to be used.%0A    static final int MAGNETIC_FIELD_DECREASING_TEST = 4;%0A    static final int MAGNETIC_FIELD_INCREASING_TEST = 3;%0A
     Digi
@@ -1479,17 +1479,46 @@
 alInput(
-4
+MAGNETIC_FIELD_DECREASING_TEST
 );%0A    D
@@ -1559,10 +1559,164 @@
 put(
-3)
+MAGNETIC_FIELD_INCREASING_TEST);%0A%0A    // Find the output foltage from the low-pass filter from the pwm out%0A    static final int ABSOLUTE_ENCODER_CHANNEL = 1
 ;%0A  
@@ -1764,108 +1764,89 @@
 nel(
-1
+ABSOLUTE_ENCODER_CHANNEL
 );%0A    
-/**
 %0A    
- * This function is called once each time the robot enters operator control.%0A     */
+static final double HEURISTIC_VOLTAGE = 4.9;%0A
 %0A   
@@ -1936,17 +1936,17 @@
 e is ~4.
-1
+9
 v. Conve
@@ -1950,16 +1950,121 @@
 nversion
+ to angle%0A            // If the max voltage doesn't give 360 degrees, then the heuristic voltage is wrong
 %0A       
@@ -2111,19 +2111,33 @@
 ) * 360/
-4.9
+HEURISTIC_VOLTAGE
 ; %0A%0A    
@@ -2444,32 +2444,83 @@
 .getVoltage());%0A
+            System.out.println(%22angle:%22 + angle);%0A%0A
         %7D%0A%0A    %7D

add variables describing constansts