127 lines
3.6 KiB
C++
127 lines
3.6 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 = new Propulsion();
|
|
this->networkSync = nullptr;
|
|
this->position = Spherical16::zero;
|
|
this->orientation = SwingTwist16::identity;
|
|
}
|
|
|
|
Roboid::Roboid(Propulsion *propulsion) : Roboid() {
|
|
this->propulsion = propulsion;
|
|
if (propulsion != nullptr)
|
|
propulsion->roboid = this;
|
|
}
|
|
|
|
void Roboid::Update(unsigned long currentTimeMs) {
|
|
if (perception != nullptr)
|
|
perception->Update(currentTimeMs);
|
|
|
|
if (propulsion != nullptr) {
|
|
propulsion->Update(currentTimeMs);
|
|
|
|
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
|
|
|
|
this->linearVelocity = this->propulsion->GetVelocity();
|
|
|
|
Spherical16 translation =
|
|
this->orientation * this->linearVelocity * deltaTime;
|
|
SetPosition(this->position + translation);
|
|
|
|
this->angularVelocity = this->propulsion->GetAngularVelocity();
|
|
SwingTwist16 rotation =
|
|
SwingTwist16::AngleAxis(this->angularVelocity.distance * deltaTime,
|
|
this->angularVelocity.direction);
|
|
if (perception != nullptr)
|
|
perception->UpdatePose(rotation);
|
|
|
|
SetOrientation(this->orientation * rotation);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
/*
|
|
void Roboid::SetPosition(Spherical16 newWorldPosition) {
|
|
SwingTwist16 roboidOrientation = this->GetOrientation();
|
|
Spherical16 translation = newWorldPosition - this->roboidPosition;
|
|
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) {
|
|
printf("roboid translation %f, %f\n", polarTranslation.distance,
|
|
polarTranslation.angle.InDegrees());
|
|
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);
|
|
}
|
|
}
|
|
|
|
#include <Arduino.h>
|
|
void Passer::RoboidControl::Roboid::Release(Thing *child) {
|
|
if (RemoveChild(child) != nullptr) {
|
|
child->position = this->position;
|
|
child->orientation = this->orientation;
|
|
printf("obj distance %f\n", child->position.distance);
|
|
this->perception->AddTrackedObject(nullptr, child);
|
|
}
|
|
}
|
|
|
|
void Roboid::LoadModel(const char *url) {
|
|
this->modelUrl = url;
|
|
if (this->networkSync == nullptr)
|
|
return;
|
|
|
|
this->networkSync->Download(url);
|
|
} |