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"
 using namespace std;
 #define CMPS03_SOFTWARE_REVISION 0x0
 #define SRF02_SOFTWARE_REVISION 0x0
 #define BC_Addr	    0x0B
 #define US_Addr	    0x70 // 0xE0 in fact
-#define M1_Addr	0x50 // 0xA0 in fact
+#define M1	0x50 // 0xA0 in fact
-#define M2_Addr	0x51 // 0xA2 in fact
+#define M2	0x51 // 0xA2 in fact
-char vystup[30];
+char vystup[50];
-pthread_t thread_1, thread_2;
+pthread_t thread_1, thread_2, thread_3;
 FILE *pRouraO,*pRouraI;
 unsigned int vzdalenost;
 char command,ble;
 int file;
+double n, e;
+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)
+{
+    char Buf[1];
+    I2C_addr (Addr);
+    Buf[0]=speed;
+    write(file, Buf, 1);
+}
 int main(int argc, char *argv[], char *envp[])
+{
 	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;
+	}
     pthread_create(&thread_1, NULL, print_tele, NULL);
 //    pthread_create(&thread_2, NULL, sensors, NULL);
     char Buf[64];
-    command=127;
+    command=0;
     while(true)
+    {
+        switch (command)
+        {
+            case 'f':   // forward
+                go(M1, 20);
+                go(M2, 20);
+                command=0;
+                break;
+            case 'b':   // backward
+                go(M1, -20);
+                go(M2, -20);
+                command=0;
+                break;
+            case 's':   // stop
+                go(M1, 0);
+                go(M2, 0);
+                command=0;
+                break;
+            case 'g':
+usleep(180000);
-        I2C_addr(US_Addr);
+                I2C_addr(US_Addr);
-        Buf[0]=0x0;
+                Buf[0]=0x0;
-        Buf[1]=0x51;
+                Buf[1]=0x51;
-        write(file, Buf, 2);
+                write(file, Buf, 2);
-        usleep(80000);
+                usleep(80000);
-        read(file, Buf, 3);
+                read(file, Buf, 3);
-        vzdalenost=(Buf[1]*256+Buf[2]);
+                vzdalenost=(Buf[1]*256+Buf[2]);
-        usleep(300000);
-        if (command!=127)
+                if ((vzdalenost>50)&&(vzdalenost<80))
+                {
-            I2C_addr(M1_Addr);
+                    go(M1, 20);
+                    go(M2, 20);
-            Buf[0]=command;
+                    break;
+                };
+                if ((vzdalenost>30)&&(vzdalenost<120))
+                {
+                    if (vzdalenost<50)
+                    {
-            write(file, Buf, 1);
+                        go(M1, 20);
-            I2C_addr(M2_Addr);
+                        go(M2, 40);
+                    }
-            Buf[0]=command;
+                    else
+                    {
+                        go(M1, 40);
+                        go(M2, 20);
+                    }
+                }
+                else
+                {
+                    go(M1, 0);  // zastav, neni videt doprovod
-            write(file, Buf, 1);
+                    go(M2, 0);
+                };
-            command=127;
+                break;
+        }
     };
 	close(file);
     pthread_join(thread_1, NULL);
     pthread_join(thread_2, NULL);
 	return 0;
+}
 void *print_tele(void *unused)
+{
-    char i;
+    char string[2];
     while(true)
+    {
         pRouraI = fopen("/home/ble/pipe","r");
-        i=fgetc(pRouraI);
+        command=fgetc(pRouraI);
-        switch (i)
-        {
-            case 'a':
-                command=0;
+        string[0]=command;
-                break;
-            case 'b':
-                command=20;
-                break;
-            case 'c':
-                command=-20;
-                break;
+        string[1]=0;
-        }
         fclose(pRouraI);
         pRouraO = fopen("/home/ble/pipe","w");
-        fprintf(pRouraO,"Vzdalenost: %u cm Command: 0x%X\n",vzdalenost,command);
+        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(49.266667, 14.7, 49.266667, 14.716667)*1000);
+   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, 49.266667, 14.716667));
-	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(49.266667, 14.7, 49.266667, 14.716667));
+	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, 49.266667, 14.716667));
+    pthread_mutex_unlock(&mutex);
         fclose(pRouraO);
+    }
+}
+void *gps(void *unused)
+{
+    FILE *pRS232;
+    double pomN, pomE;
+    pRS232 = fopen("/dev/ttyS1","r");
+    while(true)
+    {
+        fscanf(pRS232,"$GPGGA,*,%lf,N,%lf,E", &pomN, &pomE);
+        pthread_mutex_lock(&mutex);
+        n=pomN; e=pomE;
+        pthread_mutex_unlock(&mutex);
+        usleep(500000);
+    }
+}
 /*
 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 @ 369 – Rev 366 → 367