Keuronde/Robot2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
38b81e8c6c
Robot2025/Triangulation/Triangulation.ino

450 lines
15 KiB
C++
Raw Blame History

/*
Test de triangulation
*/
#include <M5Core2.h>
#include <math.h>
#include <WiFi.h>
const char* ssid = "riombotique";
const char* password = "password";
IPAddress local_IP(192, 168, 99, 102);
IPAddress gateway(192, 168, 99, 1);
IPAddress subnet(255, 255, 255, 0);
WiFiServer server(80);
boolean reading = false;
String Lecture;
#define CONV 0.000628318530717959
#define CONV2 1.570796326794896619
#define pi 3.141592653589793
#define pi2 6.283185307179586
int Balise[4][2]; // 4 balises potentielles i de 0 a 3
// Frequence modulation individuelle balises
// Balise[i][0] est la frequence de la balise
// Balise[i][1] est l'angle de la balise
bool Balise_Valide;
int A_Depart_Balise;
int A_Fin_Balise;
long Temps_Tour = millis();
long T_Depart_Balise = 0;
long T_Fin_Balise = 0;
long Frequence_Balise = 0;
long Nb_Pulses = 0;
long Chrono = 0;
int Angle = 0;
int Angle_Balise = 0;
int Nb_Balises = 0;
int Capteur = 2;
bool Trigger_Balises = true;
bool Calcul_Valide = false;
bool Balise_0 = false;
bool Balise_1 = false;
bool Balise_2 = false;
int Nb_tours = 0;
int Old_Nb_tours = 0;
int Pulse_Moteur = 190;
int Frequence_0 = 5000; // frequence balise 0
int Frequence_1 = 6000; // frequence balise 1
int Frequence_2 = 4000; // frequence balise 2
int Bande_P = 200; // bande passante balises
int X1 = 0; // abcisse balise 0
int X2 = 0; // abcisse balise 1
int X3 = 2400; // abcisse balise 2
int Y1 = 2400; // ordonnee balise 0
int Y2 = 0; // ordonnee balise 1
int Y3 = 0; // ordonnee balise 2
int Xp1 = X1 - X2;
int Yp1 = Y1 - Y2;
int Xp3 = X3 - X2;
int Yp3 = Y3 - Y2;
int Xr, Yr, Angle_Robot_Deg_int;
int alignement = 2200;
float Angle_Robot_RAD, Angle_Robot_Deg, Angle_B1, angle_B1_calc, Angle_Ref_Theorique, calc1, calc2, offset;
void IRAM_ATTR fonction_AB(){ // fonction de comptage des pas du codeur angulaire
Angle ++;
if (Trigger_Balises){
Capteur = 2;
if (Angle > 1316) Capteur = 1;
if (Angle > 2633) Capteur = 2;
if (Angle > 4300) Capteur = 1;
if (Angle > 5966) Capteur = 2;
if (Angle > 7933) Capteur = 1;
if (Angle > 9299) Capteur = 2;
}
if (((Angle - A_Fin_Balise) > 100) && (!Trigger_Balises)){
if (Nb_Pulses > 20){ // analyse de la balise si plus de 10 pulses uniquement
// calcul de la frequence moyenne et validation de la balise
int Delta_T = (T_Fin_Balise - T_Depart_Balise) / 10;
Frequence_Balise = 100000 * (Nb_Pulses);
if (Delta_T > 0) Frequence_Balise = Frequence_Balise / Delta_T;
if (A_Fin_Balise < A_Depart_Balise) A_Fin_Balise = A_Fin_Balise + 10000;
Angle_Balise = (A_Fin_Balise + A_Depart_Balise) / 2;
if (Angle_Balise > 10000) Angle_Balise = Angle_Balise - 10000;
if ((Frequence_Balise > (Frequence_0 - Bande_P)) && (Frequence_Balise < (Frequence_0 + Bande_P)) && !Balise_0) {
Balise[0][0] = Frequence_Balise;
Balise[0][1] = Angle_Balise;
Nb_Balises ++;
Balise_0 = true; // la balise a ete vue, plus possible de la voir a nouveau
}
if ((Frequence_Balise > (Frequence_1 - Bande_P)) && (Frequence_Balise < (Frequence_1 + Bande_P)) && !Balise_1) {
Balise[1][0] = Frequence_Balise;
Balise[1][1] = Angle_Balise;
Nb_Balises ++;
Balise_1 = true; // la balise a ete vue, plus possible de la voir a nouveau
}
if ((Frequence_Balise > (Frequence_2 - Bande_P)) && (Frequence_Balise < (Frequence_2 + Bande_P)) && !Balise_2) {
Balise[2][0] = Frequence_Balise;
Balise[2][1] = Angle_Balise;
Nb_Balises ++;
Balise_2 = true; // la balise a ete vue, plus possible de la voir a nouveau
}
}
Trigger_Balises = true;
}
}
void IRAM_ATTR fonction_Z(){ // appelée à chaque tour
Nb_tours ++;
Angle = 0;
}
void IRAM_ATTR fonction_Balise_capteur_1(){ // appelée à chaque detection de baslise capteur 1
if (Capteur == 1) {
A_Fin_Balise = Angle;
// effectue a partir du 2eme pulse
Chrono = micros();
Nb_Pulses++;
T_Fin_Balise = Chrono;
// effectue au 1er pulse
if (Trigger_Balises){
A_Depart_Balise = Angle;
T_Depart_Balise = Chrono;
Nb_Pulses = 0;
Trigger_Balises = false;
//est-ce bien une balise modulee ? -> mesure du temps avant apparition de la prochaine impulsion impusion
}
}
}
void IRAM_ATTR fonction_Balise_capteur_2(){ // appelée à chaque detection de baslise capteur 2
if (Capteur == 2) {
A_Fin_Balise = Angle;
// effectue a partir du 2eme pulse
Chrono = micros();
Nb_Pulses++;
T_Fin_Balise = Chrono;
// effectue au 1er pulse
if (Trigger_Balises){
A_Depart_Balise = Angle;
T_Depart_Balise = Chrono;
Nb_Pulses = 0;
Trigger_Balises = false;
//est-ce bien une balise modulee ? -> mesure du temps avant apparition de la prochaine impulsion impusion
}
}
}
void IRAM_ATTR traitement_donnees() {
// calcul des coordonnees .........................
Calcul_Valide = false;
double A1 = (Balise[1][1] - Balise[0][1]) * CONV;
double A2 = (Balise[2][1] - Balise[1][1]) * CONV;
if(sin(A1)==0 || sin(A2)==0) return;
double T12 = cos(A1) / sin(A1);
double T23 = cos(A2) / sin(A2);
if ((T12 + T23)==0) return;
double T31 = (1 - (T12 * T23)) / (T12 + T23);
double Xp12 = Xp1 + (T12 * Yp1);
double Yp12 = Yp1 - (T12 * Xp1);
double Xp23 = Xp3 - (T23 * Yp3);
double Yp23 = Yp3 + (T23 * Xp3);
double Xp31 = (Xp3 + Xp1) + (T31 * (Yp3 - Yp1));
double Yp31 = (Yp3 + Yp1) - (T31 * (Xp3 - Xp1));
double Kp31 = (Xp1 * Xp3) + (Yp1 * Yp3) + (T31 * ((Xp1 * Yp3) - (Xp3 * Yp1)));
double D = ((Xp12 - Xp23) * (Yp23 - Yp31)) - ((Yp12 - Yp23) * (Xp23 - Xp31));
if (D==0) return;
Xr = X2 + ((Kp31 * (Yp12 - Yp23)) / D);
Yr = Y2 + ((Kp31 * (Xp23 - Xp12)) / D);
Calcul_Valide = true;
}
void setup() {
Serial.begin(115200);
M5.begin();
M5.Axp.SetSpkEnable(0);
// moteur
ledcSetup(0, 1000, 8);
ledcAttachPin(2, 0);
ledcWrite(0, Pulse_Moteur);
// I2C
Wire.setPins(32, 33);
I2C_Slave_init(0x30);
M5.lcd.setTextSize(2);
//Initialisation wifi
//------------WIFI------------
//Initialisation wifi
WiFi.config(local_IP, gateway, subnet);
WiFi.begin(ssid, password);
int test_wifi = 0;
while (WiFi.status() != WL_CONNECTED){
delay(300);
M5.Lcd.print(".");
test_wifi ++;
if (test_wifi > 10) break;
}
if (WiFi.status() == WL_CONNECTED) {
server.begin();
delay(500);
M5.Lcd.clear();
M5.Lcd.setCursor(10,10);
M5.Lcd.print("Connecte au reseau ;-)");
M5.Lcd.setCursor(10,30);
M5.Lcd.print(ssid);
}
else {
// le routeur riombotique n'a pas été trouvé - création d'un point d'accès
WiFi.mode(WIFI_OFF);
if (!WiFi.softAP("triangulation", "ilestsecret")) {
log_e("Soft AP creation failed.");
while(1);
}
IPAddress myIP = WiFi.softAPIP();
server.begin();
M5.Lcd.clear();
M5.Lcd.setCursor(10,10);
M5.Lcd.print("Creation Hotspot ;-)");
M5.Lcd.setCursor(10,30);
M5.Lcd.print("triangulation 192.168.4.1");
M5.Lcd.setCursor(10,50);
M5.Lcd.print("mdp : ilestsecret");
}
delay(1000);
M5.Lcd.clear();
// --------------------
pinMode(19, INPUT_PULLDOWN); // entree pulse tour codeur
pinMode(27, INPUT_PULLDOWN); // entree pulse angle codeur
pinMode(25, INPUT_PULLDOWN); // entree phototransistor capteur 1
pinMode(26, INPUT_PULLDOWN); // entree phototransistor capteur 2
attachInterrupt(27, fonction_AB, RISING); // appel fonction AB a chaque pulse codeur
attachInterrupt(19, fonction_Z, RISING); // appel fonction Z a chaque tour codeur
attachInterrupt(25, fonction_Balise_capteur_1, FALLING); // appel fonction Balise capteur 1 a chaque detection phototransistor
attachInterrupt(26, fonction_Balise_capteur_2, FALLING); // appel fonction Balise capteur 2 a chaque detection phototransistor
M5.Lcd.setCursor(10,10);
M5.lcd.print(" Salut Riombotique");
delay(500);
M5.Lcd.clear();
//M5.Lcd.setCursor(10,10);
//M5.Lcd.print("Nb tours : ");
//M5.Lcd.setCursor(10,30);
//M5.Lcd.print("Balise : ");
M5.Lcd.setCursor(10,0);
M5.Lcd.print("Angle 1 : ");
M5.Lcd.setCursor(10,20);
M5.Lcd.print("Angle 2 : ");
M5.Lcd.setCursor(10,40);
M5.Lcd.print("Angle 3 : ");
//M5.Lcd.setCursor(10,110);
//M5.Lcd.print("Frequence 1 : ");
//M5.Lcd.setCursor(10,130);
//M5.Lcd.print("Frequence 2 : ");
//M5.Lcd.setCursor(10,150);
//M5.Lcd.print("Frequence 3 : ");
M5.Lcd.setCursor(0,60);
M5.Lcd.print("A float = ");
M5.Lcd.setCursor(0,80);
M5.Lcd.print("a RAD = ");
M5.Lcd.setCursor(0,100);
M5.Lcd.print("angle = ");
M5.Lcd.setCursor(160,180);
M5.Lcd.print("X = ");
M5.Lcd.setCursor(160,210);
M5.Lcd.print("Y = ");
}
void loop() {
//Effectue a chaque tour ........................
if ((Old_Nb_tours != Nb_tours) && (Trigger_Balises)){
Old_Nb_tours = Nb_tours;
if (!Balise_Valide){
Balise_Valide = true;
rapport();
}
//_____________________________________________ gestion I2C et Data sent
if(Xr>0){
Angle_B1 = ((Balise[0][1] + 10000 - alignement) % 10000) * CONV; // lorsque le robot est orienté vers la balise, angle codeur = 0
//calc1 = (Y1-Yr)*1000/Xr;
//Angle_Ref_Theorique = atan(calc1/1000); // angle théorique entre axe X et alignement vers balise depuis la position X, Y
calc1 = Xr*1000/(Y1-Yr);
Angle_Ref_Theorique = atan(calc1/1000); // angle théorique entre axe Y et alignement vers balise depuis la position X, Y
//calc2 = fmod(((Angle_B1/pi2*360)+360 - ((CONV2-Angle_Ref_Theorique)/pi2*360)),360); // angle codeur - (90°-angle theorique mais sur Y) modulo 360° et oui le modulo ne prend que de l'entier...
calc2 = fmod(((Angle_B1/pi2*360)+360 - (Angle_Ref_Theorique/pi2*360)),360); // angle codeur - angle theorique sur Y) modulo 360° et oui le modulo ne prend que de l'entier...
Angle_Robot_RAD = calc2/360*pi2; // en radian
Angle_Robot_Deg_int = calc2; // arrondi des degres dans un entier
Angle_Robot_Deg = calc2; // angle d'orientation robot en degres (0 sur l'axe Y)
}
uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3);
I2C_envoi_8bits(etat_balises,0);
// TODO: Mettre Xr et Yr dans la mémoire I2C ################################
I2C_envoi_32bits(Xr, 1);
I2C_envoi_32bits(Yr, 5);
//I2C_envoi_32bits(Angle_Robot_RAD, 9);
I2C_envoi_32bits(Angle_Robot_Deg_int, 9);
//_____________________________________________
if (Nb_Balises == 3) traitement_donnees(); //calcul des coordonnees si 3 balises
affichage_resultats();
checkForClient();
Nb_Balises = 0;
Balise_0 = false;
Balise_1 = false;
Balise_2 = false;
for (int i = 0; i <= (2); i++) {
//Remise à zero des balises
Balise[i][0] = 0;
Balise[i][1] = 0;
}
vitesse_moteur();
}
//________________________________________
}
void rapport(){
Serial.print("Frequence ");
Serial.print(Frequence_Balise);
Serial.print(" / Pulses ");
Serial.print(Nb_Pulses);
Serial.print("/ Angle 1 ");
Serial.print(A_Depart_Balise);
Serial.print("/ Angle 2 ");
Serial.println(A_Fin_Balise);
}
void vitesse_moteur() {
if ((millis() - Temps_Tour) < 500){ //250 millisecondes par tour correspond à la vitesse de rotation desiree
Pulse_Moteur ++;
}
else {
Pulse_Moteur --;
}
ledcWrite(0, Pulse_Moteur); // Ajustement de la vitesse
Temps_Tour = millis();
}
void affichage_resultats() {
// affichage des resultats .........................
//M5.Lcd.setCursor(140,10);
//M5.Lcd.print(Nb_tours);
//M5.Lcd.print(" ");
//M5.Lcd.setCursor(140,30);
//M5.Lcd.print(Nb_Balises);
//M5.Lcd.print(" ");
M5.Lcd.setCursor(140,0);
M5.Lcd.print(Balise[0][1]);
//M5.Lcd.print(" / ");
//M5.Lcd.print(Angle_B[0][3]);
M5.Lcd.print(" ");
M5.Lcd.setCursor(140,20);
M5.Lcd.print(Balise[1][1]);
//M5.Lcd.print(" / ");
// M5.Lcd.print(Angle_B[1][3]);
M5.Lcd.print(" ");
M5.Lcd.setCursor(140,40);
M5.Lcd.print(Balise[2][1]);
// M5.Lcd.print(" / ");
// M5.Lcd.print(Angle_B[2][3]);
M5.Lcd.print(" ");
//M5.Lcd.setCursor(200,110);
//M5.Lcd.print(Balise[0][0]);
//M5.Lcd.print(" ");
//M5.Lcd.setCursor(200,130);
//M5.Lcd.print(Balise[1][0]);
//M5.Lcd.print(" ");
//M5.Lcd.setCursor(200,150);
//M5.Lcd.print(Balise[2][0]);
//M5.Lcd.print(" ");
M5.Lcd.setCursor(150,60);
M5.Lcd.print(Angle_B1);
M5.Lcd.setCursor(150,80);
M5.Lcd.print(Angle_Ref_Theorique);
M5.Lcd.setCursor(150,100);
M5.Lcd.print(Angle_Robot_Deg);
M5.Lcd.print(" ° ");
M5.Lcd.setCursor(200,180);
M5.Lcd.print(Xr);
M5.Lcd.print(" mm ");
M5.Lcd.setCursor(200,210);
M5.Lcd.print(Yr);
M5.Lcd.print(" mm ");
}
void checkForClient(){
WiFiClient client = server.available();
if (client) {
while (client.connected()) {
if (client.available()) {
char c = client.read();
if (c == '\n') {
// fin du message
client.println("HTTP/1.1 200 OK");
client.println("Content-Type: text/html");
client.println("Connection: close");
client.println();
client.println("<!DOCTYPE HTML>");
client.println("<meta http-equiv='Refresh' content='2'>");
client.print("<html>");
//client.print("Nb tours : ");
//client.print(Nb_tours);
//client.print("<BR>");
//client.print("Balise : ");
//client.print(Nb_Balises);
//client.print("<BR>");
client.print("Angle 1 : ");
client.print(Balise[0][1]);
client.print("<BR>");
client.print("Angle 2 : ");
client.print(Balise[1][1]);
client.print("<BR>");
client.print("Angle 3 : ");
client.print(Balise[2][1]);
client.print("<BR>");
//client.print("Frequence 1 : ");
//client.print(Balise[0][0]);
//client.print("<BR>");
//client.print("Frequence 2 : ");
//client.print(Balise[1][0]);
//client.print("<BR>");
//client.print("Frequence 3 : ");
//client.print(Balise[2][0]);
//client.print("<BR>");
client.print("X = ");
if (Xr < 3000) client.print(Xr);
client.println(" mm");
client.print("<BR>");
client.print("Y = ");
if (Yr < 3000) client.print(Yr);
client.println(" mm");
client.println("</html>");
break;
}
}
else {
break;
}
}
delay(10); // give the web browser time to receive the data
client.stop(); // clos la connection
delay(10);
}
}
Reference in New Issue View Git Blame Copy Permalink