135 lines
3.3 KiB
C++
135 lines
3.3 KiB
C++
#include "DRV8833.h"
|
|
|
|
#include <Arduino.h>
|
|
|
|
namespace RoboidControl {
|
|
namespace Arduino {
|
|
|
|
#pragma region DRV8833
|
|
|
|
DRV8833::DRV8833(Configuration config, Thing& parent) : Thing(parent) {
|
|
this->pinStandby = config.standby;
|
|
if (pinStandby != 255)
|
|
pinMode(pinStandby, OUTPUT);
|
|
|
|
this->motorA = new DRV8833Motor(*this, config.AIn1, config.AIn2);
|
|
this->motorA->SetName("Motor A");
|
|
this->motorB = new DRV8833Motor(*this, config.BIn1, config.BIn2);
|
|
this->motorB->SetName("Motor B");
|
|
}
|
|
|
|
#pragma endregion DRV8833
|
|
|
|
#pragma region Differential drive
|
|
|
|
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
|
|
Thing& parent)
|
|
: RoboidControl::DifferentialDrive(parent), drv8833(config) {
|
|
this->leftWheel = this->drv8833.motorA;
|
|
this->rightWheel = this->drv8833.motorB;
|
|
}
|
|
|
|
void DRV8833::DifferentialDrive::Update(bool recurse) {
|
|
RoboidControl::DifferentialDrive::Update(recurse);
|
|
this->drv8833.Update(false);
|
|
}
|
|
|
|
#pragma endregion Differential drive
|
|
|
|
#pragma region Motor
|
|
|
|
#if (ESP32)
|
|
uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
|
|
#endif
|
|
|
|
DRV8833Motor::DRV8833Motor(DRV8833& driver,
|
|
unsigned char pinIn1,
|
|
unsigned char pinIn2,
|
|
bool reverse)
|
|
: Motor() {
|
|
this->SetParent(driver);
|
|
|
|
this->pinIn1 = pinIn1;
|
|
this->pinIn2 = pinIn2;
|
|
|
|
#if (ESP32)
|
|
in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
|
|
ledcSetup(in1Ch, 500, 8);
|
|
ledcAttachPin(pinIn1, in1Ch);
|
|
|
|
in2Ch = DRV8833Motor::nextAvailablePwmChannel++;
|
|
ledcSetup(in2Ch, 500, 8);
|
|
ledcAttachPin(pinIn2, in2Ch);
|
|
|
|
#else
|
|
pinMode(pinIn1, OUTPUT); // configure the in1 pin to output mode
|
|
pinMode(pinIn2, OUTPUT); // configure the in1 pin to output mode
|
|
#endif
|
|
|
|
// this->reverse = reverse;
|
|
}
|
|
|
|
// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
|
|
// this->maxRpm = rpm;
|
|
// }
|
|
|
|
void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
|
|
Motor::SetTargetVelocity(motorSpeed);
|
|
|
|
uint8_t motorSignal =
|
|
(uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
|
|
|
|
// std::cout << "moto speed " << this->name << " = " << motorSpeed
|
|
// << ", motor signal = " << (int)motorSignal << "\n";
|
|
|
|
#if (ESP32)
|
|
if (motorSpeed == 0) { // stop
|
|
ledcWrite(in1Ch, 0);
|
|
ledcWrite(in2Ch, 0);
|
|
|
|
} else if (motorSpeed > 0) { // forward
|
|
#if FAST_DECAY
|
|
ledcWrite(in1Ch, motorSignal);
|
|
ledcWrite(in2Ch, 0);
|
|
#else
|
|
ledcWrite(in1Ch, 255);
|
|
ledcWrite(in2Ch, 255 - motorSignal);
|
|
#endif
|
|
} else { // (motorSpeed < 0) reverse
|
|
#if FAST_DECAY
|
|
ledcWrite(in1Ch, 0);
|
|
ledcWrite(in2Ch, motorSignal);
|
|
#else
|
|
ledcWrite(in1Ch, 255 - motorSignal);
|
|
ledcWrite(in2Ch, 255);
|
|
#endif
|
|
}
|
|
#else // not ESP32
|
|
if (motorSpeed == 0) { // stop
|
|
analogWrite(pinIn1, 0);
|
|
analogWrite(pinIn2, 0);
|
|
|
|
} else if (motorSpeed > 0) { // forward
|
|
#if FAST_DECAY
|
|
analogWrite(pinIn1, motorSignal);
|
|
analogWrite(pinIn2, 0);
|
|
#else
|
|
analogWrite(pinIn1, 255);
|
|
analogWrite(pinIn2, 255 - motorSignal);
|
|
#endif
|
|
} else { // (motorSpeed < 0) reverse
|
|
#if FAST_DECAY
|
|
analogWrite(pinIn1, 0);
|
|
analogWrite(pinIn2, motorSignal);
|
|
#else
|
|
analogWrite(pinIn1, 255 - motorSignal);
|
|
analogWrite(pinIn2, 255);
|
|
#endif
|
|
}
|
|
#endif
|
|
}
|
|
|
|
#pragma endregion Motor
|
|
|
|
} // namespace Arduino
|
|
} // namespace RoboidControl
|