int estimate_track_puck_theta() {
int sensor_b_l = sensor_values[2];
int sensor_t_l = sensor_values[1];
int sensor_t_r = sensor_values[4];
int sensor_b_r = sensor_values[3];
switch (determine_quadrant()) {
case T_R:
return sensor_b_r - sensor_t_r;
case B_R:
return sensor_b_r - sensor_t_r;
case B_L:
return sensor_t_l + sensor_b_l;
case T_L:
return -(sensor_t_l + sensor_b_l);
}
return 0; // shouldn't happen.
}
Vector::Vector(double x1, double y1, double x2, double y2)
{
x_component_ = std::abs(x2 - x1);
y_component_ = std::abs(y2 - y1);
int quadrant = determine_quadrant(x1, y1, x2, y2);
magnitude_ = sqrt(pow(x_component_, 2) + pow(y_component_, 2));
double theta = radians_to_degrees(acos(x_component_ / magnitude_));
direction_ = determine_direction(quadrant, theta);
set_component_signs(direction_);
Logger::write(Logger::string_stream << "creating vector:");
Logger::write(Logger::string_stream << "\t\tmagnitude: " << magnitude_);
Logger::write(Logger::string_stream << "\t\tdirection: " << direction_);
Logger::write(Logger::string_stream << "\t\tx_component: " << x_component_);
Logger::write(Logger::string_stream << "\t\ty_component: " << y_component_);
Logger::write(Logger::string_stream << "\t\tquadrant: " << quadrant);
Logger::write(Logger::string_stream << "\t\t(" << x1 << "," << y1 << ") & (" << x2 << "," << y2 << ")");
}
