Compare commits
3 Commits
2a83dbe7ca
...
c2ac149df6
Author | SHA1 | Date | |
---|---|---|---|
c2ac149df6 | |||
9a2ce4dd5e | |||
b8eb9bb712 |
@ -29,7 +29,7 @@ DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
|
||||
: RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
|
||||
|
||||
void DigitalInput::TouchSensor::Update(bool recursive) {
|
||||
this->touchedSomething = digitalInput.isLow;
|
||||
this->internalTouch = digitalInput.isLow;
|
||||
}
|
||||
|
||||
#pragma endregion Touch sensor
|
||||
@ -98,8 +98,8 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) {
|
||||
this->pulseFrequency = pulseCount / timeStep;
|
||||
this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
|
||||
|
||||
std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
|
||||
<< " timestep " << timeStep << std::endl;
|
||||
// std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
|
||||
// << " timestep " << timeStep << std::endl;
|
||||
|
||||
this->lastUpdateTime = currentTimeMs;
|
||||
}
|
||||
|
@ -65,7 +65,7 @@ UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
|
||||
void UltrasonicSensor::TouchSensor::Update(bool recursive) {
|
||||
RoboidControl::TouchSensor::Update(recursive);
|
||||
this->ultrasonic.Update(false);
|
||||
this->touchedSomething |= (this->ultrasonic.distance > 0 &&
|
||||
this->internalTouch |= (this->ultrasonic.distance > 0 &&
|
||||
this->ultrasonic.distance <= this->touchDistance);
|
||||
}
|
||||
|
||||
|
@ -5,6 +5,83 @@
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
void LowLevelMessages::SendSpherical(char* buffer,
|
||||
unsigned char* ix,
|
||||
Spherical s) {
|
||||
SendFloat16(buffer, ix, s.distance);
|
||||
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
|
||||
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
|
||||
}
|
||||
Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
|
||||
unsigned char* startIndex) {
|
||||
float distance = ReceiveFloat16(buffer, startIndex);
|
||||
|
||||
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle horizontal = Angle::Radians(horizontal8.InRadians());
|
||||
|
||||
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle vertical = Angle::Radians(vertical8.InRadians());
|
||||
|
||||
Spherical s = Spherical(distance, horizontal, vertical);
|
||||
return s;
|
||||
}
|
||||
|
||||
void LowLevelMessages::SendSwingTwist(char* buffer,
|
||||
unsigned char* ix,
|
||||
SwingTwist s) {
|
||||
SendAngle8(buffer, ix, s.swing.horizontal.InDegrees());
|
||||
SendAngle8(buffer, ix, s.swing.vertical.InDegrees());
|
||||
SendAngle8(buffer, ix, s.twist.InDegrees());
|
||||
}
|
||||
|
||||
SwingTwist LowLevelMessages::ReceiveSwingTwist(const char* buffer,
|
||||
unsigned char* startIndex) {
|
||||
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle horizontal = Angle::Radians(horizontal8.InRadians());
|
||||
|
||||
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle vertical = Angle::Radians(vertical8.InRadians());
|
||||
|
||||
Angle8 twist8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle twist = Angle::Radians(twist8.InRadians());
|
||||
|
||||
SwingTwist s = SwingTwist(horizontal, vertical, twist);
|
||||
return s;
|
||||
}
|
||||
|
||||
void LowLevelMessages::SendQuat32(char* buffer,
|
||||
unsigned char* ix,
|
||||
SwingTwist rotation) {
|
||||
Quaternion q = rotation.ToQuaternion();
|
||||
unsigned char qx = (char)(q.x * 127 + 128);
|
||||
unsigned char qy = (char)(q.y * 127 + 128);
|
||||
unsigned char qz = (char)(q.z * 127 + 128);
|
||||
unsigned char qw = (char)(q.w * 255);
|
||||
if (q.w < 0) {
|
||||
qx = -qx;
|
||||
qy = -qy;
|
||||
qz = -qz;
|
||||
qw = -qw;
|
||||
}
|
||||
// std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
|
||||
// << "\n";
|
||||
buffer[(*ix)++] = qx;
|
||||
buffer[(*ix)++] = qy;
|
||||
buffer[(*ix)++] = qz;
|
||||
buffer[(*ix)++] = qw;
|
||||
}
|
||||
|
||||
SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
|
||||
unsigned char* ix) {
|
||||
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qw = buffer[(*ix)++] / 255.0F;
|
||||
Quaternion q = Quaternion(qx, qy, qz, qw);
|
||||
SwingTwist s = SwingTwist::FromQuaternion(q);
|
||||
return s;
|
||||
}
|
||||
|
||||
void LowLevelMessages::SendAngle8(char* buffer,
|
||||
unsigned char* ix,
|
||||
const float angle) {
|
||||
@ -42,58 +119,5 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
|
||||
return (float)f.toFloat();
|
||||
}
|
||||
|
||||
void LowLevelMessages::SendSpherical(char* buffer,
|
||||
unsigned char* ix,
|
||||
Spherical s) {
|
||||
SendFloat16(buffer, ix, s.distance);
|
||||
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
|
||||
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
|
||||
}
|
||||
Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
|
||||
unsigned char* startIndex) {
|
||||
float distance = ReceiveFloat16(buffer, startIndex);
|
||||
|
||||
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle horizontal = Angle::Radians(horizontal8.InRadians());
|
||||
|
||||
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle vertical = Angle::Radians(vertical8.InRadians());
|
||||
|
||||
Spherical s = Spherical(distance, horizontal, vertical);
|
||||
return s;
|
||||
}
|
||||
|
||||
void LowLevelMessages::SendQuat32(char* buffer,
|
||||
unsigned char* ix,
|
||||
SwingTwist rotation) {
|
||||
Quaternion q = rotation.ToQuaternion();
|
||||
unsigned char qx = (char)(q.x * 127 + 128);
|
||||
unsigned char qy = (char)(q.y * 127 + 128);
|
||||
unsigned char qz = (char)(q.z * 127 + 128);
|
||||
unsigned char qw = (char)(q.w * 255);
|
||||
if (q.w < 0) {
|
||||
qx = -qx;
|
||||
qy = -qy;
|
||||
qz = -qz;
|
||||
qw = -qw;
|
||||
}
|
||||
// std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
|
||||
// << "\n";
|
||||
buffer[(*ix)++] = qx;
|
||||
buffer[(*ix)++] = qy;
|
||||
buffer[(*ix)++] = qz;
|
||||
buffer[(*ix)++] = qw;
|
||||
}
|
||||
|
||||
SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
|
||||
unsigned char* ix) {
|
||||
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qw = buffer[(*ix)++] / 255.0F;
|
||||
Quaternion q = Quaternion(qx, qy, qz, qw);
|
||||
SwingTwist s = SwingTwist::FromQuaternion(q);
|
||||
return s;
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
@ -11,6 +11,10 @@ class LowLevelMessages {
|
||||
static Spherical ReceiveSpherical(const char* buffer,
|
||||
unsigned char* startIndex);
|
||||
|
||||
static void SendSwingTwist(char* buffer, unsigned char* ix, SwingTwist s);
|
||||
static SwingTwist ReceiveSwingTwist(const char* buffer,
|
||||
unsigned char* startIndex);
|
||||
|
||||
static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
|
||||
static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
|
||||
|
||||
|
@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) {
|
||||
this->thingId = buffer[ix++];
|
||||
this->poseType = buffer[ix++];
|
||||
this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
|
||||
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
|
||||
this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix);
|
||||
// linearVelocity
|
||||
// angularVelocity
|
||||
}
|
||||
@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
|
||||
if ((this->poseType & Pose_Position) != 0)
|
||||
LowLevelMessages::SendSpherical(buffer, &ix, this->position);
|
||||
if ((this->poseType & Pose_Orientation) != 0)
|
||||
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
|
||||
LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation);
|
||||
if ((this->poseType & Pose_LinearVelocity) != 0)
|
||||
LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
|
||||
if ((this->poseType & Pose_AngularVelocity) != 0)
|
||||
|
@ -62,7 +62,7 @@ bool Participant::Send(IMessage* msg) {
|
||||
return true;
|
||||
|
||||
// std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
|
||||
// << " to " << remoteParticipant->ipAddress << std::endl;
|
||||
// << " to " << this->ipAddress << std::endl;
|
||||
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
Windows::ParticipantUDP* thisWindows =
|
||||
|
@ -97,7 +97,7 @@ void ParticipantUDP::Update() {
|
||||
if (this->remoteSite == nullptr)
|
||||
this->Publish(msg);
|
||||
else
|
||||
this->remoteSite->Send(msg);
|
||||
this->Send(msg);
|
||||
|
||||
delete msg;
|
||||
|
||||
@ -127,19 +127,19 @@ void ParticipantUDP::UpdateMyThings() {
|
||||
continue;
|
||||
|
||||
// std::cout << thing->name << "\n";
|
||||
if (thing->hierarchyChanged) {
|
||||
if (!(this->isIsolated || this->networkId == 0)) {
|
||||
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
|
||||
this->remoteSite->Send(thingMsg);
|
||||
delete thingMsg;
|
||||
// if (thing->hierarchyChanged) {
|
||||
// if (!(this->isIsolated || this->networkId == 0)) {
|
||||
// ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
|
||||
// this->remoteSite->Send(thingMsg);
|
||||
// delete thingMsg;
|
||||
|
||||
if (thing->nameChanged) {
|
||||
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
||||
this->remoteSite->Send(nameMsg);
|
||||
delete nameMsg;
|
||||
}
|
||||
}
|
||||
}
|
||||
// if (thing->nameChanged) {
|
||||
// NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
||||
// this->remoteSite->Send(nameMsg);
|
||||
// delete nameMsg;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// std::cout << "B\n";
|
||||
// Why don't we do recursive?
|
||||
@ -149,26 +149,26 @@ void ParticipantUDP::UpdateMyThings() {
|
||||
thing->Update(false);
|
||||
|
||||
// std::cout << "C\n";
|
||||
if (!(this->isIsolated || this->networkId == 0)) {
|
||||
if (thing->terminate) {
|
||||
DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
|
||||
this->remoteSite->Send(destroyMsg);
|
||||
delete destroyMsg;
|
||||
} else {
|
||||
// Send to remote site
|
||||
if (thing->nameChanged) {
|
||||
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
||||
this->remoteSite->Send(nameMsg);
|
||||
delete nameMsg;
|
||||
}
|
||||
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
|
||||
this->remoteSite->Send(poseMsg);
|
||||
delete poseMsg;
|
||||
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
|
||||
this->remoteSite->Send(binaryMsg);
|
||||
delete binaryMsg;
|
||||
}
|
||||
}
|
||||
// if (!(this->isIsolated || this->networkId == 0)) {
|
||||
// if (thing->terminate) {
|
||||
// DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
|
||||
// this->remoteSite->Send(destroyMsg);
|
||||
// delete destroyMsg;
|
||||
// } else {
|
||||
// // Send to remote site
|
||||
// if (thing->nameChanged) {
|
||||
// NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
||||
// this->remoteSite->Send(nameMsg);
|
||||
// delete nameMsg;
|
||||
// }
|
||||
// PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
|
||||
// this->remoteSite->Send(poseMsg);
|
||||
// delete poseMsg;
|
||||
// BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
|
||||
// this->remoteSite->Send(binaryMsg);
|
||||
// delete binaryMsg;
|
||||
// }
|
||||
// }
|
||||
// std::cout << "D\n";
|
||||
if (thing->terminate)
|
||||
this->Remove(thing);
|
||||
@ -230,34 +230,10 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
|
||||
delete binaryMsg;
|
||||
}
|
||||
|
||||
// bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
|
||||
// int bufferSize = msg->Serialize(this->buffer);
|
||||
// if (bufferSize <= 0)
|
||||
// return true;
|
||||
|
||||
// // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
|
||||
// // << " to " << remoteParticipant->ipAddress << std::endl;
|
||||
|
||||
// #if defined(_WIN32) || defined(_WIN64)
|
||||
// Windows::ParticipantUDP* thisWindows =
|
||||
// static_cast<Windows::ParticipantUDP*>(this);
|
||||
// return thisWindows->Send(remoteParticipant, bufferSize);
|
||||
// #elif defined(__unix__) || defined(__APPLE__)
|
||||
// Posix::ParticipantUDP* thisPosix =
|
||||
// static_cast<Posix::ParticipantUDP*>(this); return
|
||||
// thisPosix->Send(remoteParticipant, bufferSize);
|
||||
// #elif defined(ARDUINO)
|
||||
// Arduino::ParticipantUDP* thisArduino =
|
||||
// static_cast<Arduino::ParticipantUDP*>(this);
|
||||
// return thisArduino->Send(remoteParticipant, bufferSize);
|
||||
// #elif defined(IDF_VER)
|
||||
// EspIdf::ParticipantUDP* thisEspIdf =
|
||||
// static_cast<EspIdf::ParticipantUDP*>(this);
|
||||
// return thisEspIdf->Send(remoteParticipant, bufferSize);
|
||||
// #else
|
||||
// return false;
|
||||
// #endif
|
||||
// }
|
||||
bool ParticipantUDP::Send(IMessage* msg) {
|
||||
if (this->remoteSite != nullptr)
|
||||
this->remoteSite->Send(msg);
|
||||
}
|
||||
|
||||
void ParticipantUDP::PublishThingInfo(Thing* thing) {
|
||||
// std::cout << "Publish thing info" << thing->networkId << "\n";
|
||||
|
@ -77,7 +77,6 @@ public:
|
||||
long publishInterval = 3000; // 3 seconds
|
||||
|
||||
protected:
|
||||
char buffer[1024];
|
||||
|
||||
#if !defined(ARDUINO)
|
||||
#if defined(__unix__) || defined(__APPLE__)
|
||||
@ -114,7 +113,7 @@ public:
|
||||
void SendThingInfo(Participant* remoteParticipant, Thing* thing);
|
||||
void PublishThingInfo(Thing* thing);
|
||||
|
||||
//bool Send(Participant* remoteParticipant, IMessage* msg);
|
||||
virtual bool Send(IMessage* msg) override;
|
||||
bool Publish(IMessage* msg);
|
||||
|
||||
#pragma endregion Send
|
||||
|
@ -39,8 +39,8 @@ void ControlledMotor::Update(bool recurse) {
|
||||
this->lastError = error;
|
||||
|
||||
float output = p_term + i_term + d_term;
|
||||
std::cout << "target " << this->targetVelocity << " actual "
|
||||
<< this->actualVelocity << " output = " << output << std::endl;
|
||||
// std::cout << "target " << this->targetVelocity << " actual "
|
||||
// << this->actualVelocity << " output = " << output << std::endl;
|
||||
// float acceleration =
|
||||
// error * timeStep * pidP; // Just P is used at this moment
|
||||
// std::cout << "motor acc. " << acceleration << std::endl;
|
||||
|
@ -1,5 +1,8 @@
|
||||
#include "Motor.h"
|
||||
|
||||
#include "Messages/BinaryMsg.h"
|
||||
#include "Participant.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
Motor::Motor(Thing* parent) : Thing(parent) {
|
||||
@ -7,7 +10,20 @@ Motor::Motor(Thing* parent) : Thing(parent) {
|
||||
}
|
||||
|
||||
void Motor::SetTargetVelocity(float targetSpeed) {
|
||||
if (targetSpeed != this->targetVelocity) {
|
||||
this->targetVelocity = targetSpeed;
|
||||
|
||||
if (this->owner->networkId != 0) {
|
||||
// in other word: if we are connected...
|
||||
BinaryMsg* binaryMsg = new BinaryMsg(this->owner->networkId, this);
|
||||
this->owner->Send(binaryMsg);
|
||||
delete binaryMsg;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float Motor::GetTargetVelocity() {
|
||||
return this->targetVelocity;
|
||||
}
|
||||
|
||||
int Motor::GenerateBinary(char* data, unsigned char* ix) {
|
||||
|
@ -14,11 +14,11 @@ class Motor : public Thing {
|
||||
Direction direction;
|
||||
|
||||
virtual void SetTargetVelocity(float velocity); // -1..0..1
|
||||
virtual float GetTargetVelocity();
|
||||
|
||||
int GenerateBinary(char* bytes, unsigned char* ix) override;
|
||||
// virtual void ProcessBinary(char* bytes) override;
|
||||
|
||||
//protected:
|
||||
protected:
|
||||
float targetVelocity = 0;
|
||||
};
|
||||
|
||||
|
@ -7,8 +7,12 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent) {
|
||||
this->name = "Touch sensor";
|
||||
}
|
||||
|
||||
bool TouchSensor::IsTouching() {
|
||||
return this->internalTouch || this->externalTouch;
|
||||
}
|
||||
|
||||
void TouchSensor::PrepareForUpdate() {
|
||||
this->touchedSomething = this->externalTouch;
|
||||
//this->internalTouch = this->externalTouch;
|
||||
}
|
||||
|
||||
void TouchSensor::Update(bool recursive) {
|
||||
@ -16,7 +20,8 @@ void TouchSensor::Update(bool recursive) {
|
||||
}
|
||||
|
||||
int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
|
||||
bytes[(*ix)++] = this->touchedSomething ? 1 : 0;
|
||||
std::cout << "BinaryMsg Touch " << this->internalTouch << std::endl;
|
||||
bytes[(*ix)++] = this->internalTouch ? 1 : 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
@ -16,7 +16,7 @@ class TouchSensor : public Thing {
|
||||
|
||||
/// @brief Value which is true when the sensor is touching something, false
|
||||
/// otherwise
|
||||
bool touchedSomething = false;
|
||||
bool IsTouching();
|
||||
|
||||
virtual void PrepareForUpdate() override;
|
||||
virtual void Update(bool recursive) override;
|
||||
@ -30,6 +30,7 @@ class TouchSensor : public Thing {
|
||||
virtual void ProcessBinary(char* bytes) override;
|
||||
protected:
|
||||
bool externalTouch = false;
|
||||
bool internalTouch = false;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
|
@ -27,10 +27,10 @@ int main() {
|
||||
while (true) {
|
||||
// The left wheel turns forward when nothing is touched on the right side
|
||||
// and turn backward when the roboid hits something on the right
|
||||
float leftWheelSpeed = (touchRight.touchedSomething) ? -600.0f : 600.0f;
|
||||
float leftWheelSpeed = (touchRight.internalTouch) ? -600.0f : 600.0f;
|
||||
// The right wheel does the same, but instead is controlled by
|
||||
// touches on the left side
|
||||
float rightWheelSpeed = (touchLeft.touchedSomething) ? -600.0f : 600.0f;
|
||||
float rightWheelSpeed = (touchLeft.internalTouch) ? -600.0f : 600.0f;
|
||||
// When both sides are touching something, both wheels will turn backward
|
||||
// and the roboid will move backwards
|
||||
bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
|
||||
|
Loading…
x
Reference in New Issue
Block a user