Hi all,
I’m the programmer from Team 1635. Recently I’ve been programming our sonar sensor to measure the distance to a target. However, when an initially moving robot stops, the sonar always has a 1 second delay before it returns the correct distance. Are there any ways to compensate for the delay? Any help will be much appreciated.
Post some code?
What type of sonar sensor is it, and how it it configured in software?
Some output an analog voltage. These should be pretty clear and easy to use with an AnalogInput.
Some have two digital lines, one pulsed to transmit and one pulsed when the sonar receives a ping back. You have to pulse the TX pin and measure the time before you get a response on the RX pin. This can take time, and if your timeout is very long (e.g. a second) and the response pulse is either missed or lost then your code would wait for a very long time. My guess is this particular condition causes a pulse to be lost, but the RoboRIO is waiting for a pulse for a second before giving up on it.
Another option is that something else in the same thread is blocking, and preventing the sonar code from executing.
here’s our code.
We’re using the analog sonar from MaxBotix
package org.usfirst.frc.team1635.robot.subsystems;
import org.usfirst.frc.team1635.robot.RobotMap;
import org.usfirst.frc.team1635.robot.commands.DriveWithJoystick;
import NavxMXP.AHRS;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.RobotDrive.MotorType;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Ultrasonic;
/**
*/
public class DriveTrain extends Subsystem {
RobotDrive robotDrive;
Joystick stick;
// int frontLeft, frontRight, rearLeft, rearRight;
private SpeedController frontLeft, backLeft, frontRight, backRight;
boolean onTarget;
SerialPort serial_port;
// IMU imu; // This class can be used w/nav6 and navX MXP.
// IMUAdvanced imu; // This class can be used w/nav6 and navX MXP.
AHRS imu; // This class can only be used w/the navX MXP.
boolean first_iteration;
// Ultrasonic sonar;
AnalogInput sonar;
double DistanceToStop;
double Degrees;
// analog
// Put methods for controlling this subsystem
// here. Call these from Commands.
public DriveTrain() {
super();
// DigitalInput echoChannel;
// DigitalInput pingChannel;
// sonar = new Ultrasonic(0, 1);
sonar = new AnalogInput(0);
// frontLeft = RobotMap.frontLeftChannel;
// frontRight = RobotMap.frontRightChannel;
// rearLeft = RobotMap.rearLeftChannel;
// rearRight = RobotMap.rearRightChannel;
frontLeft = new Victor(RobotMap.frontLeftChannel);
backLeft = new Victor(RobotMap.rearLeftChannel);
frontRight = new Victor(RobotMap.frontRightChannel);
backRight = new Victor(RobotMap.rearRightChannel);
robotDrive = new RobotDrive(frontLeft, backLeft, frontRight, backRight);
robotDrive.setExpiration(0.1);
robotDrive.setInvertedMotor(MotorType.kFrontRight, true); // invert the
// left side
// motors
robotDrive.setInvertedMotor(MotorType.kRearRight, true);
// robotDrive.setInvertedMotor(MotorType.kRearLeft, true);
try {
serial_port = new SerialPort(57600, SerialPort.Port.kMXP);
byte update_rate_hz = 50;
// imu = new IMU(serial_port,update_rate_hz);
// imu = new IMUAdvanced(serial_port,update_rate_hz);
imu = new AHRS(serial_port, update_rate_hz);
} catch (Exception ex) {
ex.printStackTrace();
}
if (imu != null) {
LiveWindow.addSensor("IMU", "Gyro", imu);
}
first_iteration = true;
// When calibration has completed, zero the yaw
// Calibration is complete approaximately 20 seconds
// after the robot is powered on. During calibration,
// the robot should be still
boolean is_calibrating = imu.isCalibrating();
if (first_iteration && !is_calibrating) {
Timer.delay(0.3);
imu.zeroYaw();
first_iteration = false;
}
}
/*
* to obtain the gyro value in degrees
*/
public double obtainYaw() {
return imu.getYaw();
}
public float convertToFarenheit() {
float celcius = imu.getTempC();
float farenheit = (float) (celcius * 1.8 + 32);
return farenheit;
}
public void log() {
SmartDashboard.putNumber("Gyro", obtainYaw());
SmartDashboard.putNumber("Temperature", convertToFarenheit());
SmartDashboard.putNumber("FrontLeftMotorSpd", frontLeft.get());
SmartDashboard.putNumber("FrontRightMotorSpd", frontRight.get());
SmartDashboard.putNumber("RearLeftMotorSpd", backLeft.get());
SmartDashboard.putNumber("RearRightMotorSpd", backRight.get());
SmartDashboard.putNumber("DistanceSonar", getDistanceSonar());
// SmartDashboard.putNumber("", getDistanceSonar());
}
// codes for the mecanum drive
public void mecanumDrive(Joystick joy) {
//robotDrive.mecanumDrive_Cartesian(-joy.getX() * 0.6, -joy.getY() * 0.6, -joy.getRawAxis(3) * 0.6, 0);
robotDrive.mecanumDrive_Cartesian(-joy.getX()*3 , -joy.getY()*3, -joy.getRawAxis(3)*3, 0);
// robotDrive.mecanumDrive_Cartesian(-joy.getX() * 0.6, -joy.getY() *
// 0.6,
// 0, 0);
SmartDashboard.putNumber("Xdirection_values", joy.getX());
SmartDashboard.putNumber("rotation_xxx_value", joy.getRawAxis(3));
}
public void mecanumFixed(Joystick joy) {
if (joy.getX() != 0 && joy.getY() == 0 && joy.getRawAxis(3) == 0) {
if (joy.getX() < 0) {
robotDrive.mecanumDrive_Cartesian(-joy.getX() * 0.6, 0, 0, 0);
backLeft.set(frontLeft.get() * 2);
frontRight.set(frontLeft.get() * 2);
// frontRight.set(speed);
// frontLeft.set(speed);
// backLeft.set(speed);
// backleft.set
} else if (joy.getX() > 0) {
robotDrive.mecanumDrive_Cartesian(0, -joy.getY() * 0.6, -joy.getRawAxis(3) * 0.6, 0);
backLeft.set(frontLeft.get() * 2);
frontRight.set(frontLeft.get() * 2);
}
} else if (joy.getX() == 0) {
robotDrive.mecanumDrive_Cartesian(-joy.getX() * 0.6, -joy.getY() * 0.6, -joy.getRawAxis(3) * 0.6, 0);
}
}
/*
*
* this code is designed to obtain the distance from the
* xxx_MLG_NEXus_LI_XXX_#360n0SC()P3
*/
public double getDistanceSonar() {// sonar is weird: it has a deadzone of 12
// inches; distance will always be 0
// within the deadzone
// double distance = sonar.getRangeInches();
// double distance = sonar.getValue();
//double valueToInches = 0.125;
double valueToInches_2 = 1 / 20.5;// 14.45
double distanceX = sonar.getAverageValue();
double distance = (distanceX - 237) * valueToInches_2 + 12; // convert
// voltage
// into
// inches
int distanceInt=(int) distance;
return distanceInt;
}
public void setDistToStop(double dist_) {
this.DistanceToStop = dist_;
}
public void stopRobotAtDistance() {
double distanceObtained;
// getDistanceSonar()= 0 ;
distanceObtained = getDistanceSonar();
if (distanceObtained <= DistanceToStop) {
robotDrive.mecanumDrive_Cartesian(0, 0, 0, 0);
onTarget = true;
System.out.println(" is robot finished " + true);
} else {
// Timer.delay(0.05);// compensate for sonar reading delays
robotDrive.mecanumDrive_Cartesian(0, 0.2, 0, 0);
// xxx_delayMecanum(0, 0.2, 0, 0);
System.out.println(" is robot finished " + false);
}
}
public void mecanumWithParameters(double x_axis, double y_axis, double rotaion) {
robotDrive.mecanumDrive_Cartesian(-x_axis * 0.6, -y_axis * 0.6, -rotaion * 0.6, 0);
}
public void initDefaultCommand() {
this.setDefaultCommand(new DriveWithJoystick());
// Set the default command for a subsystem here.
// setDefaultCommand(new MySpecialCommand());
}
public boolean isOnTarget() {
return onTarget;
}
public void reset() {
imu.zeroYaw();
//sonar.resetAccumulator();
onTarget = false;
// getDistanceSonar()=0;
}
public void stop() {
robotDrive.mecanumDrive_Cartesian(0, 0, 0, 0);
reset();
}This may not be it, but I notice you guys are using getAverageValue() which generates the average values the analog sensor is detecting at that very moment… maybe you guys should try and change that to getValue() (which gets the current value) My hunch is maybe there is a 1 second delay because the sensor is computing the average value instead of just spitting out the current value. (the current value will oscillate, but there might not be a 1 second delay)
Two things, if you’re posting on CD, maybe have a little class and change your username in the comments in the top area of the code. Second, post your code in between code tags. Code tags are CODE ] / CODE], except don’t put spaces between the brackets and the word code. This formats it properly and makes it much easier to read.
You should remove the comments from the code to save weight on the robot anyways.
^
Yep. Best way to save weight.
Also to readdress the question let me define the method getAverageValue() as it is stated in the latest FRC api.
public int getAverageValue():
Gets a sample from the output of the oversample and average engine for this channel. The sample is 12-bit + the bits configured in SetOversampleBits(). The value configured in setAverageBits() will cause this value to be averaged 2^bits number of samples. This is not a sliding window. The sample will not change until 2^(OversampleBits + AverageBits) samples have been acquired from this channel. Use getAverageVoltage() to get the analog value in calibrated units.
Method Returns:
A sample from the oversample and average engine for this channel.
So in this case, the output is having a 1 second delay because the sample will not change until 2^(OversampleBits + AverageBits) have been acquired.
Hope this answers your question!