179 lines
4.5 KiB
C++
179 lines
4.5 KiB
C++
/**
|
|
* @file moveControl.h
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief Contains the MoveControl class
|
|
* @version 0.1
|
|
* @date 2021-12-14
|
|
*
|
|
* @copyright Copyright (c) 2021
|
|
*
|
|
*/
|
|
#ifndef MOVE_CONTROL_H
|
|
#define MOVE_CONTROL_H
|
|
|
|
#include <cstdint>
|
|
#include <iostream>
|
|
#include <PID_v1.h>
|
|
#include <Arduino.h>
|
|
|
|
#include "motorControl.h"
|
|
#include "speedometer.h"
|
|
#include "config.h"
|
|
#include "debugTimes.h"
|
|
#include "component.h"
|
|
|
|
|
|
/**
|
|
* @brief This class manages the motors and the encoders
|
|
*
|
|
* The class uses two PIDs to control the motors. The PIDs
|
|
* use the Speedometer class to get the current speed and the
|
|
* given target speed to calculate a new duty cycle for the motors.
|
|
*
|
|
*/
|
|
class MoveControl : public Component {
|
|
public:
|
|
/**
|
|
* @brief used to set driving status
|
|
*
|
|
* When set to stop all motors are set to halt
|
|
*
|
|
*/
|
|
enum Status {Stop,
|
|
Drive,
|
|
Raw};
|
|
|
|
|
|
/**
|
|
* @brief Construct a new Move Control object
|
|
*
|
|
* Initialize the motors, encoders and PIDs with given
|
|
* values in moveControlConfig.h
|
|
*/
|
|
MoveControl();
|
|
|
|
/**
|
|
* @brief Destroy the Move Control object
|
|
*
|
|
* Stops the motors with emergencyStop()
|
|
*/
|
|
~MoveControl();
|
|
|
|
/**
|
|
* @brief Set the Status
|
|
*
|
|
* @see Status
|
|
* @param status
|
|
*/
|
|
void setDrivingStatus(Status status);
|
|
|
|
/**
|
|
* @brief Stops the engine immediately
|
|
*
|
|
*/
|
|
void emergencyStop();
|
|
|
|
/**
|
|
* @brief Set the target speed
|
|
*
|
|
* If the given speed is close to zero, then the
|
|
* target speed is set to zero.
|
|
* @param speed in m/s
|
|
*/
|
|
void setSpeed(double speed);
|
|
|
|
/**
|
|
* @brief Set the rotationspeed
|
|
*
|
|
* If the given rotationspeed is close to zero, then the
|
|
* target rotationspeed is set to zero.
|
|
* @param speed rad/s
|
|
*/
|
|
void setRotationSpeed(double speed);
|
|
|
|
/**
|
|
* @brief Set Raw Power Left
|
|
*
|
|
* This value has only an effect if Status is raw.
|
|
*
|
|
* @param power between -100 and 100
|
|
*/
|
|
void setRawPowerLeft(int16_t power);
|
|
|
|
/**
|
|
* @brief Set the Raw Power Right
|
|
*
|
|
* This value has only an effect if Status is raw.
|
|
*
|
|
* @param power between -100 and 100
|
|
*/
|
|
void setRawPowerRight(int16_t power);
|
|
|
|
/**
|
|
* @brief Set the pid tunings
|
|
*
|
|
* @param side 0 -> left, 1 -> right
|
|
* @param p
|
|
* @param i
|
|
* @param d
|
|
*/
|
|
void setPidTunings(uint8_t side, double p, double i, double d);
|
|
|
|
/**
|
|
* @brief Returns the PID object of the choosen side.
|
|
*
|
|
* @param side 0 -> left, 1 -> right
|
|
* @return PID*
|
|
*/
|
|
PID* getPID(uint8_t side) const;
|
|
|
|
/**
|
|
* @brief Get the Speedometer Left object
|
|
*
|
|
* @return Speedometer*
|
|
*/
|
|
Speedometer* getSpeedometerLeft() const { return this->left_speedometer; }
|
|
|
|
/**
|
|
* @brief Get the Speedometer Right object
|
|
*
|
|
* @return Speedometer*
|
|
*/
|
|
Speedometer* getSpeedometerRight() const { return this->right_speedometer; }
|
|
|
|
uint16_t getDutycycleLeft() const { return this->left_motor->getDutycycle(); }
|
|
uint16_t getDutycycleRight() const { return this->right_motor->getDutycycle(); }
|
|
|
|
private:
|
|
void run() override;
|
|
void setSpeedometerDirection(Speedometer *speedometer, double value);
|
|
void calcTargetWheelSpeed();
|
|
void regulateMotors();
|
|
void updateCurrentWheelSpeed();
|
|
|
|
MotorControl *left_motor;
|
|
MotorControl *right_motor;
|
|
Speedometer *left_speedometer;
|
|
Speedometer *right_speedometer;
|
|
PID *left_pid;
|
|
PID *right_pid;
|
|
|
|
Status driving_status = Status::Stop;
|
|
|
|
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;
|
|
|
|
uint8_t overTimeCounter = 0;
|
|
uint8_t overTimeMax = 100;
|
|
int8_t rawPowerLeft = 0;
|
|
int8_t rawPowerRight = 0;
|
|
};
|
|
#endif // MOVE_CONTROL_H
|