Klagarge/SDi-STOMP
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Activity

use vector for choose direction

Browse Source
This commit is contained in:
Rémi Heredero
2023-12-28 14:57:13 +01:00
parent 17ba18a200
commit 633592a4ea
7 changed files with 162 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
app.cpp
View File

@@ -1,9 +1,9 @@
#include "app.h"
App::App(Stomp* st) {
st_ = st;
st_->connectRequest("sdi.hevs.ch", 61614, "/", "sdi10", "809c02f36becb0868da98761fe3209f6");
st_->sendRequest("/topic/sdi10.gem.command", "right");
st_->subscribeRequest("/topic/sdi10.gem.field", 1);
}
@@ -38,19 +38,11 @@ void App::disconnectIndication() {
void App::addGem(int x, int y, int pts) {
Gem g;
g.x = x;
g.y = y;
g.coordinate = Vector2D(Vector2D::CreatePoint(x, GRID_SIZE-y));
g.pts = pts;
g.relativePts = computeRelativePts(g);
gems_.append(g);
}
void App::printGem() {
for (Gem g: gems_) {
qDebug() << g.x << ":" << g.y << " - " << g.pts << Qt::endl;
}
}
void App::fillField(QString body) {
int x = 0;
int y = 0;
@@ -58,9 +50,7 @@ void App::fillField(QString body) {
for(int i = 0; i<body.length(); i++) {
const QChar c = body.at(i);
if(c == 'Y') {
myVehicle_.x = x;
myVehicle_.y = y;
myVehicle_.me = true;
myVehicle_ = Vector2D(Vector2D::CreatePoint(x, GRID_SIZE-y));
}
if(c == 'g') addGem(x, y, 100);
if(c == 'G') addGem(x, y, 250);
@@ -74,18 +64,65 @@ void App::fillField(QString body) {
}
}
QVector2D App::computeRelativePts(Gem g) {
QVector2D v = QVector2D(myVehicle_.x, myVehicle_.y) - QVector2D(g.x, g.y);
Vector2D App::computeRelativePts(Gem g) {
Vector2D v = g.coordinate-myVehicle_;
v.reverse();
v.normalize(MAX_LENGHT);
return Vector2D(v.lenght()*g.pts*v.lenght(), v.angle());
}
void App::computeMove() {
Gem myGem;
static const double PI = 3.14159265358979323846;
Vector2D myRelativePts;
for(Gem g : gems_) {
if(g.pts > myGem.pts) myGem = g;
myRelativePts = myRelativePts+computeRelativePts(g);
}
double angle = myRelativePts.angle();
if(angle <0) angle = angle+2*PI;
angle *= 180/PI;
qDebug() << "Angle: " << angle << "°";
qDebug() << "Length: " << myRelativePts.lenght() << Qt::endl;
if(angle > 360) {
qDebug() << "Error angle: " << angle << Qt::endl;
}
else if(angle <= -360) {
qDebug() << "Error angle: " << angle << Qt::endl;
}
else if(angle > 315) { // angle > 315°
st_->sendRequest("/topic/sdi10.gem.command", "right");
}
else if(angle <= -315) { // angle <= -315° || angle <= 45°
st_->sendRequest("/topic/sdi10.gem.command", "right");
}
else if(angle > 225) { // angle > 225°
st_->sendRequest("/topic/sdi10.gem.command", "down");
}
else if(angle <= -225) { // angle <= -225° || angle <= 135°
st_->sendRequest("/topic/sdi10.gem.command", "up");
}
else if(angle > 135) { // angle > 135°
st_->sendRequest("/topic/sdi10.gem.command", "left");
}
else if(angle <= -135) { // angle <= -135° || angle <= 225°
st_->sendRequest("/topic/sdi10.gem.command", "left");
}
else if(angle > 45) { // angle > 45°
st_->sendRequest("/topic/sdi10.gem.command", "up");
}
else if(angle <= -45) { // angle <= -45° || angle <= 315°
st_->sendRequest("/topic/sdi10.gem.command", "down");
}
else {
st_->sendRequest("/topic/sdi10.gem.command", "right");
}
if(myGem.x>myVehicle_.x) st_->sendRequest("/topic/sdi10.gem.command", "right");
else if(myGem.x<myVehicle_.x) st_->sendRequest("/topic/sdi10.gem.command", "left");
else if(myGem.y>myVehicle_.y) st_->sendRequest("/topic/sdi10.gem.command", "down");
else if(myGem.y<myVehicle_.y) st_->sendRequest("/topic/sdi10.gem.command", "up");
}