119 lines
3.3 KiB
C++
119 lines
3.3 KiB
C++
#include "DRV8833.h"
|
|
|
|
#include <Arduino.h>
|
|
|
|
namespace RoboidControl {
|
|
|
|
DRV8833Motor::DRV8833Motor(Participant* participant,
|
|
unsigned char pinIn1,
|
|
unsigned char pinIn2,
|
|
RotationDirection direction)
|
|
: Thing(participant) {
|
|
this->pinIn1 = pinIn1;
|
|
this->pinIn2 = pinIn2;
|
|
|
|
#if (ESP32)
|
|
in1Ch = nextAvailablePwmChannel++;
|
|
ledcSetup(in1Ch, 500, 8);
|
|
ledcAttachPin(pinIn1, in1Ch);
|
|
in2Ch = 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->direction = direction;
|
|
}
|
|
|
|
void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
|
|
this->maxRpm = rpm;
|
|
}
|
|
|
|
void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
|
|
Thing::SetAngularVelocity(velocity);
|
|
// ignoring rotation axis for now.
|
|
// Spherical angularVelocity = this->GetAngularVelocity();
|
|
float angularSpeed = velocity.distance; // in degrees/sec
|
|
|
|
float rpm = angularSpeed / 360 * 60;
|
|
float motorSpeed = rpm / this->maxRpm;
|
|
|
|
uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
|
|
// if (direction == RotationDirection::CounterClockwise)
|
|
// speed = -speed;
|
|
// Determine the rotation direction
|
|
if (velocity.direction.horizontal.InDegrees() < 0)
|
|
motorSpeed = -motorSpeed;
|
|
|
|
std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm " << rpm
|
|
<< ", 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
|
|
}
|
|
|
|
DRV8833::DRV8833(Participant* participant,
|
|
unsigned char pinAIn1,
|
|
unsigned char pinAIn2,
|
|
unsigned char pinBIn1,
|
|
unsigned char pinBIn2,
|
|
unsigned char pinStandby,
|
|
DRV8833Motor::RotationDirection directionA,
|
|
DRV8833Motor::RotationDirection directionB)
|
|
: Thing(participant) {
|
|
this->pinStandby = pinStandby;
|
|
if (pinStandby != 255)
|
|
pinMode(pinStandby, OUTPUT);
|
|
|
|
this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, directionA);
|
|
this->motorA->name = "Motor A";
|
|
this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, directionB);
|
|
this->motorB->name = "Motor B";
|
|
}
|
|
|
|
} // namespace RoboidControl
|