odom_control.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <vector>
5 #include <string>
6 
8 
9 namespace openrover
10 {
12 {
13 public:
14  OdomControl(); // default
15 
16  OdomControl(bool use_control, PidGains pid_gains, int max, int min,
17  std::string log_filename); // max min values for returned value
18  OdomControl(bool use_control, PidGains pid_gains, int max, int min); // max min values for returned value
19 
20  unsigned char run(bool e_stop_on, bool control_on, double commanded_vel, double measured_vel,
21  double dt); // in m/s
22  void start(bool use_control, PidGains pid_gains, int max, int min);
23  void reset();
24 
25  const int MOTOR_MAX_; // 250
26  const int MOTOR_MIN_; // 0
27  const int MOTOR_DEADBAND_; // = 9;
28 
29  const double MAX_ACCEL_CUTOFF_; // 20
30  const double MIN_VELOCITY_; // 0.04
31  const double MAX_VELOCITY_; // 2.5ish?
32 
35 
36  // Can poll these values to see if motor speed is saturating
40 
41  std::string log_filename_;
42 
43  //.csv Debuggin
44  std::ofstream fs_;
45 
46  // General Class variables
47  double K_P_;
48  double K_I_;
49  double K_D_;
52 
53  // Returned value
54  int motor_speed_; // value between 0-250
55  unsigned char deadband_offset_;
56 
57  // velocity feedback
61  std::vector<double> velocity_history_;
64 
65 private:
66  // Controller Functions
67  void velocityController();
68  double filter(double left_motor_vel, double dt);
69  bool hasZeroHistory(const std::vector<double>& vel_history);
70  int boundMotorSpeed(int motor_speed, int max, int min);
71  int deadbandOffset(int motor_speed, int deadband_offset);
72  double P(double error, double dt);
73  double I(double error, double dt);
74  double D(double error, double dt);
75  int PID(double error, double dt);
76  int feedThroughControl();
77 };
78 
79 }
bool hasZeroHistory(const std::vector< double > &vel_history)
const double MIN_VELOCITY_
int deadbandOffset(int motor_speed, int deadband_offset)
unsigned char deadband_offset_
double D(double error, double dt)
std::vector< double > velocity_history_
int PID(double error, double dt)
const double MAX_VELOCITY_
int boundMotorSpeed(int motor_speed, int max, int min)
double I(double error, double dt)
void start(bool use_control, PidGains pid_gains, int max, int min)
double P(double error, double dt)
double filter(double left_motor_vel, double dt)
unsigned char run(bool e_stop_on, bool control_on, double commanded_vel, double measured_vel, double dt)
const double MAX_ACCEL_CUTOFF_
int min(int a, int b)


rr_openrover_basic
Author(s): Jack Kilian
autogenerated on Fri Jan 17 2020 03:18:17