Merge pull request #2 from flochre/release/1.2

Release/1.2

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.9)
-project(serial_megapi VERSION 1.1 DESCRIPTION "lib to communicate with the megapi over serial")
+project(serial_megapi VERSION 1.2 DESCRIPTION "lib to communicate with the megapi over serial")
 include(GNUInstallDirs)
 
 set(package serial_megapi)
@@ -17,15 +17,20 @@ add_compile_options(-pipe)
 add_library(${PROJECT_NAME} SHARED 
     src/serial_megapi.c
 )
+target_link_libraries(${PROJECT_NAME} wiringPi wiringPiDev pthread m crypt rt)
 
 add_executable(serialTest examples/serialTest.c)
 target_link_libraries(serialTest ${PROJECT_NAME} wiringPi wiringPiDev pthread m crypt rt)
 
+add_executable(serialTestExt examples/serialTest_external.c)
+target_link_libraries(serialTestExt ${PROJECT_NAME} wiringPi wiringPiDev pthread m crypt rt)
+
 file(GLOB_RECURSE PUBLIC_INCLUDE_FILES "include/*.h*")
 
 target_include_directories(${PROJECT_NAME} PRIVATE .)
 target_include_directories(${PROJECT_NAME} PUBLIC include)
 target_include_directories(serialTest PUBLIC include)
+target_include_directories(serialTestExt PUBLIC include)
 target_include_directories(${PROJECT_NAME} PRIVATE src)
 
 set_target_properties(${PROJECT_NAME} PROPERTIES

examples/serialTest_external.c

@@ -0,0 +1,125 @@
+/*
+ * serialTest.c:
+ *	Very simple program to test the serial port. Expects
+ *	the port to be looped back to itself
+ *
+ * Copyright (c) 2012-2013 Gordon Henderson. <[email protected]>
+ ***********************************************************************
+ * This file is part of wiringPi:
+ *	https://projects.drogon.net/raspberry-pi/wiringpi/
+ *
+ *    wiringPi is free software: you can redistribute it and/or modify
+ *    it under the terms of the GNU Lesser General Public License as published by
+ *    the Free Software Foundation, either version 3 of the License, or
+ *    (at your option) any later version.
+ *
+ *    wiringPi 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 Lesser General Public License for more details.
+ *
+ *    You should have received a copy of the GNU Lesser General Public License
+ *    along with wiringPi.  If not, see <http://www.gnu.org/licenses/>.
+ ***********************************************************************
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <wiringPi.h>
+#include <wiringSerial.h>
+
+#include "serial_megapi.h"
+
+int main ()
+{
+  int fd;
+  int count ;
+  unsigned int nextTime ;
+
+  // if ((fd = serialOpen ("/dev/ttyAMA0", 115200)) < 0)
+  // {
+  //   fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno)) ;
+  //   return 1 ;
+  // }
+
+  // if (wiringPiSetup () == -1)
+  // {
+  //   fprintf (stdout, "Unable to start wiringPi: %s\n", strerror (errno)) ;
+  //   return 1 ;
+  // }
+
+  if ((fd = init_serial("/dev/ttyAMA0", 115200)) < 0){
+    fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno)) ;
+    return 1 ;
+  }
+
+  nextTime = millis () + 300 ;
+
+  for (count = 0 ; count < 256 ; )
+  {
+    if (millis () > nextTime)
+    {
+      printf ("\nOut: %3d: %d", count, fd) ;
+      fflush (stdout) ;
+
+      // write (fd, gyro_msg, 8) ;
+      // write (fd, uss_msg, USS_MSG_SIZE) ;
+
+      int my_test_speed = 20;
+
+      if(5 == count){
+        set_speed(fd, 2, my_test_speed);
+        set_speed(fd, 3, -my_test_speed);
+      }
+
+      if(105 == count){
+        set_speed(fd, 2, 0);
+        set_speed(fd, 3, 0);
+      }
+
+      if(120 == count){
+        set_speed(fd, 2, -my_test_speed);
+        set_speed(fd, 3, my_test_speed);
+      }
+
+      if(220 == count){
+        set_speed(fd, 2, 0);
+        set_speed(fd, 3, 0);
+      }
+
+      if(0 == count % 2){
+        // printf ("\nOut Gyro: %f: ", get_gyro(fd, GYRO_AXE_X));  
+        // request_gyro_all_axes(fd);
+        // request_gyro(fd, GYRO_ALL_AXES);
+        // request_gyro(fd, GYRO_AXE_Z);
+        // request_motor_pos(fd, 2);
+        request_motor_speed(fd, 3);
+        // request_motor_speed(fd, 2); 
+      } else {
+        // printf ("\nOut USS : %f: ", get_uss(fd, 0x7));
+        // request_uss(fd, 0x7);
+        // request_motor_pos(fd, 1);  
+        // request_motor_speed(fd, 1); 
+        request_motor_speed(fd, 2); 
+        // request_motor_speed(fd, 3); 
+        // request_motor_speed(fd, 4); 
+      }
+
+      // printf( "gyro_x : %f / gyro_z : %f / uss_cm : %f \n", get_gyro_x(), get_gyro_z(), get_uss(7));
+
+      nextTime += 300 ;
+      ++count ;
+    }
+
+    // delay (3);
+
+    // receive_msg(fd);
+
+  }
+
+  printf ("\n") ;
+  return 0 ;
+}

include/serial_megapi.h

@@ -1,8 +1,16 @@
 #ifndef SERIAL_MEGAPI_H_
 #define SERIAL_MEGAPI_H_
 
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <wiringPi.h>
+#include <wiringSerial.h>
+
 #define MAKEBLOCK_MSG_SIZE  0xa
-#define MKBLK_MAX_MSG_SIZE  0x14
+#define MKBLK_MAX_MSG_SIZE  0x37
 
 #define HEADER_MSG_SIZE     0x3
 
@@ -15,6 +23,7 @@
 #define GYRO_PORT           0x0
 
 #define SET_MOTOR_MSG_SIZE  0x7
+#define RESET_MOTOR_MSG_SIZE 0x2
 #define READ_MOTOR_MSG_SIZE 0x6
   //Secondary command
   #define READ_MOTOR_POS    0x01
@@ -39,32 +48,55 @@
 #define USS_LAST_SLOT       USS_FIRST_SLOT + USS_MAX_NB - 1
 
 #define DATA_TYPE_FLOAT     0x2
+#define DATA_TYPE_DOUBLE    0x5
 #define DATA_TYPE_LONG      0x6
 
 #define ACTION_GET          0x1
 #define ACTION_RUN          0x2
 #define ACTION_RESET        0x4
 #define ACTION_START        0x5
 
+#define MOTOR_MUTEX         0x0
+#define IMU_MUTEX           0x1
+#define USS_MUTEX           0x2
+#define OTHER_MUTEX         0x3
+
 #ifdef __cplusplus
 extern "C" {
 #endif
-float get_gyro_x();
-float get_gyro_y();
-float get_gyro_z();
+
+int init_gyro(const int fd, char port);
+int stop_gyro(const int fd, char port);
+int init_serial(const char * device, int baud);
+int init_two_motors_info(const int fd, char motor_1, char motor_2);
+
+int is_gyro_new_data();
+int is_motor_new_data();
+
+float get_gyro_roll();
+float get_gyro_pitch();
+float get_gyro_yaw();
+float get_gyro_angular_x();
+float get_gyro_angular_y();
+float get_gyro_angular_z();
+float get_gyro_linear_x();
+float get_gyro_linear_y();
+float get_gyro_linear_z();
+
 int   get_motor_position(int port);
 float get_motor_speed(int port);
 float get_uss(int port);
 
-int request_gyro_all_axes(const int fd);
-int request_gyro(const int fd, char axis);
+// int request_gyro_all_axes(const int fd);
+// int request_gyro(const int fd, char axis);
 int request_motor_position(const int fd, char motor);
 int request_motor_speed(const int fd, char motor);
 int request_two_motors_pos_speed(const int fd, char motor_1, char motor_2);
 int request_uss(const int fd, char port);
 
 void receive_msg(const int fd);
 
+int reset_motors(const int fd);
 int set_speed(const int fd, char motor, int speed);
 
 #ifdef __cplusplus