kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

done with all changes for new encoder

Browse Source
This commit is contained in:
kleiax
2023-02-21 11:18:49 +01:00
parent 8bb38d560e
commit ba91da7ed3
7 changed files with 106 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/moveControlConfig.h
+2 -4
View File
@@ -13,14 +13,12 @@
#define M_DIR_11 12
#define M_DIR_12 27
#define M_PWM_1 13
#define M_ENCODE_1A 32
#define M_ENCODE_1B 33
#define M_ENCODE_LEFT 26
//Right Motor
#define M_DIR_21 23
#define M_DIR_22 14
#define M_PWM_2 22
#define M_ENCODE_2A 25
#define M_ENCODE_2B 26
#define M_ENCODE_RIGHT 12
// PID config
#define PID_OUT_MIN -100
lib/PulseCounter/pulseCounter.cpp
View File
lib/PulseCounter/pulseCounter.h
-27
View File
@@ -1,27 +0,0 @@
/**
* @file pulseCounter.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-02-18
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include <stdint.h.h>
#include <Arduino.h>
#include <esp32_pcnt.h>
class PulseCounter {
public:
PulseCounter(uint8_t pin, uint8_t unit);
private:
uint8_t pin;
uint8_t unit;
int16_t counter = 0;
};
lib/Speedometer/speedometer.cpp
+50 -31
View File
@@ -11,25 +11,18 @@
*/
#include "speedometer.h"
Speedometer::Speedometer() {}
Speedometer::~Speedometer() {
delete[] this->buf;
}
void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg) {
this->init(pinA, pinB, diameter, steps);
Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps, uint8_t numOfValForAvg) {
this->init(pin, 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;
Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) {
this->init(pin, diameter, steps);
}
initAvgBuf();
this->isInit = true;
Speedometer::~Speedometer() {
delete[] this->buf;
delete this->pulseCounter;
}
uint16_t Speedometer::loop() {
@@ -54,9 +47,27 @@ void Speedometer::runSpeedometer() {
uint16_t elapsed_time = time - last_millis_calc;
last_millis_calc = time;
int16_t count = encoder.getCount();
int16_t count = this->pulseCounter->get_value();
switch (this->currentDirection) {
case Direction::Forward :
this->addValToBuf(count);
this->encoder.clearCount();
break;
case Direction::Backward :
this->addValToBuf(-count);
break;
case Direction::None :
this->addValToBuf(0);
break;
default:
std::cout << "Wrong value in Speedometer::runSpeedometer" << std::endl;
break;
}
this->pulseCounter->clear();
this->pulseCounter->resume();
uint16_t count_abs = abs(this->calcAverage()); // Absolute time in milliseconds
double n = (double)count_abs / steps; // Wheel revolutions in absolute time
@@ -71,41 +82,49 @@ void Speedometer::runSpeedometer() {
this->speed = 0;
}
std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
// std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
}
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 delayLoop) {
this->delayLoop = delayLoop;
}
double Speedometer::getSpeed() {
return this->speed;
this->pulseCounter->set_filter_value(val);
}
void Speedometer::calibrationMeasurementStart() {
this->calibrationRunning = true;
this->encoder.clearCount();
this->pulseCounter->clear();
this->pulseCounter->resume();
}
uint16_t Speedometer::calibrationMeasurementStop() {
this->calibrationRunning = false;
uint16_t res = abs(this->encoder.getCount());
this->encoder.clearCount();
uint16_t res = abs(this->pulseCounter->get_value());
this->pulseCounter->clear();
this->pulseCounter->resume();
return res;
}
void Speedometer::init(uint8_t pin, double diameter, uint16_t steps) {
this->diameter = diameter;
this->steps = steps;
this->pulseCounter = new PulseCounter();
this->pulseCounter->initialise(pin, PCNT_PIN_NOT_USED);
this->pulseCounter->set_mode(PCNT_COUNT_INC, PCNT_COUNT_DIS, PCNT_MODE_KEEP, PCNT_MODE_KEEP);
this->pulseCounter->set_filter_value(1000); // ignore pulses less than 1000 x 2.5ns
this->pulseCounter->clear();
this->pulseCounter->resume();
initAvgBuf();
}
void Speedometer::initAvgBuf() {
this->buf = new int16_t[bufSize];
for (uint8_t i = 0; i < bufSize; i++)
lib/Speedometer/speedometer.h
+41 -13
View File
@@ -15,7 +15,7 @@
#include <Arduino.h>
#include <cstdint>
#include <iostream>
#include <ESP32Encoder.h>
#include <esp32_pcnt.h>
/**
* @brief The default size of numbers to be taken in account for the average.
@@ -39,20 +39,31 @@
*/
class Speedometer {
public:
Speedometer();
~Speedometer();
/**
* @brief Enum to control the direction.
*
* If the Direction is Forward, the internal counter counts up and a positiv speed will be returned.
* If the Direction is Backward, the internal counter counts down and a negativ speed will be returned.
* If the Dorection is None, no measurement will be taken.
*/
enum Direction {
None,
Forward,
Backward
};
/**
* @brief Initialize the speedometer
* @brief Construct a new Speedometer object
*
* @param pinA Pin on the Esp from the encoder.
* @param pinB Pin on the Esp from the encoder.
* @param pin 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);
Speedometer(uint8_t pin, double diameter, uint16_t steps, uint8_t numOfValForAvg);
Speedometer(uint8_t pin, double diameter, uint16_t steps);
~Speedometer();
/**
* @brief Calls runSpeedometer() to update all Values.
@@ -72,6 +83,13 @@ class Speedometer {
*/
void runSpeedometer();
/**
* @brief Set the direction
*
* @param dir Direction
*/
void setDirection(Direction dir) { this->currentDirection = dir; }
/**
* @brief Set the number of last values ​​to be taken into account for the average.
*
@@ -82,7 +100,9 @@ class Speedometer {
/**
* @brief Set the Enc Filter to prevent bouncing
*
* @param val default = 250, max = 1023
* ignore pulses less than val x 2.5ns
*
* @param val default = 1000, max = 1023
*/
void setEncFilter(uint16_t val);
@@ -91,14 +111,21 @@ class Speedometer {
*
* @param delayLoop time in Milliseconds
*/
void setDelay(uint8_t delayLoop);
void setDelay(uint8_t delayLoop) { this->delayLoop = delayLoop; }
/**
* @brief Get the Direction
*
* @return Direction
*/
Direction getDirection() { return this->currentDirection; }
/**
* @brief Get the calculated speed of the Wheel
*
* @return double speed in m/s
*/
double getSpeed();
double getSpeed() { return this->speed; }
/**
* @brief Start calibration
@@ -120,14 +147,15 @@ class Speedometer {
private:
void init(uint8_t pin, double diameter, uint16_t steps);
void initAvgBuf();
void addValToBuf(int16_t val);
void updateAvgBufSize();
int16_t calcAverage();
ESP32Encoder encoder;
PulseCounter* pulseCounter;
Direction currentDirection = Direction::None;
bool isInit = false;
bool calibrationRunning = false;
double speed = 0;
platformio.ini
+3 -2
View File
@@ -16,9 +16,7 @@ framework = arduino
monitor_speed = 115200
upload_speed = 921600
monitor_port = COM3
monitor_filters = esp32_exception_decoder
lib_deps =
madhephaestus/ESP32Encoder@^0.10.1
jvpernis/PS3 Controller Host@^1.1.0
sparkfun/SparkFun u-blox GNSS Arduino Library@^2.2.7
knolleary/PubSubClient@^2.8
@@ -27,15 +25,18 @@ lib_deps =
marian-craciunescu/ESP32Ping@^1.7
bblanchon/ArduinoJson@^6.20.0
mprograms/QMC5883LCompass@^1.1.1
mike-gofton/ESP32PulseCounter@^0.2.0
https://git.kleiax.de/PlatformIO-Libs/Menu.git#v1.0
upload_port = COM3
[env:release]
build_type = release
lib_deps = mike-gofton/ESP32PulseCounter@^0.2.0
[env:debug]
monitor_filters = esp32_exception_decoder
build_type = debug
check_tool = clangtidy
[platformio]
description = A Rover who should be drive a route by gps.
src/moveControl.cpp
+7 -5
View File
@@ -13,8 +13,8 @@
MoveControl::MoveControl() {
this->left_motor = new MotorControl();
this->right_motor = new MotorControl();
this->left_speedometer = new Speedometer;
this->right_speedometer = new Speedometer;
this->left_speedometer = new Speedometer(M_ENCODE_LEFT, WHEEL_DIAMETER, ENC_STEPS);
this->right_speedometer = new Speedometer(M_ENCODE_RIGHT, WHEEL_DIAMETER, ENC_STEPS);
this->left_pid = new PID( &this->wheelspeed_left,
&this->left_pid_out,
@@ -37,14 +37,16 @@ MoveControl::MoveControl() {
this->left_motor->init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
this->right_motor->init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
this->left_speedometer->init(M_ENCODE_1A, M_ENCODE_1B, WHEEL_DIAMETER, ENC_STEPS);
this->right_speedometer->init(M_ENCODE_2A, M_ENCODE_2B, WHEEL_DIAMETER, ENC_STEPS);
}
MoveControl::~MoveControl() {
this->left_motor->emergencyStop();
this->right_motor->emergencyStop();
delete this->left_motor;
delete this->right_motor;
delete this->left_speedometer;
delete this->right_speedometer;
}
void MoveControl::loop() {