181 lines
4.8 KiB
C++
181 lines
4.8 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 "moveControlConfig.h"
|
|
#include "debugTimes.h"
|
|
|
|
/**
|
|
* @brief used to set driving status
|
|
*
|
|
* When set to stop all motors are set to halt
|
|
*
|
|
*/
|
|
enum DrivingStatus {stop,
|
|
drive,
|
|
raw};
|
|
|
|
/**
|
|
* @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:
|
|
/**
|
|
* @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 Calls runMoveControl() to update all Values.
|
|
*
|
|
* Besides that this function calls the loop() functions for the motors and encodes.
|
|
* This function should be called every mainloop. If the delay is not reached, than the
|
|
* functions returns immediately.
|
|
* @see runMoveControl()
|
|
* @see setDelay()
|
|
*/
|
|
void loop();
|
|
|
|
/**
|
|
* @brief Normally called repeatedly by loop() to calculate new values.
|
|
*/
|
|
void runMoveControl();
|
|
|
|
/**
|
|
* @brief Set the DrivingStatus
|
|
*
|
|
* @see DrivingStatus
|
|
* @param status
|
|
*/
|
|
void setDrivingStatus(DrivingStatus status);
|
|
|
|
/**
|
|
* @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);
|
|
|
|
void setRawPowerLeft(int16_t power);
|
|
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);
|
|
Speedometer* getSpeedometerLeft() const { return this->left_speedometer; }
|
|
Speedometer* getSpeedometerRight() const { return this->right_speedometer; }
|
|
|
|
/**
|
|
* @brief Set the min delay between each loop
|
|
*
|
|
* @param delay_ time in Milliseconds
|
|
*/
|
|
void setDelay(uint8_t delay) { this->delay = delay; }
|
|
|
|
private:
|
|
/**
|
|
* @brief Converts values into wheel speeds
|
|
*
|
|
* Converts target speed and target rotationspeed into
|
|
* wheel speeds
|
|
*/
|
|
void calcTargetWheelSpeed();
|
|
|
|
/**
|
|
* @brief Set the target power to motors
|
|
*
|
|
* Checks if the driving_status is set to drive.
|
|
* If yes, then the motors get the pid_out values as targetpower.
|
|
* If no, then the motors target power is set to zero.
|
|
*/
|
|
void regulateMotors();
|
|
|
|
/**
|
|
* @brief Updates the wheel speeds with speedometer
|
|
*/
|
|
void updateCurrentWheelSpeed();
|
|
|
|
MotorControl *left_motor;
|
|
MotorControl *right_motor;
|
|
Speedometer *left_speedometer;
|
|
Speedometer *right_speedometer;
|
|
PID *left_pid;
|
|
PID *right_pid;
|
|
|
|
DrivingStatus driving_status = DrivingStatus::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 delay = 20;
|
|
uint8_t overTimeCounter = 0;
|
|
uint8_t overTimeMax = 100;
|
|
int8_t rawPowerLeft = 0;
|
|
int8_t rawPowerRight = 0;
|
|
uint32_t lastMillis = 0;
|
|
};
|
|
#endif // MOVE_CONTROL_H
|