RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added component

Browse Source
This commit is contained in:
Pascal Serrarens 2023-11-21 14:38:45 +01:00
parent c2478ce057
commit 2ff984b5bc
2 changed files with 44 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
Propulsion.cpp
View File

@ -1,59 +1,54 @@
#include "ControlledMotor.h"
#include "Propulsion.h" #include "Propulsion.h"
#include "ControlledMotor.h"
#include "FloatSingle.h" #include "FloatSingle.h"
extern "C" {
#include "debug.h"
}
Propulsion::Propulsion() { Propulsion::Propulsion() {
this->placement = nullptr; this->placement = nullptr;
this->motorCount = 0; this->motorCount = 0;
} }
//void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) { // void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
// this->palce = motors; // this->palce = motors;
// this->motorCount = motorCount; // this->motorCount = motorCount;
//} // }
void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
//this->placement = motors; // this->placement = motors;
//this->motorCount = motorCount; // this->motorCount = motorCount;
this->motorCount = 0; this->motorCount = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing; Thing* thing = things[thingIx].thing;
if (thing->isMotor) if (thing->isMotor)
motorCount++; motorCount++;
} }
this->placement = new Placement[motorCount]; 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 motorIx = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing;
if (thing->isMotor)
this->placement[motorIx++] = things[thingIx];
}
} }
void Propulsion::AddQuadcopter(Quadcopter* quadcopter) { void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
this->quadcopter = quadcopter; this->quadcopter = quadcopter;
} }
unsigned int Propulsion::GetMotorCount() { unsigned int Propulsion::GetMotorCount() {
return this->motorCount; return this->motorCount;
} }
Motor* Propulsion::GetMotor(unsigned int motorId) { Motor* Propulsion::GetMotor(unsigned int motorId) {
if (motorId >= this->motorCount) if (motorId >= this->motorCount)
return nullptr;
Thing* thing = this->placement[motorId].thing;
if (thing->isMotor)
return (Motor*)thing;
return nullptr; return nullptr;
Thing* thing = this->placement[motorId].thing;
if (thing->isMotor)
return (Motor*)thing;
return nullptr;
} }
void Propulsion::Update() { void Propulsion::Update() {
@ -63,14 +58,14 @@ void Propulsion::Update() {
// lastMillis = curMillis; // lastMillis = curMillis;
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
//Placement placement = placement[motorIx]; // Placement placement = placement[motorIx];
// placement.controlledMotor->Update(timeStep); // placement.controlledMotor->Update(timeStep);
} }
} }
void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) { void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Motor* motor = (Motor*) placement[motorIx].thing; Motor* motor = (Motor*)placement[motorIx].thing;
if (motor == nullptr) if (motor == nullptr)
continue; continue;
@ -84,11 +79,11 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) { void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) {
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
//Placement placement = placement[motorIx]; // Placement placement = placement[motorIx];
//if (placement.position.x < 0) // if (placement.position.x < 0)
// placement.controlledMotor->SetTargetVelocity(leftVelocity); // placement.controlledMotor->SetTargetVelocity(leftVelocity);
//else if (placement.position.x > 0) // else if (placement.position.x > 0)
// placement.controlledMotor->SetTargetVelocity(rightVelocity); // placement.controlledMotor->SetTargetVelocity(rightVelocity);
}; };
} }
@ -105,13 +100,13 @@ void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) {
SetDiffDriveSpeed(leftSpeed, rightSpeed); SetDiffDriveSpeed(leftSpeed, rightSpeed);
if (quadcopter != nullptr) { if (quadcopter != nullptr) {
quadcopter->SetTwistSpeed(forward, yaw, pitch); quadcopter->SetTwistSpeed(forward, yaw, pitch);
} }
} }
void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) { void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
if (quadcopter != nullptr) if (quadcopter != nullptr)
quadcopter->SetTwistSpeed(linear, yaw); quadcopter->SetTwistSpeed(linear, yaw);
} }
void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) { void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) {
@ -121,10 +116,10 @@ void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity)
} }
void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed, float rollSpeed) { void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed, float rollSpeed) {
if (quadcopter != nullptr) if (quadcopter != nullptr)
quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed); quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
} }
Quadcopter* Propulsion::GetQuadcopter() { Quadcopter* Propulsion::GetQuadcopter() {
return quadcopter; return quadcopter;
} }

1
component.mk Normal file
View File

@ -0,0 +1 @@
COMPONENT_ADD_INCLUDEDIRS = ./VectorAlgebra/include