Ultrasonic sometimes reporting -1 Pros

I have a robot that is required to drive autonomously. To achieve that I’m using 4 ultrasonic sensors. Two of them are in the front at an angle so that they can see in front of the robot and down a bit. While running my program the sensors will occasionally report a -1 instead of a real value. It’s every couple of seconds and when it starts it doesn’t seem to stop without moving the robot of putting something in front of the sensor. I provided my code but I’m sure that it isn’t it but threw it in anyway. IMG_20200730_143536

#include "main.h"

// controller declaration
Controller master (ControllerId::master);

bool chassisBrake = false;

Motor frontLeftMotor(20, false, AbstractMotor::gearset::green, AbstractMotor::encoderUnits::degrees);
Motor backLeftMotor(19, false, AbstractMotor::gearset::green, AbstractMotor::encoderUnits::degrees);
Motor frontRightMotor(11, true, AbstractMotor::gearset::green, AbstractMotor::encoderUnits::degrees);
Motor backRightMotor(12, true, AbstractMotor::gearset::green, AbstractMotor::encoderUnits::degrees);

pros::Imu IMU(15);

pros::ADIUltrasonic FLUltra('C', 'D');
pros::ADIUltrasonic FRUltra('E', 'F');

pros::ADIUltrasonic LUltra ('A', 'B');
pros::ADIUltrasonic RUltra('G', 'H');

std::shared_ptr<OdomChassisController> chassis = ChassisControllerBuilder()
	.withMotors(frontLeftMotor, frontRightMotor, backRightMotor, backLeftMotor)
  .withMaxVelocity(50)
    .withGains(
        {0.0025, 0, 0}, // Distance controller gains
        {0.002, 0, 0}, // Turn controller gains
        {0.002, 0, 0.00006}  // Angle controller gains (helps drive straight)
    )
    .withDimensions(AbstractMotor::gearset::green, {{4_in, 8.5_in}, imev5GreenTPR})
    .withOdometry() // use the same scales as the chassis (above)
    .buildOdometry(); // build an odometry chassis

std::shared_ptr<okapi::SkidSteerModel> driveTrain = std::dynamic_pointer_cast<SkidSteerModel>(chassis->getModel());


bool isAuto = false;
int nearThrethhold = 250;

int FLUltraValue;
int FRUltraValue;
int LUltraValue;
int RUltraValue;

void opcontrol() {
  lv_tabview_set_tab_act(tabview, 1, true);
  chassis->setTurnsMirrored(false);

  IMU.reset();
  pros::delay(2000);
  // std::vector<int> ultraSonicBaseValues;
  // ultraSonicBaseValues.push_back(LUltra.get_value());
  // ultraSonicBaseValues.push_back(RUltra.get_value());
  // ultraSonicBaseValues.push_back(FLUltra.get_value());
  // ultraSonicBaseValues.push_back(FRUltra.get_value());

  while (true) {
    FLUltraValue = FLUltra.get_value();
    pros::delay(50);
    FRUltraValue = FRUltra.get_value();
    LUltraValue = LUltra.get_value();
    RUltraValue = RUltra.get_value();

    // Graphs the drive motor temps
    // lv_chart_set_next(chart, NavyLine, frontLeftMotor.getTemperature());
    // lv_chart_set_next(chart, BlueLine, backLeftMotor.getTemperature());
    // lv_chart_set_next(chart, GreenLine, frontRightMotor.getTemperature());
    // lv_chart_set_next(chart, LimeLine, backRightMotor.getTemperature());


    double difference=((LUltraValue-RUltraValue)/1);

    updateLineVariable(1, difference);
    updateLineVariable(2, LUltraValue);
    updateLineVariable(3, RUltraValue);
    updateLineVariable(4, FLUltraValue);
    updateLineVariable(5, FRUltraValue);

    if (master.getDigital(ControllerDigital::A)) {
      isAuto = true;
    }
    else {
      isAuto = false;
    }

    if (isAuto == false) {
      chassis->getModel()->arcade((master.getAnalog(ControllerAnalog::leftY)/2), (master.getAnalog(ControllerAnalog::rightX)/2), 0.03);
    }
    else if (FLUltraValue != -1 && FRUltraValue != -1 && LUltraValue != -1 && RUltraValue != -1) {
      if (FLUltraValue < nearThrethhold && FRUltraValue < nearThrethhold) {
        chassis->moveDistance(-4_in);
      }
      else if (FLUltraValue < nearThrethhold) {
        chassis->getModel()->tank(-0.2, 0.2);
      }
      else if (FRUltraValue < nearThrethhold) {
        chassis->getModel()->tank(0.2, -0.2);
      }
      else if (FLUltraValue > nearThrethhold && FLUltraValue > nearThrethhold) {
        chassis->getModel()->tank(0.2, 0.2);
      }
    }
    else {
      chassis->stop();
    }
  }
}
1 Like

-1 simply means the sonar doesn’t have a valid reading, just ignore it.

12 Likes

Note, if you are averaging values, remember not to include the - 1.

11 Likes

@jpearman @tabor473 Thanks for the confirmation that it was an error state. After a lot of testing I found that it had to do with the sensors being angled.

2 Likes

I guess I’ll mention to myself.

Averaging can have a bunch of problems and often a median filter is better. It also has the fortunate biproduct of naturally not being effected by the - 1. (assuming small amount of - 1s)

2 Likes