Skip to content

Commit

Permalink
Updates Thruster Control Names
Browse files Browse the repository at this point in the history 
Thruster control names are updated to reflect changes in hardware.
  • Loading branch information
@TekuConcept
TekuConcept committed Mar 22, 2017
1 parent 1609874 commit c7e94ff
Show file tree
Hide file tree
Showing 7 changed files with 194 additions and 238 deletions.
15 changes: 8 additions & 7 deletions BoneCentral/Peripherals/Peripherals.Core/include/CommandDispatcher.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,15 +29,16 @@ class CommandDispatcher {
bool shouldExit_;

void dispatchCommand(std::stringstream& cmd);

void goDirection(std::stringstream& cmdString);
void faceDirection(std::stringstream& cmdString);
void thrustForward(std::stringstream& cmdString);
void rotate(std::stringstream& cmdString);
void move(std::stringstream& cmdString);
void strafe(std::stringstream& cmdString);
void dive(std::stringstream& cmdString);

void setForwardTrim(std::stringstream& cmdString);
void setStrafeTrim(std::stringstream& cmdString);
void setDiveTrim(std::stringstream& cmdString);
void setDiveOffset(std::stringstream& cmdString);
void yaw(std::stringstream& cmdString);
void pitch(std::stringstream& cmdString);
void roll(std::stringstream& cmdString);
void kill();

void _getAcceleration();
void _getAngularAcceleration();
Expand Down
12 changes: 6 additions & 6 deletions BoneCentral/Peripherals/Peripherals.Core/include/Interfaces/IThrusterFactory.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,12 +10,12 @@

class IThrusterFactory {
public:
virtual std::shared_ptr<IThruster> createLeftForwardThruster() = 0;
virtual std::shared_ptr<IThruster> createRightForwardThruster() = 0;
virtual std::shared_ptr<IThruster> createLeftStrafeThruster() = 0;
virtual std::shared_ptr<IThruster> createRightStrafeThruster() = 0;
virtual std::shared_ptr<IThruster> createForwardDiveThruster() = 0;
virtual std::shared_ptr<IThruster> createRearDiveThruster() = 0;
virtual std::shared_ptr<IThruster> createMoveThruster() = 0;
virtual std::shared_ptr<IThruster> createStrafeThruster() = 0;
virtual std::shared_ptr<IThruster> createDiveThruster() = 0;
virtual std::shared_ptr<IThruster> createYawThruster() = 0;
virtual std::shared_ptr<IThruster> createPitchThruster() = 0;
virtual std::shared_ptr<IThruster> createRollThruster() = 0;
};

#endif //PERIPHERALS_I_THRUSTER_FACTORY_H
51 changes: 21 additions & 30 deletions BoneCentral/Peripherals/Peripherals.Core/include/ThrustController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,42 +16,33 @@ class ThrustController
{
public:
ThrustController(IThrusterFactory& thrusterFactory, std::shared_ptr<ILogger> logger);
void goDirection(float forward, float strafe, float dive);
void faceDirection(float yaw, float dive);
void thrustForward(float left, float right);
void dive(float front, float rear);
void setForwardTrim(float left, float right);
void setStrafeTrim(float left, float right);
void setDiveTrim(float front, float back);
void setDiveOffset(float front, float back);
void killAllThruster();
void goDirection(float _move_, float _strafe_, float _dive_);
void rotate(float _yaw_, float _pitch_, float _roll_);
void move(float throttle);
void strafe(float throttle);
void dive(float throttle);
void yaw(float throttle);
void pitch(float throttle);
void roll(float throttle);
void killAllThrusters();
~ThrustController();

private:
std::shared_ptr<IThruster> leftForwardThruster_;
std::shared_ptr<IThruster> rightForwardThruster_;
std::shared_ptr<IThruster> leftStrafeThruster_;
std::shared_ptr<IThruster> rightStrafeThruster_;
std::shared_ptr<IThruster> forwardDiveThruster_;
std::shared_ptr<IThruster> rearDiveThruster_;
std::shared_ptr<IThruster> moveThruster_;
std::shared_ptr<IThruster> strafeThruster_;
std::shared_ptr<IThruster> diveThruster_;
std::shared_ptr<IThruster> yawThruster_;
std::shared_ptr<IThruster> pitchThruster_;
std::shared_ptr<IThruster> rollThruster_;

std::shared_ptr<ILogger> logger_;

const float maxPower = 1.0f;
const float minPower = 0.0f;
const float reverseRatio = 0.84507f;
const float strafeRatio = 0.7f;

FloatPair forwardTrim;
FloatPair diveTrim;
FloatPair diveOffset;
FloatPair strafeTrim;

FloatPair getReciprocalValues(float value);
float getSafeOffset(float a, float b);
float getScaleToMaxPower(float left, float right);
float getMaxMag(float left, float right);
void setThrust(FloatPair forwardPair, FloatPair strafePair, FloatPair divePair);
const float MAX_THROTTLE = 1.0f;
const float MIN_THROTTLE = -1.0f;
// const float reverseRatio = 0.84507f;
// const float strafeRatio = 0.7f;

float getSafeThrottle(float throttle);
};

#endif
154 changes: 99 additions & 55 deletions BoneCentral/Peripherals/Peripherals.Core/src/CommandDispatcher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,9 @@
#include "CommandDispatcher.h"
using json = nlohmann::json;

CommandDispatcher::CommandDispatcher(std::istream& in, std::ostream& out, ImuSensor& imuSensor, ThrustController& thrustController, PowerManager& powerManager, IHeadlights& lights)
CommandDispatcher::CommandDispatcher(std::istream& in, std::ostream& out,
ImuSensor& imuSensor, ThrustController& thrustController,
PowerManager& powerManager, IHeadlights& lights)
: in_(in),
out_(out),
imuSensor_(imuSensor),
Expand All @@ -26,77 +28,119 @@ void CommandDispatcher::runLoop() {
void CommandDispatcher::dispatchCommand(std::stringstream& cmdString) {
std::string cmd;
cmdString >> cmd;
if(cmd == "goDirection") goDirection(cmdString);
else if(cmd == "faceDirection") faceDirection(cmdString);
else if(cmd == "thrustForward") thrustForward(cmdString);
else if(cmd == "dive") dive(cmdString);
else if(cmd == "turnOnEscs") powerManager_.turnOnEscs();
else if(cmd == "turnOffEscs") powerManager_.turnOffEscs();
else if(cmd == "switchLights") lights_.switchLights();
else if(cmd == "turnOnImu") powerManager_.turnOnImuSensor();
else if(cmd == "turnOffImu") powerManager_.turnOffImuSensor();
else if(cmd == "setForwardTrim") setForwardTrim(cmdString);
else if(cmd == "setStrafeTrim") setStrafeTrim(cmdString);
else if(cmd == "setDiveTrim") setDiveTrim(cmdString);
else if(cmd == "setDiveOffset") setDiveOffset(cmdString);
else if(cmd == "getAcceleration") _getAcceleration();
else if(cmd == "getAngularAcceleration") _getAngularAcceleration();
else if(cmd == "getHeading") _getHeading();
else if(cmd == "getInternalTemperature") _getInternalTemperature();
else if(cmd == "getInternalPressure") _getInternalPressure();
else if(cmd == "getExternalTemperature") _getExternalTemperature();
else if(cmd == "getExternalPressure") _getExternalPressure();
else if(cmd == "exit") shouldExit_ = true;
if(cmd == "goDirection") goDirection(cmdString);
else if(cmd == "rotate") rotate(cmdString);
else if(cmd == "move") move(cmdString);
else if(cmd == "secondaryDive") strafe(cmdString);
else if(cmd == "primaryDive") dive(cmdString);
else if(cmd == "yaw") yaw(cmdString);
else if(cmd == "pitch") pitch(cmdString);
else if(cmd == "roll") roll(cmdString);
else if(cmd == "killThrust") kill();
else if(cmd == "turnOnEscs") powerManager_.turnOnEscs();
else if(cmd == "turnOffEscs") powerManager_.turnOffEscs();
else if(cmd == "switchLights") lights_.switchLights();
else if(cmd == "turnOnImu") powerManager_.turnOnImuSensor();
else if(cmd == "turnOffImu") powerManager_.turnOffImuSensor();
else if(cmd == "getAcceleration") _getAcceleration();
else if(cmd == "getAngularAcceleration") _getAngularAcceleration();
else if(cmd == "getHeading") _getHeading();
else if(cmd == "getInternalTemperature") _getInternalTemperature();
else if(cmd == "getInternalPressure") _getInternalPressure();
else if(cmd == "getExternalTemperature") _getExternalTemperature();
else if(cmd == "getExternalPressure") _getExternalPressure();
else if(cmd == "exit") shouldExit_ = true;
}

void CommandDispatcher::goDirection(std::stringstream& cmdString) {
float forward, strafe, dive;
cmdString >> forward >> strafe >> dive;
thrustController_.goDirection(forward, strafe, dive);
}

void CommandDispatcher::faceDirection(std::stringstream& cmdString) {
float yaw, dive;
cmdString >> yaw >> dive;
thrustController_.faceDirection(yaw, dive);
}

void CommandDispatcher::thrustForward(std::stringstream &cmdString) {
float left, right;
cmdString >> left >> right;
thrustController_.thrustForward(left, right);

void CommandDispatcher::goDirection(std::stringstream& cmdString) {
float move, strafe, dive;
cmdString >> move >> strafe >> dive;
#ifdef DEBUG
std::cout << "Move: " << move;
std::cout << " Strafe: " << strafe;
std::cout << " Dive: " << dive << std::endl;
#endif
thrustController_.goDirection(move, strafe, dive);
}

void CommandDispatcher::rotate(std::stringstream& cmdString) {
float yaw, pitch, roll;
cmdString >> yaw >> pitch >> roll;
#ifdef DEBUG
std::cout << "Yaw: " << yaw;
std::cout << " Pitch: " << pitch;
std::cout << " Roll: " << roll << std::endl;
#endif
thrustController_.rotate(yaw, pitch, roll);
}

void CommandDispatcher::move(std::stringstream &cmdString) {
float throttle;
cmdString >> throttle;
#ifdef DEBUG
std::cout << "Move: " << throttle << std::endl;
#endif
thrustController_.move(throttle);
}

void CommandDispatcher::strafe(std::stringstream &cmdString) {
float throttle;
cmdString >> throttle;
#ifdef DEBUG
std::cout << "Strafe: " << throttle << std::endl;
#endif
thrustController_.strafe(throttle);
}

void CommandDispatcher::dive(std::stringstream &cmdString) {
float front, rear;
cmdString >> front >> rear;
thrustController_.dive(front, rear);
float throttle;
cmdString >> throttle;
#ifdef DEBUG
std::cout << "Dive: " << throttle << std::endl;
#endif
thrustController_.dive(throttle);
}

void CommandDispatcher::setForwardTrim(std::stringstream& cmdString) {
float a, b;
cmdString >> a >> b;
thrustController_.setForwardTrim(a, b);
void CommandDispatcher::yaw(std::stringstream &cmdString) {
float throttle;
cmdString >> throttle;
#ifdef DEBUG
std::cout << "Yaw: " << throttle << std::endl;
#endif
thrustController_.yaw(throttle);
}

void CommandDispatcher::setStrafeTrim(std::stringstream& cmdString) {
float a, b;
cmdString >> a >> b;
thrustController_.setStrafeTrim(a, b);
void CommandDispatcher::pitch(std::stringstream &cmdString) {
float throttle;
cmdString >> throttle;
#ifdef DEBUG
std::cout << "Pitch: " << throttle << std::endl;
#endif
thrustController_.pitch(throttle);
}

void CommandDispatcher::setDiveTrim(std::stringstream& cmdString) {
float a, b;
cmdString >> a >> b;
thrustController_.setDiveTrim(a, b);
void CommandDispatcher::roll(std::stringstream &cmdString) {
float throttle;
cmdString >> throttle;
#ifdef DEBUG
std::cout << "Roll: " << throttle << std::endl;
#endif
thrustController_.roll(throttle);
}

void CommandDispatcher::setDiveOffset(std::stringstream& cmdString) {
float a, b;
cmdString >> a >> b;
thrustController_.setDiveOffset(a, b);
void CommandDispatcher::kill() {
#ifdef DEBUG
std::cout << "Killed Thrusters" << std::endl;
#endif
thrustController_.killAllThrusters();
}




void CommandDispatcher::_getAcceleration() {
auto data = imuSensor_.getAcceleration();
auto accelJson = json{
Expand Down
Loading

0 comments on commit c7e94ff

Please sign in to comment.