134 lines
3.7 KiB
C++
134 lines
3.7 KiB
C++
#include "Propulsion.h"
|
|
#include "ControlledMotor.h"
|
|
|
|
#include "FloatSingle.h"
|
|
|
|
#include <Arduino.h>
|
|
|
|
Propulsion::Propulsion() {
|
|
this->placement = nullptr;
|
|
this->motorCount = 0;
|
|
}
|
|
|
|
void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
|
|
this->motorCount = 0;
|
|
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
|
|
Thing* thing = things[thingIx].thing;
|
|
if (thing->IsMotor())
|
|
motorCount++;
|
|
if (thing->type == Thing::ControlledMotorType)
|
|
hasOdometer = true;
|
|
}
|
|
this->placement = new Placement[motorCount];
|
|
|
|
unsigned int motorIx = 0;
|
|
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
|
|
Thing* thing = things[thingIx].thing;
|
|
if (thing->IsMotor())
|
|
this->placement[motorIx++] = things[thingIx];
|
|
}
|
|
}
|
|
|
|
unsigned int Propulsion::GetMotorCount() {
|
|
return this->motorCount;
|
|
}
|
|
|
|
Motor* Propulsion::GetMotor(unsigned int motorId) {
|
|
if (motorId >= this->motorCount)
|
|
return nullptr;
|
|
|
|
Thing* thing = this->placement[motorId].thing;
|
|
if (thing->IsMotor())
|
|
return (Motor*)thing;
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
Placement* Propulsion::GetMotorPlacement(unsigned int motorId) {
|
|
if (motorId >= this->motorCount)
|
|
return nullptr;
|
|
|
|
Placement* placement = &this->placement[motorId];
|
|
if (placement->thing->IsMotor())
|
|
return placement;
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
void Propulsion::Update(float currentTimeMs) {
|
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
|
Thing* thing = placement[motorIx].thing;
|
|
if (thing->type == Thing::ControlledMotorType) {
|
|
ControlledMotor* motor = (ControlledMotor*)thing;
|
|
motor->Update(currentTimeMs);
|
|
}
|
|
}
|
|
this->lastUpdateTime = currentTimeMs;
|
|
}
|
|
|
|
void Propulsion::SetMaxSpeed(float maxSpeed) {
|
|
this->maxSpeed = abs(maxSpeed);
|
|
}
|
|
|
|
void Propulsion::SetTwistSpeed(float forward, float yaw) {}
|
|
|
|
void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
|
|
|
|
void Propulsion::SetTwistSpeed(Vector3 linear,
|
|
float yaw,
|
|
float pitch,
|
|
float roll) {}
|
|
|
|
/// @brief Odometer returns the total distance traveled since start
|
|
/// @return The total distance
|
|
/// This returns the average distance of all wheels. The odometer cannot be
|
|
/// reset. When using a non-directional encoder, the distance is always
|
|
/// increasing. When using a directional encoder, the distance may go down when
|
|
/// the robot is driving backward.
|
|
/// When no wheel encoder is present, this function always returns zero.
|
|
float Propulsion::GetOdometer() {
|
|
float odometer = 0;
|
|
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
|
|
Thing* thing = placement[motorIx].thing;
|
|
if ((thing->type & Thing::ControlledMotorType) != 0) {
|
|
ControlledMotor* motor = (ControlledMotor*)thing;
|
|
odometer += motor->encoder->GetDistance() / this->motorCount;
|
|
}
|
|
}
|
|
return odometer;
|
|
}
|
|
|
|
bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) {
|
|
if (!this->driving) {
|
|
this->startTime = time(NULL);
|
|
this->targetDistance = point.magnitude();
|
|
if (hasOdometer)
|
|
this->startOdometer = GetOdometer();
|
|
this->driving = true;
|
|
}
|
|
if (hasOdometer) {
|
|
float distance = GetOdometer() - this->startOdometer;
|
|
if (distance >= this->targetDistance) {
|
|
this->driving = false;
|
|
point = Vector3::zero;
|
|
rotation = 0;
|
|
}
|
|
} else {
|
|
double duration = difftime(time(NULL), this->startTime);
|
|
if (duration >= this->targetDistance) {
|
|
this->driving = false;
|
|
point = Vector3::zero;
|
|
rotation = 0;
|
|
}
|
|
}
|
|
|
|
if (rotation > 0)
|
|
rotation = 1;
|
|
if (rotation < 0)
|
|
rotation = -1;
|
|
SetTwistSpeed(point.normalized() * this->maxSpeed, rotation);
|
|
|
|
Update(currentTimeMs);
|
|
|
|
return (!this->driving);
|
|
} |