Skip to content
Snippets Groups Projects
Commit 2bbcb0ac authored by Istreon's avatar Istreon
Browse files

Update

parent 0f08086a
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 337 additions and 111 deletions
.vscode/settings.json
+ 2
− 1
View file @ 2bbcb0ac
{
"files.associations": {
"cmath": "cpp",
"atomic": "cpp"
"atomic": "cpp",
"system_error": "cpp"
}
}
\ No newline at end of file
controllers/mona/build/release/mona.exe
+ 0
− 0
View file @ 2bbcb0ac
No preview for this file type
controllers/mona/build/release/mona.o
+ 0
− 0
View file @ 2bbcb0ac
No preview for this file type
controllers/mona/mona.exe
+ 0
− 0
View file @ 2bbcb0ac
No preview for this file type
controllers/mona/mona.ino
+ 335
− 107
View file @ 2bbcb0ac
......@@ -27,11 +27,14 @@
#define SERVER "192.168.56.1" // IP adress server
#define PORT 10001 // Destination port
#define BUFLEN 475 // Buffer size : Adapt it with the number of robots (For Max 40 robots : message size = 3784 bits / 473 bytes)
#define THRESHOLD 30
#else
#define ID_ROBOT 1
#define BUFLEN 475
#define THRESHOLD 120
#endif
#define BUFFERDECTECT 15
#define MAXROBOTS 40
#define MAX_SPEED 150
#define NBRULES 7
......@@ -60,7 +63,7 @@ struct neighbour{
};
// Nb of neighbour :
int nbNeighbours = 0;
unsigned int nbNeighbours = 0;
// List of neighbour :
neighbour listNeighbours[MAXROBOTS];
......@@ -88,13 +91,22 @@ vector2 acceleration;
vector2 speed;
// Command variable :
uint8_t kind = 0;
unsigned char kind = 0;
// Point to reach (for initialization):
vector2 point;
// Time :
double prevTime = 0;
double nextTime = 0;
double timeStep = 0;
// Buffer detection :
bool bufferDetec[BUFFERDECTECT];
int cpt = 0;
float sum = 0.0;
float moy = 0.0;
// --------------------------------------- Decode functions --------------------------------------- //
// Union of a Float 32 type
union f_u32
......@@ -116,7 +128,6 @@ uint32_t parse_u32_le(char *&buf_p)
f_u32 conv;
memcpy(conv.bts, buf_p, 4);
buf_p += 4;
return conv.i;
}
// Decode float 32 from udp buffer
......@@ -125,12 +136,10 @@ float parse_f32_le(char *&buf_p)
f_u32 conv;
memcpy(conv.bts, buf_p, 4);
buf_p += 4;
return conv.f;
}
// --------------------------------------- Utility functions --------------------------------------- //
// Set the speed to the minimum between of current speed and the max speed :
vector2 MinSpeed(vector2 vec, double max)
{
......@@ -180,6 +189,7 @@ void Set_speed(vector2 vec)
{
float right = 0;
float left = 0;
double norm = VectorSize(vec);
vector2 force = Normalise(vec);
// If we are on Webots, the Z axis is at the opposite from Optitrack
......@@ -198,6 +208,11 @@ void Set_speed(vector2 vec)
{
left = 100;
right = 100 * force.x;
if(norm<1.0)
{
left = left*norm;
right = right*norm;
}
// Green light ON
led(0,100,0);
}
......@@ -205,6 +220,11 @@ void Set_speed(vector2 vec)
{
right = 100;
left = 100 * force.x;
if(norm<1.0)
{
left = left*norm;
right = right*norm;
}
// Green light ON
led(0,100,0);
}
......@@ -225,6 +245,11 @@ void Set_speed(vector2 vec)
{
left = 100;
right = 100 * force.x;
if(norm<1.0)
{
left = left*norm;
right = right*norm;
}
// Green light ON
led(0,100,0);
}
......@@ -232,6 +257,11 @@ void Set_speed(vector2 vec)
{
right = 100;
left = 100 * force.x;
if(norm<1.0)
{
left = left*norm;
right = right*norm;
}
// Green light ON
led(0,100,0);
}
......@@ -262,7 +292,7 @@ vector2 Attraction()
force.x = 0;
force.z = 0;
if(nbNeighbours>0) {
for(int i=0; i< nbNeighbours; i++) {
for(unsigned int i=0; i< nbNeighbours; i++) {
force = Add(force,listNeighbours[i].position);
}
force = Divide(force, nbNeighbours);
......@@ -278,7 +308,7 @@ vector2 Repulsion()
force.x = 0;
force.z = 0;
if(nbNeighbours>0) {
for(int i=0; i< nbNeighbours; i++) {
for(unsigned int i=0; i< nbNeighbours; i++) {
vector2 temp = Mult(listNeighbours[i].position,-1);
force = Add(force,Normalise(temp));
}
......@@ -297,7 +327,7 @@ vector2 Alignment()
ret.z = 0;
float sum = 0;
if(nbNeighbours>0){
for(int i=0; i<nbNeighbours;i++)
for(unsigned int i=0; i<nbNeighbours;i++)
{
sum += listNeighbours[i].angle;
}
......@@ -340,7 +370,7 @@ vector2 CollisionAvoidance()
force.x = 0;
force.z = 0;
if(nbNeighbours>0) {
for(int i=0; i<nbNeighbours; i++)
for(unsigned int i=0; i<nbNeighbours; i++)
{
double dist = VectorSize(listNeighbours[i].position);
if(dist<0.09){
......@@ -371,6 +401,10 @@ vector2 AvoidObstacles(int threshold)
// Obstacle angle :
float angle = 0;
#ifndef _WIN32
bool obst = 0;
#endif
if(c1 || c2 || c3 || c4 || c5)
{
if(c1)
......@@ -397,14 +431,70 @@ vector2 AvoidObstacles(int threshold)
#else
ret.x = cos(angle/(180.0*PI));
ret.z = -sin(angle/(180.0*PI));
obst = 1;
#endif
}
// Force multiplication :
#ifdef _WIN32
ret = Mult(ret, param[6]);
#else
// High frequency filter :
bufferDetec[cpt] = obst;
cpt++;
if(cpt == BUFFERDECTECT){
cpt = 0;
}
if(sizeof(bufferDetec) == BUFFERDECTECT){
moy = 0.0;
sum = 0.0;
for(int i=0; i<BUFFERDECTECT; i++){
sum += bufferDetec[i];
}
moy = sum/BUFFERDECTECT;
}
// Check if there is an obstable :
if(moy>=0.8)
{
ret = Mult(ret, param[6]);
}
else
{
ret.x = 0;
ret.z = 0;
}
#endif
// Return :
return ret;
}
// --------------------------------------- Init function -------------------------------- //
void RobotsInitialisation(vector2 point)
{
Serial.print("X : ");
Serial.println(point.x);
Serial.print("Z : ");
Serial.println(point.z);
if(point.z > 0.01)
{
Set_motors_intensity(0, 100);
}
else if (point.z < -0.01)
{
Set_motors_intensity(100, 0);
}
else if (point.x > 0.0)
{
Set_motors_intensity(100, 100);
}
else
{
Set_motors_intensity(0.0, 0.0);
}
}
// --------------------------------------- Setup --------------------------------------- //
void setup() {
// Random Seed :
......@@ -430,6 +520,9 @@ void setup() {
acceleration.z = 0;
speed.x = 0;
speed.z = 0;
// Point to reach init (for initialisation)
point.x = 0;
point.z = 0;
// Timestep initialisation :
prevTime = millis();
......@@ -452,7 +545,7 @@ void setup() {
{
//Serial.println("Socket created");
}
/* Socket confifuration in non-blocking mode */
// Socket confifuration in non-blocking mode
u_long nonBlocking = 1;
if (ioctlsocket(client_socket, FIONBIO, &nonBlocking) == SOCKET_ERROR) {
Serial.print("Configuration failed..");
......@@ -476,15 +569,16 @@ void setup() {
#else
// Open a UDP connection
udp.begin(localPort);
Serial.print("UDP Client : ");
Serial.print(WiFi.localIP().toString().c_str());
Serial.print(":");
Serial.println(localPort);
// Send a udp packet with ID and IP adress of the robot
udp.beginPacket(server_ip, localPort);
// Serial.print("UDP Client : ");
// Serial.print(WiFi.localIP().toString().c_str());
// Serial.print(":");
// Serial.println(localPort);
// Message construction
char buf[1];
char ID = ID_ROBOT;
sprintf(buf, "%u", ID);
// Send a udp packet with ID and IP adress of the robot
udp.printf(buf);
udp.endPacket();
#endif
......@@ -515,9 +609,6 @@ void loop() {
char *i = answer;
// Get the message type
kind = parse_u8(i);
// Get the number of neighbour
uint32_t neighbors = parse_u32_le(i);
nbNeighbours = neighbors;
// Number of rules
uint8_t nbparam = parse_u8(i);
// For each rules update the intensity
......@@ -525,8 +616,31 @@ void loop() {
{
param[cpt] = parse_f32_le(i);
}
// Depending on the message kind received
switch (int(kind))
{
case 0:{
break;
}
case 1:{
break;
}
case 2:{
// x point
float x = parse_f32_le(i);
point.x = x;
// z point
float z = parse_f32_le(i);
point.z = z;
break;
}
}
// Get the number of neighbour
nbNeighbours = parse_u32_le(i);
// For each neighbour
for (uint32_t cpt = 0; cpt < neighbors; ++cpt)
for (uint32_t cpt = 0; cpt < nbNeighbours; ++cpt)
{
// Get the relative distance X
float x = parse_f32_le(i);
......@@ -534,7 +648,7 @@ void loop() {
float y = parse_f32_le(i);
// Get the relative angle
float angle = parse_f32_le(i);
// List neighbors
// List nbNeighbours
listNeighbours[cpt].position.x = x;
listNeighbours[cpt].position.z = y;
listNeighbours[cpt].angle = angle;
......@@ -556,9 +670,6 @@ void loop() {
char *i = packetBuffer;
// Get the message type
kind = parse_u8(i);
// Get the number of neighbour
uint32_t neighbors = parse_u32_le(i);
nbNeighbours = neighbors;
// Number of rules
uint8_t nbparam = parse_u8(i);
// For each rules update the intensity
......@@ -566,8 +677,32 @@ void loop() {
{
param[cpt] = parse_f32_le(i);
}
// Depending on the message kind received
// Depending on the message kind received
switch (int(kind))
{
case 0:{
break;
}
case 1:{
break;
}
case 2:{
// x point
float x = parse_f32_le(i);
point.x = x;
// z point
float z = parse_f32_le(i);
point.z = z;
break;
}
}
// Get the number of neighbour
nbNeighbours = parse_u32_le(i);
// For each neighbour
for (uint32_t cpt = 0; cpt < neighbors; ++cpt)
for (uint32_t cpt = 0; cpt < nbNeighbours; ++cpt)
{
// Get the relative distance X
float x = parse_f32_le(i);
......@@ -575,21 +710,68 @@ void loop() {
float y = parse_f32_le(i);
// Get the relative angle
float angle = parse_f32_le(i);
// List neighbors
// List nbNeighbours
listNeighbours[cpt].position.x = x;
listNeighbours[cpt].position.z = y;
listNeighbours[cpt].angle = angle;
}
}
#endif
// ==================================== //
// Time Step (in ms):
// Next time Step (in ms):
nextTime = millis();
// Time step computation :
timeStep = nextTime - prevTime;
// TimeStep converstion in s :
timeStep = timeStep / 1000.0;
// Robot command :
switch (kind)
{
// Stop
case 0:{
// Stop motors :
Set_motors_intensity(0,0);
// IR detection
bool obj = false;
for(int i=1; i<6; i++)
{
if(Detect_object(i, THRESHOLD))
{
obj = true;
}
}
// High frequency filter :
bufferDetec[cpt] = obj;
cpt++;
if(cpt == BUFFERDECTECT){
cpt = 0;
}
if(sizeof(bufferDetec) == BUFFERDECTECT){
moy = 0.0;
sum = 0.0;
for(int i=0; i<BUFFERDECTECT; i++){
sum += bufferDetec[i];
}
moy = sum/BUFFERDECTECT;
}
// Set leds :
if(moy>=0.8)
{
led(100,0,0);
}
else
{
led(0,100,0);
}
break;
}
// Flocking, swarming, etc.. :
case 1:{
// Acceleration reset :
acceleration.x = 0;
acceleration.z = 0;
......@@ -601,26 +783,72 @@ void loop() {
acceleration = Add(acceleration, Alignment());
acceleration = Add(acceleration, CollisionAvoidance());
acceleration = Add(acceleration, RandomMovement());
acceleration = Add(acceleration, AvoidObstacles(90));
acceleration = Add(acceleration, AvoidObstacles(THRESHOLD));
// Update Speed
speed = Add(speed,Mult(acceleration,(float)(timeStep)));
speed = MinSpeed(speed, 1.0);
// Set speed
if(kind==1)
{
Set_speed(speed);
break;
}
else
// Initialisation
case 2:{
RobotsInitialisation(point);
break;
}
}
// ================== If we are simulating on Webots ================== //
#ifdef _WIN32
// Setup feedback
char feedback[BUFLEN];
int j;
j = sprintf(feedback, "%u", ROBOT_ID);
j += sprintf(feedback+j, "%s", "_");
j += sprintf(feedback+j, "%u", kind);
j += sprintf(feedback+j, "%s", "_");
j += sprintf(feedback+j, "%u", nbNeighbours);
j += sprintf(feedback+j, "%s", "_");
j += sprintf(feedback+j, "%.2f", speed.x);
j += sprintf(feedback+j, "%s", "_");
j += sprintf(feedback+j, "%.2f", speed.z);
// send the message
if (sendto(client_socket, feedback, strlen(feedback), 0, (sockaddr*)&server, sizeof(sockaddr_in)) == SOCKET_ERROR)
{
Set_motors_intensity(0,0);
Serial.print(Read_IR(3));
Serial.println("sendto() failed with error code: ");
Serial.println(WSAGetLastError());
}
// ================== If we are with real robots ================== //
#else
udp.beginPacket(server_ip, localPort);
// Setup feedback
char feedback[BUFLEN];
int j;
char ID = ID_ROBOT;
j = sprintf(feedback, "%u", ID_ROBOT);
j += sprintf(feedback+j, "%s", "_");
j += sprintf(feedback+j, "%u", kind);
j += sprintf(feedback+j, "%s", "_");
j += sprintf(feedback+j, "%u", nbNeighbours);
j += sprintf(feedback+j, "%s", "_");
j += sprintf(feedback+j, "%.2f", speed.x);
j += sprintf(feedback+j, "%s", "_");
j += sprintf(feedback+j, "%.2f", speed.z);
// send the message
udp.printf(feedback);
udp.endPacket();
#endif
// ==================================== //
// Time save :
prevTime = millis();
// Little delay (same than the Webots basicTimeStep):
#ifndef _WIN32
// Little delay :
delay(10);
#else
delay(10);
delay(32);
#endif
}
\ No newline at end of file
controllers/mona/~BROMIUM/WiFi.dll 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
controllers/supervisor/supervisor.py
+ 0
− 3
View file @ 2bbcb0ac
......@@ -2,8 +2,6 @@
# Import
import socket
import struct
import time
import threading
from dataclasses import dataclass
from random import random
from typing import Any, Tuple
......@@ -105,7 +103,6 @@ def run(handlers):
# Time of the simulated world :
timestep = int(supervisor.getBasicTimeStep())
# While the simulation is still running :
while supervisor.step(timestep) != -1:
# Message constuction :
msg = ''
......
libraries/arduino/Adafruit/~BROMIUM/Adafruit_LIS3MDL.h 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/Adafruit/~BROMIUM/Adafruit_LSM9DS1.h 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/Adafruit/~BROMIUM/Adafruit_Sensor.h 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/WiFi/utility/~BROMIUM/debug.h 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/WiFi/utility/~BROMIUM/server_drv.cpp 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/WiFi/utility/~BROMIUM/server_drv.h 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/WiFi/utility/~BROMIUM/spi_drv.cpp 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/WiFi/utility/~BROMIUM/spi_drv.h 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/WiFi/utility/~BROMIUM/wifi_drv.cpp 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/WiFi/utility/~BROMIUM/wifi_drv.h 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/WiFi/utility/~BROMIUM/wifi_spi.h 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/WiFi/utility/~BROMIUM/wl_definitions.h 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
libraries/arduino/WiFi/utility/~BROMIUM/wl_types.h 0 → 100644
+ 0
− 0
View file @ 2bbcb0ac
File added
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment