47 lines
1.2 KiB
C++
47 lines
1.2 KiB
C++
#include "Roboid.h"
|
|
#include <Arduino.h>
|
|
|
|
Roboid::Roboid() {
|
|
// this->configuration = nullptr;
|
|
// this->thingCount = 0;
|
|
}
|
|
|
|
// Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
|
|
// // this->configuration = configuration;
|
|
// // this->thingCount = thingCount;
|
|
|
|
// perception.AddSensors(configuration, thingCount);
|
|
// propulsion.AddMotors(configuration, thingCount);
|
|
// }
|
|
|
|
Roboid::Roboid(Perception* perception, Propulsion* propulsion) {
|
|
this->perception = perception;
|
|
this->propulsion = propulsion;
|
|
}
|
|
|
|
bool Roboid::Drive(Waypoint* waypoint, float currentTimeMs) {
|
|
bool finished =
|
|
propulsion->Drive(waypoint->point, waypoint->rotation, currentTimeMs);
|
|
return finished;
|
|
}
|
|
|
|
void Roboid::FollowTrajectory(Trajectory* trajectory) {
|
|
this->trajectory = trajectory;
|
|
this->waypointIx = 0;
|
|
}
|
|
|
|
void Roboid::Update(float currentTimeMs) {
|
|
if (this->trajectory == nullptr)
|
|
return;
|
|
|
|
Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx];
|
|
if (Drive(waypoint, currentTimeMs)) {
|
|
this->waypointIx++;
|
|
if (this->waypointIx >= this->trajectory->waypointCount) {
|
|
this->trajectory = nullptr;
|
|
this->waypointIx = 0;
|
|
// stop
|
|
// propulsion.SetTwistSpeed(0, 0);
|
|
}
|
|
}
|
|
} |