Rs485 fix

rosserial_arduino/src/rosserial_arduino/__init__.py

@@ -1 +1 @@
-from SerialClient import *
+from .SerialClient import *

rosserial_vex_v5/CMakeLists.txt

@@ -18,11 +18,11 @@ install(
 )
 
 install(
-  DIRECTORY launch/
+  DIRECTORY launch/launch/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
-catkin_install_python(
-  PROGRAMS src/${PROJECT_NAME}/make_libraries.py
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
+#catkin_install_python(
+#  PROGRAMS src/${PROJECT_NAME}/make_libraries.py
+#  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+#)

rosserial_vex_v5/README.md

@@ -58,7 +58,7 @@ The project can exist anywhere (it does NOT need to be inside the ROS workspace)
 
 ```bash
 source ~/ros-vex-workspace/install/setup.bash
-rosrun rosserial_vex_v5 genscript.sh ~/path/to/prosproject
+rosrun rosserial_vex_v5 genproject.sh ~/path/to/prosproject
 ```
 
 # Examples

rosserial_vex_v5/scripts/genproject.sh

@@ -1,5 +1,5 @@
 # generate new pros project with name as first argument
-prosv5 conductor new-project $1
+prosv5 conduct new-project $1
 
 # go into incude directory and generate ros message library.
 cd $1/include

rosserial_vex_v5/src/examples/main.cpp

@@ -0,0 +1,62 @@
+#include "main.h"
+#include "helloworld.h"
+
+/**
+ * Runs initialization code. This occurs as soon as the program is started.
+ *
+ * All other competition modes are blocked by initialize; it is recommended
+ * to keep execution time for this mode under a few seconds.
+ */
+void initialize() {}
+
+/**
+ * Runs while the robot is in the disabled state of Field Management System or
+ * the VEX Competition Switch, following either autonomous or opcontrol. When
+ * the robot is enabled, this task will exit.
+ */
+void disabled() {}
+
+/**
+ * Runs after initialize(), and before autonomous when connected to the Field
+ * Management System or the VEX Competition Switch. This is intended for
+ * competition-specific initialization routines, such as an autonomous selector
+ * on the LCD.
+ *
+ * This task will exit when the robot is enabled and autonomous or opcontrol
+ * starts.
+ */
+void competition_initialize() {}
+
+/**
+ * Runs the user autonomous code. This function will be started in its own task
+ * with the default priority and stack size whenever the robot is enabled via
+ * the Field Management System or the VEX Competition Switch in the autonomous
+ * mode. Alternatively, this function may be called in initialize or opcontrol
+ * for non-competition testing purposes.
+ *
+ * If the robot is disabled or communications is lost, the autonomous task
+ * will be stopped. Re-enabling the robot will restart the task, not re-start it
+ * from where it left off.
+ */
+void autonomous() {}
+
+/**
+ * Runs the operator control code. This function will be started in its own task
+ * with the default priority and stack size whenever the robot is enabled via
+ * the Field Management System or the VEX Competition Switch in the operator
+ * control mode.
+ *
+ * If no competition control is connected, this function will run immediately
+ * following initialize().
+ *
+ * If the robot is disabled or communications is lost, the
+ * operator control task will be stopped. Re-enabling the robot will restart the
+ * task, not resume it from where it left off.
+ */
+void opcontrol() {
+	while (true) {
+		setup();
+
+		pros::delay(20);
+	}
+}

rosserial_vex_v5/src/ros_lib/V5RS485.h

@@ -0,0 +1,99 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _ROSSERIAL_VEX_V5_V5_V5RS485_H_
+#define _ROSSERIAL_VEX_V5_V5_V5RS485_H_
+
+#include "pros/apix.h"
+
+class V5RS485 {
+   public:
+    V5RS485(int readPortNum = 19, int writePortNum = 20, int baud = 115200)
+        {
+            readPort = new pros::Serial(readPortNum);
+            writePort = new pros::Serial(writePortNum);
+            readPort->set_baudrate(baud);
+            writePort->set_baudrate(baud);
+        }
+
+    void init() {
+        pros::delay(20);
+        readPort->flush();
+        pros::delay(100);
+        writePort->flush();
+    }
+
+    // read a byte from the serial port. -1 = failure
+    int read() { 
+        int32_t read = readPort->read_byte();
+        if(read == PROS_ERR){
+            std::cout << pros::millis() << " Error read " << read << std::endl;
+        } 
+        return read;
+    }
+
+    // write data to the connection to ROS
+    void write(uint8_t* data, int length) { 
+        int freeBytes = writePort->get_write_free();
+        if(freeBytes > length) { // Enough bytes free in buffer
+            writePort->write(data, length);
+            //This delay is necessary to prevent too many bytes being sent at once
+            //This will prevent send errors
+            //Without this the current scheduler will not properly space out the writes
+            //A different solution to this issue would be to edit the scheduler to minimize no-ops
+            pros::delay(2);
+        } else {
+            printf("Serial buffer full!\n");
+            writePort->write(data, freeBytes);
+            pros::delay(2);
+            for(int i = freeBytes; i < length; i++) {
+                while(writePort->get_write_free() == 0) {
+                    pros::delay(5);
+                }
+                writePort->write_byte(data[i]);
+                pros::delay(3);
+            }
+        }
+    }
+
+    // returns milliseconds since start of program
+    unsigned long time() { return pros::c::millis(); }
+
+   private:
+   //These need to be pointers
+    pros::Serial *readPort = nullptr;
+    pros::Serial *writePort = nullptr;
+};
+
+#endif

rosserial_vex_v5/src/ros_lib/V5Hardware.h → rosserial_vex_v5/src/ros_lib/V5Serial.h

@@ -1,4 +1,4 @@
-/* 
+/*
  * Software License Agreement (BSD License)
  *
  * Copyright (c) 2011, Willow Garage, Inc.
@@ -32,13 +32,12 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef _ROSSERIAL_VEX_V5_V5_HARDWARE_H_
-#define _ROSSERIAL_VEX_V5_V5_HARDWARE_H_
+#ifndef _ROSSERIAL_VEX_V5_V5_SERIAL_H_
+#define _ROSSERIAL_VEX_V5_V5_SERIAL_H_
 
-#include "main.h"
 #include "ros_lib/rosserial_vex_v5/utils/RingBuf.h"
 
-// for the mutex.
+ // for the mutex.
 #include "pros/apix.h"
 
 #define SERIAL_CLASS int
@@ -49,77 +48,78 @@ using RB = RingBufCPP<char, ROSVEX_BUFFER_INPUT_SIZE>;
 // load the serial reading into a buffer.
 inline void vexRosBufferInput(void* arg) {
 
-  void** arglist = (void**) arg;
-  RB* inputBuffer = (RB*) arglist[0];
-  __FILE* streamOut = (__FILE*) arglist[1];
+  void** arglist = (void**)arg;
+  RB* inputBuffer = (RB*)arglist[0];
+  __FILE* streamOut = (__FILE*)arglist[1];
 
   int readcount = 0;
-  while(1) {
-    char c =  fgetc(streamOut);
+  while (1) {
+    char c = fgetc(streamOut);
     inputBuffer->add(c);
   }
 }
 
-class V5Hardware {
+class V5Serial {
 
-  public:
-    V5Hardware(): rosvexMutex(), inputBuffer(rosvexMutex), failCount(), successCount() {
-    }
+public:
+  V5Serial() : rosvexMutex(), inputBuffer(rosvexMutex), failCount(), successCount() {
+  }
 
-    // any initialization code necessary to use the serial port
-    // note: the serial port initialization for rosserial for VEX Cortex must be implemented in `src/init.cpp` 
-    // see that file for more information. 
-    void init() {
-      serctl(SERCTL_DISABLE_COBS, NULL);
-      rosFile = fopen("/ser/sout", "r+");
+  // any initialization code necessary to use the serial port
+  // note: the serial port initialization for rosserial for VEX Cortex must be implemented in `src/init.cpp` 
+  // see that file for more information. 
+  void init() {
+    pros::c::serctl(SERCTL_DISABLE_COBS, NULL);
+    rosFile = fopen("/ser/sout", "r+");
+    pros::c::fdctl(fileno(rosFile), SERCTL_DEACTIVATE, NULL);
 
-      // not typesafe, be careful!
-      void** taskArgs = (void**) malloc( sizeof(void*) * 2);
-      taskArgs[0] = &inputBuffer;
-      taskArgs[1] = rosFile;
+    // not typesafe, be careful!
+    void** taskArgs = (void**)malloc(sizeof(void*) * 2);
+    taskArgs[0] = &inputBuffer;
+    taskArgs[1] = rosFile;
+
+    pros::Task reader(vexRosBufferInput, taskArgs);
+  }
 
-      pros::Task reader(vexRosBufferInput, taskArgs);
+  // read a byte from the serial port. -1 = failure
+  int read() {
+    char c;
+    // pull serial reading out of the buffer.
+    if (inputBuffer.pull(&c)) {
+      char sucmsg[16];
+      return c;
     }
 
-    // read a byte from the serial port. -1 = failure
-    int read() {
-      char c;
-      // pull serial reading out of the buffer.
-      if(inputBuffer.pull(&c)) {
-        char sucmsg[16];
-        return c;
-      }
+    return -1;
+  }
 
-      return -1;
+  // write data to the connection to ROS
+  void write(uint8_t* data, int length) {
+    for (int i = 0; i < length; i++) {
+      vexroswritechar(data[i]);
     }
+  }
+  // returns milliseconds since start of program
+  unsigned long time() {
+    return pros::c::millis();
+  }
+private:
+  int failCount;
+  int successCount;
+  pros::Mutex rosvexMutex;
+  __FILE* rosFile;
+  RB inputBuffer;
+
+  // writing helper.
+  void vexroswritechar(uint8_t data) {
+    fputc(data, rosFile);
+    fflush(rosFile);
+  }
 
-    // write data to the connection to ROS
-    void write(uint8_t* data, int length) {
-      for(int i = 0; i < length; i++) {
-        vexroswritechar(data[i]);
-      }
-    }
-    // returns milliseconds since start of program
-    unsigned long time() {
-      return pros::c::millis();
-    }
-  private:
-    int failCount;
-    int successCount;
-    pros::Mutex rosvexMutex;
-    __FILE * rosFile;
-    RB inputBuffer;
-
-    // writing helper.
-    void vexroswritechar(uint8_t data) {
-      fputc(data, rosFile);
-      fflush(rosFile);
-    }
-
-    // reading helper.
-    char vexrosreadchar() {
-      return fgetc(rosFile);
-    }
-    };
+  // reading helper.
+  char vexrosreadchar() {
+    return fgetc(rosFile);
+  }
+};
 
 #endif