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.
#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();
}
}
}