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::ofstream* fs); // 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  int MOTOR_MAX_; // 250
26  int MOTOR_MIN_; // 0
27  int MOTOR_DEADBAND_; // = 9;
28 
29  double MAX_ACCEL_CUTOFF_; // 20
30  double MIN_VELOCITY_; // 0.04
31  double MAX_VELOCITY_; // 2.5ish?
32 
34 
35  // Can poll these values to see if motor speed is saturating
39 
40  //.csv Debuggin
41  std::ofstream* fs_;
42 
43  // General Class variables
44  double K_P_;
45  double K_I_;
46  double K_D_;
50 
51  // Returned value
52  int motor_speed_; // value between 0-250
53  unsigned char deadband_offset_;
54 
55  // velocity feedback
59  std::vector<double> velocity_filtered_history_;
60  std::vector<double> velocity_history_;
63 
64 private:
65  // Controller Functions
66  void velocityController();
67  double filter(double left_motor_vel, double dt);
68  bool hasZeroHistory(const std::vector<double>& vel_history);
69  int boundMotorSpeed(int motor_speed, int max, int min);
70  int deadbandOffset(int motor_speed, int deadband_offset);
71  double P(double error, double dt);
72  double I(double error, double dt);
73  double D(double error, double dt);
74  int PID(double error, double dt);
75  int feedThroughControl();
76 };
77 
78 }
bool hasZeroHistory(const std::vector< double > &vel_history)
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)
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 min(double a, double b)
std::vector< double > velocity_filtered_history_
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)
std::ofstream * fs_
double max(double a, double b)


rr_openrover_driver
Author(s): Jack Kilian
autogenerated on Thu Sep 10 2020 03:38:38