comments for Doxygen all private Libs
This commit is contained in:
@@ -0,0 +1,201 @@
|
||||
/**
|
||||
* @file motorControl.cpp
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Implemention of the class motorControl.h.
|
||||
* @see motorControl.h
|
||||
* @version 0.1
|
||||
* @date 2021-12-13
|
||||
*
|
||||
* @copyright Copyright (c) 2021
|
||||
*
|
||||
*/
|
||||
|
||||
#include "motorControl.h"
|
||||
|
||||
MotorControl::MotorControl() {
|
||||
this->setMinPwm(PWMMIN);
|
||||
this->setMaxPwm(PWMMAX);
|
||||
}
|
||||
|
||||
void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2) {
|
||||
this->pwm_pin = pwm_pin;
|
||||
this->pwm_channel = pwm_channel;
|
||||
this->dir_1 = dir_1;
|
||||
this->dir_2 = dir_2;
|
||||
|
||||
pinMode(this->dir_1, OUTPUT);
|
||||
pinMode(this->dir_2, OUTPUT);
|
||||
|
||||
digitalWrite(this->dir_1, LOW);
|
||||
digitalWrite(this->dir_2, LOW);
|
||||
|
||||
ledcSetup(this->pwm_channel, PWMFREQ, this->pwm_res);
|
||||
ledcAttachPin(this->pwm_pin, this->pwm_channel);
|
||||
ledcWrite(this->pwm_channel, 0);
|
||||
}
|
||||
|
||||
void MotorControl::loop() {
|
||||
static uint32_t last_millis = 0;
|
||||
uint32_t time = millis();
|
||||
|
||||
//Cancel if delay is not reached
|
||||
if (time - last_millis < delay) {
|
||||
return;
|
||||
}
|
||||
runMotorControl();
|
||||
last_millis = time;
|
||||
}
|
||||
|
||||
void MotorControl::runMotorControl() {
|
||||
// Absolute difference between target_power and speed
|
||||
uint8_t abs_difference = abs(this->target_power - this->power);
|
||||
|
||||
// Difference between target_power and speed
|
||||
int16_t difference = this->target_power - this->power;
|
||||
|
||||
// Check that the target speed is close to 0 and that the abs_difference is lower than powersteps
|
||||
if (abs(this->target_power) < powersteps && abs_difference < powersteps) {
|
||||
this->setRealPower(0);
|
||||
return;
|
||||
}
|
||||
|
||||
// Correct speed
|
||||
if (abs_difference < powersteps) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Positive or negative tagret speed
|
||||
if (this->target_power >= 0) {
|
||||
// Positive or negative speed
|
||||
if (this->power >= 0) {
|
||||
if (difference > 0) {
|
||||
this->increasePower(powersteps);
|
||||
} else {
|
||||
this->increasePower(-powersteps);
|
||||
}
|
||||
} else {
|
||||
this->increasePower(powersteps);
|
||||
}
|
||||
|
||||
} else {
|
||||
// Positive or negative speed
|
||||
if (this->power >= 0) {
|
||||
this->increasePower(-powersteps);
|
||||
} else {
|
||||
if (difference > 0) {
|
||||
this->increasePower(powersteps);
|
||||
} else {
|
||||
this->increasePower(-powersteps);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MotorControl::setMinPwm(uint8_t min) {
|
||||
if (min > 80) min = 80;
|
||||
//transform percentage to real pwm value
|
||||
min = (uint8_t) ((1 >> pwm_res - 1) * (min / 100));
|
||||
this->dutycycle_min = min;
|
||||
}
|
||||
|
||||
void MotorControl::setMaxPwm(uint8_t max) {
|
||||
if (max > 100) max = 100;
|
||||
//transform percentage to real pwm value
|
||||
max = (uint8_t) ((1 >> pwm_res - 1) * (max / 100));
|
||||
this->dutycycle_max = max;
|
||||
}
|
||||
|
||||
uint16_t MotorControl::setPowerSteps(uint8_t increment) {
|
||||
this->powersteps = increment;
|
||||
return (uint16_t) (delay * ( 100 / powersteps ));
|
||||
}
|
||||
|
||||
void MotorControl::setTargetPower(int8_t power) {
|
||||
if (power <= 100 && power >= -100) {
|
||||
this->target_power = power;
|
||||
} else {
|
||||
char str[64];
|
||||
sprintf(str, "Invalid Argument (power: %d) in setTargetPower!", power);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t MotorControl::setDelay(uint8_t delay) {
|
||||
this->delay = delay;
|
||||
return (uint16_t) (delay * ( 100 / powersteps ));
|
||||
}
|
||||
|
||||
void MotorControl::stop() {
|
||||
this->target_power = 0;
|
||||
}
|
||||
|
||||
void MotorControl::emergencyStop() {
|
||||
setRealPower(0);
|
||||
}
|
||||
|
||||
int8_t MotorControl::getPower() {
|
||||
return this->power;
|
||||
}
|
||||
|
||||
int8_t MotorControl::getTargetPower() {
|
||||
return this->target_power;
|
||||
}
|
||||
|
||||
bool MotorControl::isTargetPowerReached() {
|
||||
if (this->target_power == this->power)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MotorControl::isAccelerationPositive() {
|
||||
if (power < target_power)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MotorControl::isAccelerationNegative() {
|
||||
if (power > target_power)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
void MotorControl::setRealPower(int8_t power) {
|
||||
//TODO: Exceptionhandling
|
||||
if (power <= 100 || power <= -100) {
|
||||
this->power = power;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
if (power == 0) {
|
||||
this->direction = 0;
|
||||
digitalWrite(this->dir_1, LOW);
|
||||
digitalWrite(this->dir_2, LOW);
|
||||
ledcWrite(this->pwm_channel, 0);
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycle_min, this->dutycycle_max);
|
||||
|
||||
if ((this->direction == 1 || this->direction == 0) && power < 0){ // new direction backward
|
||||
this->direction = 2;
|
||||
digitalWrite(this->dir_1, LOW);
|
||||
digitalWrite(this->dir_2, HIGH);
|
||||
} else if ((this->direction == 2 || this->direction == 0) && power > 0){ // new direction forward
|
||||
this->direction = 1;
|
||||
digitalWrite(this->dir_1, HIGH);
|
||||
digitalWrite(this->dir_2, LOW);
|
||||
}
|
||||
|
||||
ledcWrite(this->pwm_channel, pwm_val);
|
||||
}
|
||||
|
||||
void MotorControl::increasePower(int8_t power) {
|
||||
//TODO: Exceptionhandling
|
||||
//TODO: make a stop befor a direction change
|
||||
if (abs(power) > 2 * powersteps) {
|
||||
Serial.println("Invalid Argument in MotorControl::increasePower");
|
||||
return;
|
||||
}
|
||||
|
||||
this->setRealPower(this->power + power);
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
/**
|
||||
* @file motorControl.h
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Inherits a class to control a motor with pwm signal.
|
||||
* @version 0.1
|
||||
* @date 2021-12-09
|
||||
*
|
||||
* @copyright Copyright (c) 2021
|
||||
*
|
||||
*/
|
||||
#ifndef MOTOR_CONTROL_H
|
||||
#define MOTOR_CONTROL_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <Arduino.h>
|
||||
|
||||
#define DELAY 10
|
||||
#define PWMFREQ 16000
|
||||
#define PWMRES 8
|
||||
#define POWERSTEPS 2 // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
|
||||
#define PWMMIN 50
|
||||
#define PWMMAX 95 // Max 98% of 2^PWM_RES
|
||||
|
||||
/**
|
||||
* @brief A class which use PWM to control the power of DC Motor
|
||||
* You can control the acceleration of the motor, for example to
|
||||
* prevent a damage on your H-Bridge.
|
||||
*/
|
||||
class MotorControl {
|
||||
public:
|
||||
MotorControl();
|
||||
|
||||
/**
|
||||
* @brief Initalize the motorcontroler
|
||||
*
|
||||
* @param pwm_pin The output pin for the signal on the esp.
|
||||
* @param pwm_channel One of the pwm channels from the esp.
|
||||
* @param dir_1 First direction pin for the H-Bridge.
|
||||
* @param dir_2 Second direction pin for the H-Bridge.
|
||||
*/
|
||||
void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2);
|
||||
|
||||
/**
|
||||
* @brief Calls runMotorControl() to update the pwm signal
|
||||
*
|
||||
* This function should be called every mainloop. If the delay is not reached, than the
|
||||
* functions returns immediately.
|
||||
* @see runMotorControl()
|
||||
* @see DELAY
|
||||
*/
|
||||
void loop();
|
||||
|
||||
/**
|
||||
* @brief Normaly called repeatedly by loop() to update the pwm signal.
|
||||
*
|
||||
* Checks the difference between target power and current power to
|
||||
* increase or decrease the duty cycle. The amount of decrease or increase
|
||||
* is set by setPowerSetps() (default = 2).
|
||||
*/
|
||||
void runMotorControl();
|
||||
|
||||
/**
|
||||
* @brief Set the minimum duty cycle
|
||||
*
|
||||
* @param min duty cycle in percent
|
||||
*/
|
||||
void setMinPwm(uint8_t min);
|
||||
|
||||
/**
|
||||
* @brief Set the maximum duty cycle
|
||||
*
|
||||
* @param max duty cycle in percent
|
||||
*/
|
||||
void setMaxPwm(uint8_t max);
|
||||
|
||||
/**
|
||||
* @brief Set the Power Steps
|
||||
*
|
||||
* Set the increment of the steps with which the dutycycle is
|
||||
* increased or decreased. Note the dependency between the increment
|
||||
* and delay().
|
||||
*
|
||||
* The formula for the time between 0% and 100% power is:
|
||||
* time[ms] = delay * ( 100 / increment )
|
||||
* 500 ms are recommended
|
||||
*
|
||||
* @see setDelay()
|
||||
*
|
||||
* @param increment
|
||||
* @return time from 0% power to 100% power in Milliseconds
|
||||
*/
|
||||
uint16_t setPowerSteps(uint8_t increment);
|
||||
|
||||
/**
|
||||
* @brief Set the Target Power
|
||||
*
|
||||
* @param power power in percent
|
||||
*/
|
||||
void setTargetPower(int8_t power);
|
||||
|
||||
/**
|
||||
* @brief Set the min delay between each loop
|
||||
*
|
||||
* Note the dependency between delay and
|
||||
* setPowerSteps().
|
||||
*
|
||||
* @see setPowerSteps()
|
||||
*
|
||||
* @param delay time in Milliseconds
|
||||
* @return time from 0% power to 100% power in Milliseconds
|
||||
*/
|
||||
uint16_t setDelay(uint8_t delay);
|
||||
|
||||
/**
|
||||
* @brief Stops the motor like setTargetPower() to 0
|
||||
*
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* @brief Stops the motor immediately
|
||||
*
|
||||
*/
|
||||
void emergencyStop();
|
||||
|
||||
/**
|
||||
* @brief Get the current power
|
||||
*
|
||||
* @return int8_t percent of power (-100 to 100)
|
||||
*/
|
||||
int8_t getPower();
|
||||
|
||||
/**
|
||||
* @brief Get the target power
|
||||
*
|
||||
* @return int8_t percent of power (-100 to 100)
|
||||
*/
|
||||
int8_t getTargetPower();
|
||||
bool isTargetPowerReached();
|
||||
bool isAccelerationPositive();
|
||||
bool isAccelerationNegative();
|
||||
|
||||
private:
|
||||
void setRealPower(int8_t power);
|
||||
void increasePower(int8_t power);
|
||||
|
||||
int8_t target_power = 0;
|
||||
int8_t power = 0;
|
||||
uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
|
||||
|
||||
uint8_t pwm_pin;
|
||||
uint8_t pwm_channel;
|
||||
uint8_t pwm_res = PWMRES;
|
||||
uint8_t dutycycle_min;
|
||||
uint8_t dutycycle_max;
|
||||
uint8_t dir_1;
|
||||
uint8_t dir_2;
|
||||
uint8_t delay = DELAY;
|
||||
uint8_t powersteps = POWERSTEPS;
|
||||
|
||||
};
|
||||
|
||||
#endif // MOTOR_CONTROL_H
|
||||
@@ -0,0 +1,114 @@
|
||||
/**
|
||||
* @file speedometer.cpp
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Implemention of the class speedometer.h
|
||||
* @see speedometer.h
|
||||
* @version 0.1
|
||||
* @date 2021-12-13
|
||||
*
|
||||
* @copyright Copyright (c) 2021
|
||||
*
|
||||
*/
|
||||
#include "speedometer.h"
|
||||
|
||||
Speedometer::Speedometer() {}
|
||||
|
||||
void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg) {
|
||||
this->init(pinA, pinB, diameter, steps);
|
||||
this->bufSize = numOfValForAvg;
|
||||
}
|
||||
|
||||
void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps) {
|
||||
ESP32Encoder::useInternalWeakPullResistors=DOWN;
|
||||
this->encoder.attachFullQuad(pinA, pinB);
|
||||
this->diameter = diameter;
|
||||
this->steps = steps;
|
||||
|
||||
initAvgBuf();
|
||||
|
||||
this->isInit = true;
|
||||
}
|
||||
|
||||
void Speedometer::loop() {
|
||||
static uint32_t last_millis = 0;
|
||||
uint32_t time = millis();
|
||||
|
||||
//Cancel if delay is not reached
|
||||
if (time - last_millis < delay) {
|
||||
return;
|
||||
}
|
||||
runSpeedometer();
|
||||
last_millis = time;
|
||||
}
|
||||
|
||||
void Speedometer::runSpeedometer() {
|
||||
static uint32_t last_millis = 0;
|
||||
uint32_t time = millis();
|
||||
|
||||
uint16_t elapsed_time = time - last_millis;
|
||||
last_millis = time;
|
||||
|
||||
int16_t count = encoder.getCount();
|
||||
this->addValToBuf(count);
|
||||
encoder.clearCount();
|
||||
|
||||
uint16_t count_abs = abs(this->calcAverage()); // Absolute time in milliseconds
|
||||
double n = (double)count_abs / steps; // Wheel revolutions in absolute time
|
||||
double u = (double)n / ((double)elapsed_time / 1000); // Wheel revolutions per second
|
||||
double ms = u * (diameter * PI); // Speed in m/s
|
||||
|
||||
if (count > 0) {
|
||||
this->speed = ms;
|
||||
} else if (count < 0) {
|
||||
this->speed = ms * (-1);
|
||||
} else {
|
||||
this->speed = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void Speedometer::setNumOfValForAvg(uint8_t val) {
|
||||
this->bufSize = val;
|
||||
if (this->isInit)
|
||||
updateAvgBufSize();
|
||||
}
|
||||
|
||||
void Speedometer::setEncFilter(uint16_t val) {
|
||||
if (val > 1023) val = 1023;
|
||||
this->encoder.setFilter(val);
|
||||
}
|
||||
|
||||
void Speedometer::setDelay(uint8_t delay) {
|
||||
this->delay = delay;
|
||||
}
|
||||
|
||||
double Speedometer::getSpeed() {
|
||||
return this->speed;
|
||||
}
|
||||
|
||||
void Speedometer::initAvgBuf() {
|
||||
this->buf = new int16_t[bufSize];
|
||||
for (uint8_t i = 0; i < bufSize; i++)
|
||||
this->buf[i] = 0;
|
||||
}
|
||||
|
||||
void Speedometer::addValToBuf(int16_t val) {
|
||||
static uint8_t bufPos = 0;
|
||||
this->buf[bufPos] = val;
|
||||
bufPos++;
|
||||
|
||||
if (bufPos == bufSize) {
|
||||
bufPos = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void Speedometer::updateAvgBufSize() {
|
||||
delete[] this->buf;
|
||||
initAvgBuf();
|
||||
}
|
||||
|
||||
int16_t Speedometer::calcAverage() {
|
||||
int16_t sum = 0;
|
||||
for (int i = 0; i < bufSize; i++)
|
||||
sum += this->buf[i];
|
||||
return sum / bufSize;
|
||||
}
|
||||
@@ -0,0 +1,120 @@
|
||||
/**
|
||||
* @file speedometer.h
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief A complete implementation to measure wheel speeds with an encoder.
|
||||
* @version 0.1
|
||||
* @date 2021-12-09
|
||||
*
|
||||
* @copyright Copyright (c) 2021
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef SPEEDOMETER_H
|
||||
#define SPEEDOMETER_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <ESP32Encoder.h>
|
||||
|
||||
/**
|
||||
* @brief The default size of numbers to be taken in account for the average.
|
||||
*
|
||||
*/
|
||||
#define BUFSIZE 10
|
||||
|
||||
/**
|
||||
* @brief Default value for min Millisseconds between each loop
|
||||
* @see setDelay(uint8_t val)
|
||||
*/
|
||||
#define DELAY 30
|
||||
#define PI 3.1415926535897932384626433832795
|
||||
|
||||
/**
|
||||
* @brief A class which use a encoder to calc the speed
|
||||
*
|
||||
* This class use ESP32 pulse counter hardware peripheral.
|
||||
* The calclutaed speed is the average of an amount of last measurments.
|
||||
*
|
||||
*/
|
||||
class Speedometer {
|
||||
public:
|
||||
Speedometer();
|
||||
|
||||
/**
|
||||
* @brief Initalize the speedometer
|
||||
*
|
||||
* @param pinA Pin on the Esp from the encoder.
|
||||
* @param pinB Pin on the Esp from the encoder.
|
||||
* @param diameter Diameter of the wheel in meters.
|
||||
* @param steps Encodersteps for a complete wheel rotation.
|
||||
* @param numOfValForAvg Number of last values to be taken into account for the average.
|
||||
*/
|
||||
void init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg);
|
||||
void init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps);
|
||||
|
||||
/**
|
||||
* @brief Calls runSpeedometer() to update all Values.
|
||||
*
|
||||
* This function should be called every mainloop. If the delay is not reached, than the
|
||||
* functions returns immediately.
|
||||
* @see runSpeedometer()
|
||||
* @see DELAY
|
||||
*/
|
||||
void loop();
|
||||
|
||||
/**
|
||||
* @brief Noramly called repeatedly by loop() to calcluate new values.
|
||||
*
|
||||
* Add a new Value to the average and update the speed.
|
||||
*/
|
||||
void runSpeedometer();
|
||||
|
||||
/**
|
||||
* @brief Set the number of last values to be taken into account for the average.
|
||||
*
|
||||
* @param val length of the array
|
||||
*/
|
||||
void setNumOfValForAvg(uint8_t val);
|
||||
|
||||
/**
|
||||
* @brief Set the Enc Filter to prevent bouncing
|
||||
*
|
||||
* @param val default = 250, max = 1023
|
||||
*/
|
||||
void setEncFilter(uint16_t val);
|
||||
|
||||
/**
|
||||
* @brief Set the min delay between each loop
|
||||
*
|
||||
* @param val time in Milliseconds
|
||||
*/
|
||||
void setDelay(uint8_t delay);
|
||||
|
||||
/**
|
||||
* @brief Get the calculated speed of the Wheel
|
||||
*
|
||||
* @return double speed in m/s
|
||||
*/
|
||||
double getSpeed();
|
||||
|
||||
|
||||
private:
|
||||
void initAvgBuf();
|
||||
void addValToBuf(int16_t val);
|
||||
void updateAvgBufSize();
|
||||
int16_t calcAverage();
|
||||
|
||||
ESP32Encoder encoder;
|
||||
|
||||
bool isInit = false;
|
||||
|
||||
double speed = 0;
|
||||
double diameter;
|
||||
|
||||
uint8_t bufSize = BUFSIZE;
|
||||
uint16_t steps;
|
||||
uint8_t delay = DELAY;
|
||||
|
||||
int16_t *buf;
|
||||
};
|
||||
|
||||
#endif // SPEEDOMETER_H
|
||||
Reference in New Issue
Block a user