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

 /*****************************************************************************/
 /*
  *  vector.cpp - Control program for Vector robot
+ *
  *      Copyright (C) 2007 KAKL
+ *
  *      This program is free software; you can redistribute it and/or modify
  *      it under the terms of the GNU General Public License as published by
  *      the Free Software Foundation; either version 2 of the License, or
  *      (at your option) any later version.
+ *
  *      This program is distributed in the hope that it will be useful,
  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *      GNU General Public License for more details.
+ *
  */
 /*****************************************************************************/
 #include <iostream>
 #include <getopt.h>
 #include <errno.h>
 #include <string.h>
 #include <pthread.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
 #include "linux/i2c-dev.h"
 #include "linux/i2c.h"
 #include <sys/ioctl.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
 #include "geocalc.h"
 #include "track.h"
 using namespace std;
-#define CMPS03_SOFTWARE_REVISION 0x0
-#define SRF02_SOFTWARE_REVISION 0x0
 #define BC_Addr	    0x0B
-#define US3_Addr	    0x70 // 0xE0 in fact; Sonar na doprovod
+#define US3_Addr	0x70    // 0xE0 in fact; Sonar na doprovod
+#define CMPS_Addr   0x60    // 0xC0
 #define M1	0x50 // 0xA0 in fact
 #define M2	0x51 // 0xA2 in fact
+#define SEVER 0
 char vystup[50];
 pthread_t thread_1, thread_2, thread_3;
 FILE *pRouraO,*pRouraI;
 unsigned int vzdalenost;
 char command,ble;
 int file;
 double n, e;
+unsigned char azimut_mag;
 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)
+{
     if (ioctl(file, I2C_SLAVE, Addr) == -1)
+    {
         fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
         exit(-5);
+    }
+}
-void go (int Addr, int speed)
+void go (int Addr, int speed) // nastavi rychlost motoru
+{
     char Buf[1];
     I2C_addr (Addr);
     Buf[0]=speed;
     write(file, Buf, 1);
+}
-unsigned int echo(int Addr)
+unsigned int echo(int Addr)  // precte vzdalenost z US cidla
+{
     char Buf[3];
     I2C_addr(Addr);
     Buf[0]=0x0;
     Buf[1]=0x51;
     write(file, Buf, 2);
     usleep(80000);
     read(file, Buf, 3);
     return (Buf[1]*256+Buf[2]);
+}
 void FindNearestCrossing(void)
+{
     int n;
     n=0;
     do
+    {
         if (0==cros[n].id) break;
         if (GeoCalc::EllipsoidDistance(n, e, cros[n].n, cros[n].e) <= cros[n].dia)
+        {
             #include "nav.h"
             break;
+        }
     } while(++n<POINTS);
+}
-int main(int argc, char *argv[], char *envp[])
+int i2c_init()   // zinicializuje i2c
+{
-    int filtr;
-	fprintf(stdout, "**** Vector Control Programm ****\n");
 	file = open("/dev/i2c-0", O_RDWR);
 	if (file < 0)
+	{
 		cerr << "Could not open /dev/i2c-0." << endl;
 		return -1;
+	}
+	return 0;
+}
+int read_azimut_mag()  // precte azimut z kompasu
+{
+char Buf[3];      // promena pro manipulaci s i2c
+    I2C_addr(CMPS_Addr);
+    Buf[0]=1;
+    write(file,Buf,1);
+    read(file, Buf,1);
+    return Buf[0];
+}
+void turnL(unsigned char angle, signed char speed)    // otoci robota o zadany uhel
+{
+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);
+    command=0;
+}
+void turnR()    // otoci robota o zadany uhel
+{
+}
+int main(int argc, char *argv[], char *envp[])
+{
+    int filtr;
+    signed char test;
+	fprintf(stdout, "\n **** Starting Vector Control Programm **** \n \r");
+    i2c_init();
     pthread_create(&thread_1, NULL, print_tele, NULL);
     pthread_create(&thread_3, NULL, gps, NULL);
 //    pthread_create(&thread_2, NULL, sensors, NULL);
     command=0;
     filtr=0;
     while(true)
+    {
         switch (command)
+        {
             case 'f':   // forward
                 go(M1, 70);
                 go(M2, 70);
                 command=0;
                 break;
             case 'b':   // backward
                 go(M1, -70);
                 go(M2, -70);
                 command=0;
                 break;
+            case 'v':   // volnobeh
+                go(M1, -128);
+                go(M2, -128);
+                command=0;
+                break;
-            case 'v':   // stop
+            case 's':   // stop
                 go(M1, 0);
                 go(M2, 0);
                 command=0;
                 break;
-            case 's':   // stop
+            case 'a':   // test otaceni
+                go(M1, 100);
+                go(M2, -100);
+                azimut_mag=read_azimut_mag();
+                while(read_azimut_mag() >= (azimut_mag + 45)) usleep(10000);
-                go(M1, 1);
+                go(M1, 0);
-                go(M2, 1);
+                go(M2, 0);
                 command=0;
                 break;
+            case 't':   // test
+                for(test=0;test<127;test++)
+                {
+                    go(M1, test);
+                    go(M2, test);
+                    usleep(10000);
+                };
+                go(M1, 127);
+                go(M2, 127);
+                for(test=127;test>-128;test--)
+                {
+                    go(M1, test);
+                    go(M2, test);
+                    usleep(10000);
+                };
+                go(M1, -128);
+                go(M2, -128);
+                command=0;
+                break;
             case 'g':
-usleep(180000);
+    usleep(180000);  // simulace ostatnich  cidel (zdrzeni)
+                azimut_mag=read_azimut_mag();
+                FindNearestCrossing();
                 vzdalenost=echo(US3_Addr);
                 if ((vzdalenost>60)&&(vzdalenost<80))
+                {
                     go(M1, 60);
                     go(M2, 60);
                     filtr=0;
                     break;
                 };
                 if ((vzdalenost>30)&&(vzdalenost<130))
+                {
                     filtr=0;
                     if (vzdalenost<55)
+                    {
                         go(M1, 30);
                         go(M2, 60);
+                    }
                     else
+                    {
                         go(M1, 60);
                         go(M2, 30);
+                    }
+                }
                 else
+                {
                     filtr++;
                     if (filtr>5)
+                    {
                         go(M1, 1);  // zastav, neni videt doprovod
                         go(M2, 1);
                         filtr=6;
+                    }
                 };
-                FindNearestCrossing();
                 break;
+        }
     };
 	close(file);
     pthread_join(thread_1, NULL);
     pthread_join(thread_2, NULL);
     pthread_join(thread_3, NULL);
 	return 0;
+}
 void *print_tele(void *unused)
+{
     char string[2];
     while(true)
+    {
         pRouraI = fopen("/home/ble/pipe","r");
         command=fgetc(pRouraI);
         string[0]=command;
         string[1]=0;
         fclose(pRouraI);
         pRouraO = fopen("/home/ble/pipe","w");
         fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);
     pthread_mutex_lock(&mutex);
    	fprintf(pRouraO,"%f N %f E\n", n, e);
    	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));
 	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, cros[1].n, cros[1].e));
+	fprintf(pRouraO,"AzimutMag: %d (0-255)\n", azimut_mag);
     pthread_mutex_unlock(&mutex);
         fclose(pRouraO);
+    }
+}
 void *gps(void *unused)
+{
     FILE *pRS232;
     double N, E, pomN, pomE, nn, ee;
     while(true)
+    {
         pRS232 = fopen("/dev/ttyS1","r");
         fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA
         fclose(pRS232);
         nn=ldiv((long)N,100).quot;   // prepocet DDMM.MM na DD.DD
         pomN=(N-nn*100)/60+nn;
         ee=ldiv((long)E,100).quot;
         pomE=(E-ee*100)/60+ee;
         pthread_mutex_lock(&mutex);         // prepis souradnic do sdilenych promennych
         n=pomN; e=pomE;
         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);
         Buf[0]=0x0;
         Buf[1]=0x51;
         write(file, Buf, 2);
         usleep(80000);
         read(file, Buf, 3);
         vzdalenost=(Buf[1]*256+Buf[2]);
         usleep(300000);
         I2C_addr(PIC_Addr);
         Buf[0]=command;
         write(file, Buf, 1);
         read(file, Buf, 1);
         ble=Buf[0];
+    }
+}
 */

Subversion Repositories svnkaklik

(root)/roboti/Robotour/SW/vector/vector.cpp @ 377 – Rev 371 → 372