diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 3b3b0a1..1e36be5 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -1,13 +1,119 @@ #include "DRV8833.h" +#include + namespace RoboidControl { -DRV8833::DRV8833(unsigned char pinAIn1, +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, - RotationDirection directionA, - RotationDirection directionB) {} + 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 \ No newline at end of file diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index afea9e3..7ce556c 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -1,14 +1,37 @@ #pragma once #include "Thing.h" +#include "Things/DifferentialDrive.h" /// @brief Support for a DRV8833 motor controller namespace RoboidControl { -class DRV8833 { -public: + +class DRV8833Motor : public Thing { + public: /// @brief Motor turning direction enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 }; + /// @brief Setup the DC motor + /// @param pinIn1 the pin number for the in1 signal + /// @param pinIn2 the pin number for the in2 signal + /// @param direction the forward turning direction of the motor + DRV8833Motor(Participant* participant, + unsigned char pinIn1, + unsigned char pinIn2, + RotationDirection direction = RotationDirection::Clockwise); + void SetMaxRPM(unsigned int rpm); + + virtual void SetAngularVelocity(Spherical velocity) override; + + protected: + unsigned char pinIn1 = 255; + unsigned char pinIn2 = 255; + RotationDirection direction = RotationDirection::Clockwise; + unsigned int maxRpm = 200; +}; + +class DRV8833 : public Thing { + public: /// @brief Setup a DRV8833 motor controller /// @param pinAIn1 The pin number connected to the AIn1 port /// @param pinAIn2 The pin number connected to the AIn2 port @@ -18,16 +41,20 @@ public: /// indicated that the port is not connected /// @param directionA The forward turning direction of motor A /// @param directionB The forward turning direction of motor B - DRV8833(unsigned char pinAIn1, + DRV8833(Participant* participant, + unsigned char pinAIn1, unsigned char pinAIn2, unsigned char pinBIn1, unsigned char pinBIn2, unsigned char pinStandby = 255, - RotationDirection directionA = RotationDirection::Clockwise, - RotationDirection directionB = RotationDirection::Clockwise); + DRV8833Motor::RotationDirection directionA = DRV8833Motor::RotationDirection::Clockwise, + DRV8833Motor::RotationDirection directionB = DRV8833Motor::RotationDirection::Clockwise); - Thing* motorA = nullptr; - Thing* motorB = nullptr; + DRV8833Motor* motorA = nullptr; + DRV8833Motor* motorB = nullptr; + + protected: + unsigned char pinStandby = 255; }; } // namespace RoboidControl \ No newline at end of file diff --git a/Thing.h b/Thing.h index f1ab175..02fbcea 100644 --- a/Thing.h +++ b/Thing.h @@ -126,13 +126,13 @@ class Thing { /// @brief Set the linear velocity of the thing /// @param linearVelocity The new linear velocity in local space, in meters per second - void SetLinearVelocity(Spherical16 linearVelocity); + virtual void SetLinearVelocity(Spherical16 linearVelocity); /// @brief Get the linear velocity of the thing /// @return The linear velocity in local space, in meters per second virtual Spherical16 GetLinearVelocity(); /// @brief Set the angular velocity of the thing /// @param angularVelocity the new angular velocity in local space - void SetAngularVelocity(Spherical16 angularVelocity); + virtual void SetAngularVelocity(Spherical16 angularVelocity); /// @brief Get the angular velocity of the thing /// @return The angular velocity in local space virtual Spherical16 GetAngularVelocity(); diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index 531822b..c60e8c1 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -2,11 +2,14 @@ namespace RoboidControl { -RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) : Thing(participant) {} +RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) : Thing(participant) { + // this->leftWheel = new Thing(participant); + // this->rightWheel = new Thing(participant); +} void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation) { - this->wheelDiameter = abs(wheelDiameter); - this->wheelSeparation = abs(wheelSeparation); + this->wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2; + this->wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation; this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi; float distance = this->wheelSeparation / 2; @@ -27,6 +30,27 @@ void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) { this->rightWheel->SetPosition(Spherical(distance, Direction::right)); } -void DifferentialDrive::Update(unsigned long currentMs) {} +void DifferentialDrive::Update(unsigned long currentMs) { + // this assumes forward velocity only.... + float linearVelocity = this->GetLinearVelocity().distance; + + Spherical angularVelocity = this->GetAngularVelocity(); + float angularSpeed = angularVelocity.distance * Deg2Rad; // in degrees/sec + // Determine the rotation direction + if (angularVelocity.direction.horizontal.InDegrees() < 0) + angularSpeed = -angularSpeed; + + // wheel separation can be replaced by this->leftwheel->position->distance + float speedLeft = (linearVelocity + angularSpeed * this->wheelSeparation / 2) / this->wheelRadius * Rad2Deg; + if (this->leftWheel != nullptr) + this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left)); + + float speedRight = (linearVelocity - angularSpeed * this->wheelSeparation / 2) / this->wheelRadius * Rad2Deg; + if (this->rightWheel != nullptr) + this->rightWheel->SetAngularVelocity(Spherical(speedRight, Direction::right)); + + std::cout << "lin. speed " << linearVelocity << " ang. speed " << angularVelocity.distance << " left wheel " + << speedLeft << " right wheel " << speedRight << "\n"; +} } // namespace RoboidControl \ No newline at end of file diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h index fb7bda2..0614c3b 100644 --- a/Things/DifferentialDrive.h +++ b/Things/DifferentialDrive.h @@ -15,8 +15,8 @@ class DifferentialDrive : public Thing { virtual void Update(unsigned long currentMs) override; protected: - /// @brief The diameter of a wheel in meters - float wheelDiameter = 1.0f; + /// @brief The radius of a wheel in meters + float wheelRadius = 1.0f; /// @brief The distance between the wheels in meters float wheelSeparation = 1.0f;