[LunaEques]

Our team is trying to implement an ultrasonic sensor, but we are unable to get it to read a proper distance. We have written our own sensor class, but the getDistance function does not return the proper distance. We had some trouble wiring it, but we finally figured it out. Here is our code:

In class TPARobot:

public void runUltrasonicSensor(Joystick aStick, TPAUltrasonicAnalogSensor aSensor){
if (shoot8ButtonPressable && aStick.getRawButton(8)){
ultrasonicSensorOn = flipBoolean(ultrasonicSensorOn);
if(DEBUG == true){
theDriverStationLCD.println(DriverStationLCD.Line. kUser2, 1 , "RunUltrasonicSensor recognizes button 8 press" );
System.out.println("RunUltrasonicSensor recognizes button 8 press");
}
shoot8ButtonPressable = false;
}
if (!shoot8ButtonPressable && !aStick.getRawButton(8)){
shoot8ButtonPressable = true;
theDriverStationLCD.println(DriverStationLCD.Line. kUser2, 1 , "In else statement" );
System.out.println("In else statement");

}
if (ultrasonicSensorOn == true){
aSensor.enable();
theDistance = aSensor.getDistance();
theAccumulatedDistance = theAccumulatedDistance + theDistance;
theDistancesCollected = theDistancesCollected + 1;
if (theDistancesCollected == theAveragingValue){
theAveragedDistance = theAccumulatedDistance/theDistancesCollected;
theDriverStationLCD.println(DriverStationLCD.Line. kUser6,1, "" + theAveragedDistance);
theAccumulatedDistance = 0;
theDistancesCollected = 0;
}
if(DEBUG == true){
theDriverStationLCD.println(DriverStationLCD.Line. kUser4, 1 , "Sensor Enabled " );
System.out.println("Sensor Enabled");
}
}
else{
aSensor.disable();
theDriverStationLCD.println(DriverStationLCD.Line. kUser6,1, "Sensor not Enabled ");
if(DEBUG == true){
theDriverStationLCD.println(DriverStationLCD.Line. kUser4, 1 , "Sensor Disabled" );
System.out.println("Sensor Disabled");
}
}
}

The TPAUltrasonicSensor Class:

TPAUltrasonicAnalogSensor(int aDigitalChannel, int aAnalogChannel){
theDigitalOutput = new DigitalOutput (aDigitalChannel);
theAnalogOutput = new AnalogChannel (aAnalogChannel);
theScaling = 5.0/512;
theDistance = 0;
}

public double getDistance(){
aVoltage = theAnalogOutput.getVoltage();
theDistance = aVoltage/theScaling;
return theDistance;
}

Is the code wrong?