122 lines
3.7 KiB
C++
122 lines
3.7 KiB
C++
#include "Roboid.h"
|
|
|
|
#include "LinearAlgebra/FloatSingle.h"
|
|
#include "NetworkSync.h"
|
|
|
|
#include <string.h>
|
|
|
|
// #define RC_DEBUG
|
|
|
|
#ifdef RC_DEBUG
|
|
#include <Arduino.h>
|
|
#endif
|
|
|
|
Roboid::Roboid() : Thing(0) {
|
|
#ifdef RC_DEBUG
|
|
Serial.begin(115200);
|
|
#endif
|
|
this->type = RoboidType;
|
|
this->perception = new Perception();
|
|
this->perception->roboid = this;
|
|
this->propulsion = nullptr;
|
|
this->networkSync = nullptr;
|
|
|
|
this->worldPosition = Spherical16::zero;
|
|
this->worldOrientation = SwingTwist16::identity;
|
|
}
|
|
|
|
Roboid::Roboid(Propulsion *propulsion) : Roboid() {
|
|
this->propulsion = propulsion;
|
|
if (propulsion != nullptr)
|
|
propulsion->roboid = this;
|
|
}
|
|
|
|
void Passer::RoboidControl::Roboid::SetName(char *name) { this->name = name; }
|
|
|
|
#include <Arduino.h>
|
|
void Roboid::Update(unsigned long currentTimeMs) {
|
|
if (perception != nullptr)
|
|
perception->Update(currentTimeMs);
|
|
|
|
if (propulsion != nullptr) {
|
|
propulsion->Update(currentTimeMs);
|
|
|
|
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
|
|
|
|
// Conversion from old units
|
|
Polar polarVelocity = this->propulsion->GetVelocity();
|
|
this->linearVelocity = Spherical16(
|
|
polarVelocity.distance,
|
|
Angle16::Degrees(polarVelocity.angle.InDegrees()), Angle16());
|
|
float oldAngular = this->propulsion->GetAngularVelocity();
|
|
this->angularVelocity =
|
|
Spherical16(oldAngular, Angle16(), Angle16::Degrees(90));
|
|
|
|
SetPosition(this->position + this->orientation * Spherical16::forward *
|
|
this->linearVelocity.distance * deltaTime);
|
|
this->worldPosition = this->position; // assuming the roboid is the root
|
|
|
|
SwingTwist16 rotation = SwingTwist16::AngleAxis(
|
|
this->angularVelocity.distance * deltaTime, Direction16::up);
|
|
if (perception != nullptr)
|
|
perception->UpdatePose(rotation);
|
|
|
|
this->orientation = this->orientation * rotation;
|
|
|
|
this->worldOrientation =
|
|
this->orientation; // assuming the roboid is the root
|
|
}
|
|
|
|
if (childCount > 0 && children != nullptr) {
|
|
for (unsigned char childIx = 0; childIx < this->childCount; childIx++)
|
|
children[childIx]->Update(currentTimeMs);
|
|
}
|
|
|
|
if (networkSync != nullptr)
|
|
networkSync->NetworkUpdate(this);
|
|
|
|
lastUpdateTimeMs = currentTimeMs;
|
|
}
|
|
|
|
Spherical16 Roboid::GetPosition() { return this->worldPosition; }
|
|
|
|
SwingTwist16 Roboid::GetOrientation() { return this->worldOrientation; }
|
|
|
|
void Roboid::SetPosition(Spherical16 newWorldPosition) {
|
|
SwingTwist16 roboidOrientation = this->GetOrientation();
|
|
Spherical16 translation = newWorldPosition - this->worldPosition;
|
|
float distance = translation.distance;
|
|
Angle16 angle = Spherical16::SignedAngleBetween(
|
|
roboidOrientation * Spherical16::forward, translation, Spherical16::up);
|
|
Polar16 polarTranslation = Polar16(
|
|
distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance));
|
|
if (perception != nullptr)
|
|
perception->UpdatePose(polarTranslation);
|
|
|
|
this->position = newWorldPosition; // roboid is the root?
|
|
// World position should be set in the update recursion
|
|
// this->worldPosition = newWorldPosition;
|
|
|
|
// if (networkSync != nullptr)
|
|
// // networkSync->SendPosition(this->worldPosition);
|
|
// networkSync->SendPose(this->worldPosition, roboidOrientation);
|
|
}
|
|
|
|
#include <math.h>
|
|
void Roboid::SetOrientation(SwingTwist16 newOrientation) {
|
|
SwingTwist16 delta =
|
|
SwingTwist16::Inverse(this->orientation) * newOrientation;
|
|
if (perception != nullptr)
|
|
perception->UpdatePose(delta);
|
|
|
|
this->orientation = newOrientation;
|
|
}
|
|
|
|
void Roboid::AddChild(Thing *child) {
|
|
Thing::AddChild(child);
|
|
if (child->IsSensor()) {
|
|
Sensor *childSensor = (Sensor *)child;
|
|
this->perception->AddSensor(childSensor);
|
|
}
|
|
}
|