1516X Push Back 1.0
1516X's robot code for the 2025-2026 VEX Robotics Competition
Loading...
Searching...
No Matches
Functions | Variables
MCL Namespace Reference

Functions

mt19937 Random (random_device {}())
 
vector< Particle > Particles (num_particles)
 
double getAvgVelocity (void) noexcept
 
void StartMCL (double x_, double y_, double theta_)
 
void MonteCarlo (void)
 
vector< Particle > Resampled (num_particles)
 
vector< double > CDF (num_particles)
 

Variables

double X = 0
 
double Y = 0
 
double theta = 0
 
double Velo = 0
 
double Deviation
 
double weights_sum
 
double start_theta
 
vector< MCLDistanceSensor > Sensors
 
Field field_
 
vector< MCLDistanceSensor > activeSensors
 
pros::Mutex particle_mutex
 
const double LOOP_DELAY_MS = 20.0
 
const double LOOP_DT_SEC = LOOP_DELAY_MS / 1000.0
 
const float wheel_circumference = (float)lemlib::Omniwheel::NEW_325 * M_PI
 
const float gear_ratio = 4.0f / 3.0f
 
const float rpm_to_ips_factor = (wheel_circumference / gear_ratio) / 60.0f
 

Function Documentation

◆ CDF()

vector< double > MCL::CDF ( num_particles )

Referenced by MonteCarlo().

◆ getAvgVelocity()

double MCL::getAvgVelocity ( void )
noexcept

Definition at line 171 of file MCL.cpp.

171 {
172 const double V = (leftMotors.get_actual_velocity() * rpm_to_ips_factor + rightMotors.get_actual_velocity() * rpm_to_ips_factor) / 2.0;
173 return V;
174 }
rightMotors
pros::MotorGroup rightMotors
leftMotors
pros::MotorGroup leftMotors
Definition globals.cpp:20
MCL::rpm_to_ips_factor
const float rpm_to_ips_factor
Definition MCL.cpp:169

References leftMotors, rightMotors, and rpm_to_ips_factor.

Referenced by MonteCarlo().

◆ MonteCarlo()

void MCL::MonteCarlo ( void )

Definition at line 63 of file MCL.cpp.

63 {
64 while (true) {
65 uint32_t start_time = pros::millis();
66
67 Velo = getAvgVelocity();
68 double distance_step = Velo * LOOP_DT_SEC;
69 normal_distribution<double> dist_pos(0, std::abs(distance_step * 0.25));
70
71 lemlib::Pose odomPose = chassis.getPose(true);
72 const double theta_ = odomPose.theta;
73 const double rotated_theta = M_PI_2 - theta_;
74 const float cos_theta = cosf(rotated_theta);
75 const float sin_theta = sinf(rotated_theta);
76
77 particle_mutex.take();
78
79 for (auto& p : Particles) {
80 p.theta = theta_;
81 p.step = Point(cos_theta, sin_theta);
82 p.x += (distance_step * cos_theta) + dist_pos(Random);
83 p.y += (distance_step * sin_theta) + dist_pos(Random);
84 p.x = std::clamp(p.x, -field_.HalfSize, field_.HalfSize);
85 p.y = std::clamp(p.y, -field_.HalfSize, field_.HalfSize);
86 }
87
88 for (auto& sensor : Sensors) {
89 sensor.Measure();
90 }
91
92 activeSensors.clear();
93 for (auto& sensor : Sensors) {
94 if (sensor.measurement > -1) {
95 activeSensors.push_back(sensor);
96 }
97 }
98
99 weights_sum = 0;
100 if (!activeSensors.empty()) {
101 for (auto& P : Particles) {
102 double wt = 1.0;
103 for (auto& sensor : activeSensors) {
104 const double predicted = field_.get_sensor_distance(P, sensor);
105 if (predicted < 0) {
106 wt = 0;
107 break;
108 }
109 Deviation = (predicted - sensor.measurement);
110 wt *= exp((Deviation * Deviation) * inv_varience) * inv_base;
111 }
112 weights_sum += wt;
113 P.weight = wt;
114 }
115 }
116
117 if (weights_sum < MIN_WEIGHT) {
118 const double Weight = inv_num_particles;
119 for (auto &p : Particles) p.weight = Weight;
120 } else {
121 const double inv = 1.0 / weights_sum;
122 for (auto &p : Particles) p.weight *= inv;
123 }
124
125 CDF[0] = Particles[0].weight;
126 for (int i = 1; i < num_particles; ++i) {
127 CDF[i] = CDF[i - 1] + Particles[i].weight;
128 }
129
130 uniform_real_distribution<double> dist(0, inv_num_particles);
131 if(Resampled.size() != num_particles) Resampled.resize(num_particles);
132
133 for (int i = 0; i < num_particles; ++i) {
134 const double sample_point = i * (inv_num_particles) + dist(Random);
135 auto it = lower_bound(CDF.begin(), CDF.end(), sample_point);
136 int index = std::distance(CDF.begin(), it);
137 if(index >= num_particles) index = num_particles - 1;
138 Resampled[i] = Particles[index];
139 }
140 Particles = Resampled;
141
142 double new_x = 0.0, new_y = 0.0, total_weight = 0.0;
143 for (const auto& p : Particles) {
144 new_x += p.x * p.weight;
145 new_y += p.y * p.weight;
146 total_weight += p.weight;
147 }
148
149 if (total_weight > 0) {
150 new_x /= total_weight;
151 new_y /= total_weight;
152 }
153
154 X = new_x;
155 Y = new_y;
156
157 particle_mutex.give();
158
159 double dist_error = std::hypot(odomPose.x - X, odomPose.y - Y);
160
161 cout << " time: (" << pros::millis() - start_time << ") " << std::endl;
162
163 pros::Task::delay_until(&start_time, LOOP_DELAY_MS);
164 }
165 }
chassis
lemlib::Chassis chassis
MCL::activeSensors
vector< MCLDistanceSensor > activeSensors
Definition MCL.cpp:29
MCL::Y
double Y
Definition MCL.cpp:10
MCL::LOOP_DELAY_MS
const double LOOP_DELAY_MS
Definition MCL.cpp:37
MCL::Random
mt19937 Random(random_device {}())
MCL::Deviation
double Deviation
Definition MCL.cpp:15
MCL::X
double X
Definition MCL.cpp:10
MCL::getAvgVelocity
double getAvgVelocity(void) noexcept
Definition MCL.cpp:171
MCL::Velo
double Velo
Definition MCL.cpp:14
MCL::particle_mutex
pros::Mutex particle_mutex
Definition MCL.cpp:31
MCL::LOOP_DT_SEC
const double LOOP_DT_SEC
Definition MCL.cpp:38
MCL::Sensors
vector< MCLDistanceSensor > Sensors
Definition MCL.cpp:21
MCL::Particles
vector< Particle > Particles(num_particles)
MCL::Resampled
vector< Particle > Resampled(num_particles)
MCL::weights_sum
double weights_sum
Definition MCL.cpp:16
MCL::CDF
vector< double > CDF(num_particles)
MCL::field_
Field field_
Definition MCL.cpp:28

References activeSensors, CDF(), chassis, Deviation, field_, getAvgVelocity(), LOOP_DELAY_MS, LOOP_DT_SEC, particle_mutex, Particles(), Random(), Resampled(), Sensors, Velo, weights_sum, X, and Y.

◆ Particles()

vector< Particle > MCL::Particles ( num_particles )

Referenced by MonteCarlo(), and StartMCL().

◆ Random()

mt19937 MCL::Random ( random_device {} ())

Referenced by MonteCarlo(), and StartMCL().

◆ Resampled()

vector< Particle > MCL::Resampled ( num_particles )

Referenced by MonteCarlo().

◆ StartMCL()

void MCL::StartMCL ( double x_,
double y_,
double theta_ )

Definition at line 40 of file MCL.cpp.

40 {
41 particle_mutex.take();
42
43 Particles.clear();
44 Particles.resize(num_particles);
45
46 uniform_real_distribution<double> start_dist_x(-start_std_pos[0], start_std_pos[0]);
47 uniform_real_distribution<double> start_dist_y(-start_std_pos[1], start_std_pos[1]);
48 uniform_real_distribution<double> start_dist_theta(-start_std_pos[2], start_std_pos[2]);
49
50 for (int i = 0; i < num_particles; ++i) {
51 Particles[i].x = x_ + start_dist_x(Random);
52 Particles[i].y = y_ + start_dist_y(Random);
53 Particles[i].theta = lemlib::radToDeg(theta_) + start_dist_theta(Random);
54 Particles[i].weight = 1.0;
55 }
56
57 particle_mutex.give();
58 }

References particle_mutex, Particles(), and Random().

Variable Documentation

◆ activeSensors

vector<MCLDistanceSensor> MCL::activeSensors

Definition at line 29 of file MCL.cpp.

Referenced by MonteCarlo().

◆ Deviation

double MCL::Deviation

Definition at line 15 of file MCL.cpp.

Referenced by MonteCarlo().

◆ field_

Field MCL::field_

Definition at line 28 of file MCL.cpp.

Referenced by MonteCarlo().

◆ gear_ratio

const float MCL::gear_ratio = 4.0f / 3.0f

Definition at line 168 of file MCL.cpp.

◆ LOOP_DELAY_MS

const double MCL::LOOP_DELAY_MS = 20.0

Definition at line 37 of file MCL.cpp.

Referenced by MonteCarlo().

◆ LOOP_DT_SEC

const double MCL::LOOP_DT_SEC = LOOP_DELAY_MS / 1000.0

Definition at line 38 of file MCL.cpp.

Referenced by MonteCarlo().

◆ particle_mutex

pros::Mutex MCL::particle_mutex

Definition at line 31 of file MCL.cpp.

Referenced by fusion_loop_fn(), MCL_reset(), MonteCarlo(), and StartMCL().

◆ rpm_to_ips_factor

const float MCL::rpm_to_ips_factor = (wheel_circumference / gear_ratio) / 60.0f

Definition at line 169 of file MCL.cpp.

Referenced by getAvgVelocity().

◆ Sensors

vector<MCLDistanceSensor> MCL::Sensors
Initial value:
= {
MCLDistanceSensor(frontDistance, Point(-3, -0.75), FRONT),
MCLDistanceSensor(rightDistance, Point(6.3, -0.5), RIGHT),
MCLDistanceSensor(leftDistance, Point(-6.4, -0.5), LEFT),
MCLDistanceSensor(backDistance, Point(-3, -10.5), BACK),
}
backDistance
pros::Distance backDistance(11)
frontDistance
pros::Distance frontDistance(13)
leftDistance
pros::Distance leftDistance(12)
rightDistance
pros::Distance rightDistance(6)

Definition at line 21 of file MCL.cpp.

21 {
22 MCLDistanceSensor(frontDistance, Point(-3, -0.75), FRONT),
23 MCLDistanceSensor(rightDistance, Point(6.3, -0.5), RIGHT),
24 MCLDistanceSensor(leftDistance, Point(-6.4, -0.5), LEFT),
25 MCLDistanceSensor(backDistance, Point(-3, -10.5), BACK),
26 };

Referenced by MonteCarlo().

◆ start_theta

double MCL::start_theta

Definition at line 17 of file MCL.cpp.

◆ theta

double MCL::theta = 0

Definition at line 10 of file MCL.cpp.

◆ Velo

double MCL::Velo = 0

Definition at line 14 of file MCL.cpp.

Referenced by MonteCarlo().

◆ weights_sum

double MCL::weights_sum

Definition at line 16 of file MCL.cpp.

Referenced by MonteCarlo().

◆ wheel_circumference

const float MCL::wheel_circumference = (float)lemlib::Omniwheel::NEW_325 * M_PI

Definition at line 167 of file MCL.cpp.

◆ X

double MCL::X = 0

Definition at line 10 of file MCL.cpp.

Referenced by fusion_loop_fn(), MCL_reset(), and MonteCarlo().

◆ Y

double MCL::Y = 0

Definition at line 10 of file MCL.cpp.

Referenced by fusion_loop_fn(), MCL_reset(), and MonteCarlo().