57 lines
1.4 KiB
C++
57 lines
1.4 KiB
C++
#ifndef MOVE_CONTROL_H
|
|
#define MOVE_CONTROL_H
|
|
|
|
#include <cstdint>
|
|
#include <PID_v1.h>
|
|
|
|
#include "motorControl.h"
|
|
#include "speedometer"
|
|
#include "config.h"
|
|
#include "debugMqtt.h"
|
|
#include "debugTimes.h"
|
|
|
|
enum DrivingStatus {stop,
|
|
drive};
|
|
|
|
|
|
class MoveControl {
|
|
public:
|
|
MoveControl();
|
|
void init(MotorControl *left_motor, MotorControl *right_motor,
|
|
Speedometer *left_encoder, Speedometer *right_encoder);
|
|
void runMoveControl();
|
|
void setDrivingStatus(DrivingStatus status);
|
|
void setSpeed(double speed);
|
|
void setRotationspeed(double speed);
|
|
void setPidTunings(uint8_t side, double p, double i, double d);
|
|
|
|
private:
|
|
void calcWheelSpeed();
|
|
void regulateMotors();
|
|
void updateWheelSpeed();
|
|
|
|
DebugMqtt *debug;
|
|
|
|
MotorControl *left_motor;
|
|
MotorControl *right_motor;
|
|
Speedometer *left_speedometer;
|
|
Speedometer *right_speedometer;
|
|
|
|
DrivingStatus driving_status = DrivingStatus::stop;
|
|
|
|
PID *left_pid;
|
|
PID *right_pid;
|
|
|
|
double x_speed = 0;
|
|
double rotation_speed = 0;
|
|
|
|
double wheelspeed_left_target = 0;
|
|
double wheelspeed_right_target = 0;
|
|
double wheelspeed_left = 0;
|
|
double wheelspeed_right = 0;
|
|
double left_pid_out;
|
|
double right_pid_out;
|
|
|
|
};
|
|
|
|
#endif // MOVE_CONTROL_H
|