WebSVN – svnkaklik – Diff – /roboti/Robotour/SW/vector/vector.cpp

 #define SEVER 122
 char vystup[50];
 pthread_t thread_1, thread_2, thread_3;
 FILE *pRouraO,*pRouraI;
-unsigned int vzdalenost;
+unsigned int vzdalenost, lus, rus;
 char command,ble;
 int param;
 int file;
 double nord, east;
 int done; // vlajka, ze se neco udelalo
 int last_cross; // posledni krizovatka
+int korekce; // Byla korekce podle kompasu nebo zdi
+int bod;
 pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 void *print_tele(void *unused);
 void *gps(void *unused);
 //void *sensors(void *unused);
-void I2C_addr (int Addr)
+void I2C_addr (int Addr)  // vybere adresu cidla se kterym se bude komunikovat
+{
     if (ioctl(file, I2C_SLAVE, Addr) == -1)
+    {
         fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
         exit(-5);
+    }
+}
+int i2c_init()   // zinicializuje i2c
+{
+	file = open("/dev/i2c-0", O_RDWR);
+	if (file < 0)
+	{
+		cerr << "Could not open /dev/i2c-0." << endl;
+		return -1;
+	}
+	return 0;
+}
 void go (int Addr, int speed) // nastavi rychlost motoru
+{
     char Buf[1];
     I2C_addr (Addr);
     Buf[0]=15;
     Buf[1]=0xFF;
     write(file,Buf,2);
+}
-void TL (unsigned char azimut2)
+void TL (unsigned char azimut2) // otoci robota na zadany azimut rotaci vlevo
+{
     unsigned char azimut1;
     go(M1, 0);
     go(M2, 120);
     do
+    {
         azimut1=read_azimut_mag();
 //printf("az1: %d - az2: %d\n", azimut1, azimut2);
         usleep(10000);
-    } while( (azimut1!=azimut2)&&((azimut1+1)!=azimut2)&&((azimut1+2)!=azimut2)&&((azimut1+3)!=azimut2) );
+    } while( (azimut1!=azimut2)&&((azimut1+1)!=azimut2)&&((azimut1+2)!=azimut2)&&((azimut1+3)!=azimut2)&&((azimut1+4)!=azimut2) );
     go(M1, 0);
     go(M2, 0);
+}
-void TR (unsigned char azimut2)
+void TR (unsigned char azimut2) // otoci robota na zadany azimut rotaci v pravo
+{
     unsigned char azimut1;
     go(M1, 120);
     go(M2, 0);
     do
+    {
         azimut1=read_azimut_mag();
         usleep(10000);
-    } while( (azimut1!=azimut2)&&((azimut1-1)!=azimut2)&&((azimut1-2)!=azimut2)&&((azimut1-3)!=azimut2) );
+    } while( (azimut1!=azimut2)&&((azimut1-1)!=azimut2)&&((azimut1-2)!=azimut2)&&((azimut1-3)!=azimut2)&&((azimut1-4)!=azimut2) );
     go(M1, 0);
     go(M2, 0);
+}
+void SlowLeft()
+{
+    go(M1, 40);
+    go(M2, 60);
+}
-void FindNearestCrossing(void)
+void SlowRight()
+{
+    go(M1, 60);
+    go(M2, 40);
+}
+void FindNearestCrossing(void) // vykonej nejakou akci (zatoceni) pokud se robot nachazi pobliz nejakeho waypointu
+{
     int n;
     double dist, pomN, pomE;
+    unsigned char azimut;
     pthread_mutex_lock(&mutex);         // prepis souradnic do pracovnich promennych
     pomN=nord; pomE=east;
     pthread_mutex_unlock(&mutex);
-    if ( GeoCalc::EllipsoidDistance(pomN, pomE, cros[last_cross].n, cros[last_cross].e)>((double)cros[n].dia+5) ) done=0; // znovu naviguj, pokud si dal nez... od krizovatky
+//    if (last_cross!=-1) if ( GeoCalc::EllipsoidDistance(pomN, pomE, cros[last_cross].n, cros[last_cross].e)>((double)cros[n].dia+5) ) done=0; // znovu naviguj, pokud si dal nez... od krizovatky
     n=0;
     do
+    {
         if (0==cros[n].id) break;
         dist=GeoCalc::EllipsoidDistance(pomN, pomE, cros[n].n, cros[n].e);
         if (dist <= (double)cros[n].dia)
+        {
 //printf("Point ID: %d - Distance: %f\n",cros[n].id, dist);
-            if (done==0)
+            if (last_cross!=cros[n].id)
+            {
-                last_cross=n;
+                bod=cros[n].id;
                 #include "nav.h"
-                done=1;
+//                done=1;
+            }
             break;
+        }
     } while(++n<POINTS);
+}
+/*
-int i2c_init()   // zinicializuje i2c
+void FindWay(void) // koriguj smer podle kompasu
+{
+    int n;
+    double dist, pomN, pomE;
+    pthread_mutex_lock(&mutex);         // prepis souradnic do pracovnich promennych
+    pomN=nord; pomE=east;
-	file = open("/dev/i2c-0", O_RDWR);
+    pthread_mutex_unlock(&mutex);
+//    if (last_cross!=-1) if ( GeoCalc::EllipsoidDistance(pomN, pomE, cros[last_cross].n, cros[last_cross].e)>((double)cros[n].dia+5) ) done=0; // znovu naviguj, pokud si dal nez... od krizovatky
-	if (file < 0)
+    n=0;
+    do
-	{
+    {
+        if (0==way[n].id) break;
+        dist=GeoCalc::EllipsoidDistance(pomN, pomE, way[n].n, way[n].e);
-		cerr << "Could not open /dev/i2c-0." << endl;
+        if (dist <= (double)way[n].dia)
+        {
+            if (last_way!=way[n].id)
-		return -1;
+            {
+                last_way=way[n].id;
+                switch (way[n].id)
+                {
+                    case 1:
+                        TL(way[n].azimut);
+                        break;
+                    case 2:
+                        TL(80);
+                        break;
+                };
-	}
+            }
+            break;
-	return 0;
+        }
+    } while(++n<POINTS);
+}
+*/
-void turnL(unsigned char angle, signed char speed)    // otoci robota o zadany uhel
+void turnL(unsigned char angle, signed char speed)    // otoci robota o zadany uhel (otaceni v levo)
+{
 int azimut;
-    go(M1, speed);
+    go(M1, -speed);
-    go(M2, -speed);
+    go(M2, speed);
     azimut=read_azimut_mag();
-    while(read_azimut_mag() >= (azimut + angle)) usleep(10000);
+    while(read_azimut_mag() >= (azimut - angle)) usleep(10000);
     go(M1, 0);
     go(M2, 0);
+}
-void turnR()    // otoci robota o zadany uhel
+void turnR(unsigned char angle, signed char speed)    // otoci robota o zadany uhel (otaceni v pravo)
+{
+int azimut;
+    go(M1, speed);
+    go(M2, -speed);
+    azimut=read_azimut_mag();
+    while(read_azimut_mag() >= (azimut + angle)) usleep(10000);
+    go(M1, 0);
+    go(M2, 0);
+}
 int main(int argc, char *argv[], char *envp[])
+{
     int filtr;
 	fprintf(stdout, "\n **** Starting Vector Control Programm **** \n \r");
     i2c_init();
-    last_cross=0;
+    last_cross=-1;
     command=0;
     filtr=0;
     done=0;
+    korekce=0;
     pthread_create(&thread_1, NULL, print_tele, NULL);
     pthread_create(&thread_3, NULL, gps, NULL);
 //    pthread_create(&thread_2, NULL, sensors, NULL);
                 break;
             case 'g':
     usleep(180000);  // simulace ostatnich  cidel (zdrzeni)
-//!!!KAKL                FindNearestCrossing();
+                FindNearestCrossing();
+//!!!                FindWay();
                 vzdalenost=echo(US3_Addr);
+                usleep(30000);
+                lus=echo(US1_Addr);
+                usleep(30000);
+                rus=echo(US2_Addr);
+                usleep(30000);
                 if ((vzdalenost>60)&&(vzdalenost<80))
+                {
+//FindNearestCrossing();
+//                    if(korekce==0)
+                    {
-                    go(M1, 60);
+                        go(M1, 80);
-                    go(M2, 60);
+                        go(M2, 80);
+                    };
+                    korekce=0;
                     filtr=0;
                     break;
                 };
                 if ((vzdalenost>30)&&(vzdalenost<130))
+                {
+/*
+                    if ((lus<70)&&(lus>30)&&(vzdalenost>60)) // prekazka vlevo
+                    {
+                        go(M1, 60);
+                        go(M2, 40);
+                        break;
+                    };
+*/
+/*
+                    if ((rus<40)&&(rus>30)) // prekazka vpravo
+                    {
+                        go(M1, 40);
+                        go(M2, 60);
+                    };
+*/
                     filtr=0;
+/*
+                    if (vzdalenost<40)
+                    {
+                        go(M1, 40);
+                        go(M2, 60);
+                        break;
+                    }
+                    if (vzdalenost>100)
+                    {
+                        go(M1, 60);
+                        go(M2, 40);
+                        break;
+                    }
+*/
                     if (vzdalenost<55)
+                    {
-                        go(M1, 30);
+                        go(M1, 40);
                         go(M2, 60);
+                    }
                     else
+                    {
                         go(M1, 60);
-                        go(M2, 30);
+                        go(M2, 40);
+                    }
+                }
                 else
+                {
                     filtr++;
         fscanf(pRouraI,"%1s%d", &pom, &param);
         fclose(pRouraI);
         if (pom!='i') command=pom;
         pRouraO = fopen("/home/ble/pipe","w");
-        fprintf(pRouraO,"US: %u cm - ",vzdalenost);
+        fprintf(pRouraO,"US3: %u cm - ",vzdalenost);
         pthread_mutex_lock(&mutex);
         fprintf(pRouraO,"%fN %fE - ", nord, east);
-        fprintf(pRouraO,"Bod:%d - ", last_cross);
+        fprintf(pRouraO,"Bod:%d - ", bod);
         fprintf(pRouraO,"Len:%.1f m /", GeoCalc::EllipsoidDistance(nord, east, cros[last_cross].n, cros[last_cross].e));
         fprintf(pRouraO,"Az:%.2f Deg - ", GeoCalc::GCAzimuth(nord, east, cros[last_cross].n, cros[last_cross].e));
         fprintf(pRouraO,"AzMag: %d (0-255)\n", read_azimut_mag());
+        fprintf(pRouraO,"US1: %u cm - US2 %u cm\n",lus,rus);
         pthread_mutex_unlock(&mutex);
         fclose(pRouraO);
+    }
+}
         pthread_mutex_unlock(&mutex);
         usleep(800000);                     // NMEA nechodi castejc nez 1x za 1s
+    }
+}
 /*
 void *sensors(void *unused)
+{
 	char Buf[64];
     while(true)
+    {
-        I2C_addr(US_Addr);
+        vzdalenost=echo(US3_Addr);
-        Buf[0]=0x0;
-        Buf[1]=0x51;
-        write(file, Buf, 2);
-        usleep(80000);
+        usleep(20000);
-        read(file, Buf, 3);
+        left=echo(US1_Addr);
-        vzdalenost=(Buf[1]*256+Buf[2]);
-        usleep(300000);
+        usleep(20000);
-        I2C_addr(PIC_Addr);
+        right=echo(US1_Addr);
-        Buf[0]=command;
-        write(file, Buf, 1);
-        read(file, Buf, 1);
-        ble=Buf[0];
+        usleep(20000);
+    }
+}
 */

Subversion Repositories svnkaklik

(root)/roboti/Robotour/SW/vector/vector.cpp – Rev 377 → 379