66 lines
1.7 KiB
C++
66 lines
1.7 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);
|
|
}
|
|
|
|
bool Roboid::Drive(float forwardDistance) {
|
|
bool finished = true;
|
|
for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) {
|
|
bool motorFinished = false;
|
|
Thing* thing = propulsion.placement[motorIx].thing;
|
|
|
|
switch (thing->type) {
|
|
case Thing::Type::ControlledMotor: {
|
|
ControlledMotor* controlledMotor = (ControlledMotor*)thing;
|
|
motorFinished = controlledMotor->Drive(forwardDistance);
|
|
break;
|
|
}
|
|
case Thing::Type::Motor: {
|
|
Motor* motor = (Motor*)thing;
|
|
motorFinished = motor->Drive(forwardDistance);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
if (motorFinished == false) {
|
|
finished = false;
|
|
} else
|
|
Serial.printf("Motor finished\n");
|
|
}
|
|
return finished;
|
|
}
|
|
|
|
float waypointArray[] = {1, -1};
|
|
void Roboid::FollowTrajectory(Trajectory* Trajectory) {
|
|
this->waypoints = waypointArray;
|
|
this->waypointCount = 2;
|
|
this->waypointIx = 0;
|
|
}
|
|
|
|
void Roboid::Update() {
|
|
if (this->waypoints == nullptr)
|
|
return;
|
|
|
|
// Serial.printf("Driving waypoints %d %f\n", this->waypointIx,
|
|
// waypoints[waypointIx]);
|
|
if (Drive(this->waypoints[this->waypointIx])) {
|
|
this->waypointIx++;
|
|
if (this->waypointIx == this->waypointCount) {
|
|
this->waypoints = nullptr;
|
|
this->waypointIx = 0;
|
|
this->waypointCount = 0;
|
|
propulsion.SetTwistSpeed(0, 0);
|
|
}
|
|
}
|
|
} |