Merge pull request #20 from MissouriMRDT/rovecomm_lowlevel_rewrite

Refactor of RoveComm_Arduino

examples/RoveCommEthernetTCP/RoveCommEthernetTCP.ino

@@ -1,56 +1,73 @@
-#include "RoveComm.h"
-#if defined(ARDUINO_TEENSY41) 
-#include <TimerOne.h>
-#endif
-//rovecomm and packet instances
+#include <RoveComm.h>
+
+#define ROVECOMM_UPDATE_RATE 100
+
+// create rovecomm context
 RoveCommEthernet RoveComm;
-rovecomm_packet packet; 
+// packets can be quite large so better to keep just one around
+RoveCommPacket packet;
 
-//timekeeping variables
-uint32_t last_update_time;
+// timekeeping variable
+uint32_t last_update_time = 0;
 
-//declare the Ethernet Server in the top level sketch with the requisite port ID any time you want to use RoveComm
-EthernetServer TCPServer(RC_ROVECOMM_DRIVEBOARD_PORT);
+// example functions
+void driveRover(float left, float right) {
+    Serial.println("Driving rover!");
+}
+void rotateCamera(uint16_t rotation) {
+    Serial.println("Rotating camera!");
+}
 
-void setup()
-{
-    //start up serial communication
+void setup() {
+    // start up serial communication
     Serial.begin(9600);
 
-    //initialize the ethernet device and rovecomm instance
-    RoveComm.begin(RC_DRIVEBOARD_FOURTHOCTET, &TCPServer);
-    delay(100);
+    // initialize rovecomm
+    Serial.println("Initializing rovecomm...");
+    RoveComm.begin(RC_COREBOARD_IPADDRESS);
+    Serial.println("RoveComm initialized!");
 
-    //update timekeeping
     last_update_time = millis();
 }
 
-void loop()
-{
-  packet = RoveComm.read();
-  Serial.println(packet.data_id);
-  if(packet.data_id != RC_ROVECOMM_NO_DATA_DATA_ID)
-  {
-      //cast the data to the expected data type
-      uint16_t* data = (uint16_t*)packet.data;
-
-      //now print out all of the data
-      for(int i = 0; i < packet.data_count; i++)
-      {
-        Serial.println(data[i]);
-      }
-  }
-
-  //Write some sample data back every 100 milliseconds, it is important that any
-  //telemetry is NOT rate limited (using delays) as this will prevent
-  //packets from arriving in a timely manner 
-
-  if(millis()-last_update_time >= ROVECOMM_UPDATE_RATE)
-  {
-      RoveComm.writeReliable(9600, 1, (uint8_t)1);
-      RoveComm.writeReliable(9600, 1, (uint8_t)2);
-      RoveComm.writeReliable(9600, 1, (uint8_t)3);
-
-      last_update_time = millis();
-  }
-} 
+void loop() {
+    // read newest data into our packet variable
+    // if no data is available or there is no connection, the packet.dataId will be set to RC_ROVECOMM_NO_DATA_DATA_ID
+    // if a packet with an incorrect version makes it into the network, packet.dataID will be RC_ROVECOMM_INVALID_VERSION_DATA_ID
+    // as a convenience, RoveComm.read(packet) will return false if either of these error codes are returned
+    if (RoveComm.read(packet)) {
+        // Print the data id
+        Serial.printf("Received a packet with data id: %d\n", packet.dataId);
+    }
+
+    // change behavior based on packet id
+    // you can find all known dataId's in RoveCommManifest.h
+    switch (packet.dataId) {
+        case RC_COREBOARD_DRIVELEFTRIGHT_DATA_ID:
+        // because we declare a new variable inside this case block, we need to declare a scope for it
+        {
+            // get data from packet - we know from the manifest that the DriveLeftRight will contain 2 floats
+            // packet.data is uint8_t pointer, so we must tell C++ to interpret it as a float pointer instead
+            float *driveSpeeds = (float*)packet.data;
+            driveRover(driveSpeeds[0], driveSpeeds[1]);
+            break;
+        }
+        // No scope is needed since no variables are declared
+        case RC_COREBOARD_LEFTDRIVEGIMBALINCREMENT_DATA_ID:
+            // we know from the manifest that LeftDriveGimbalIncrement will contain 1 uint16_t
+            rotateCamera(((uint16_t*)packet.data)[0]); 
+    }
+
+    // write some sample data back every 100 milliseconds, it is important that any
+    // telemetry is NOT rate limited (using delays) as this will prevent
+    // packets from arriving in a timely manner 
+
+    if (millis() - last_update_time >= ROVECOMM_UPDATE_RATE) {
+        // send to all TCP connections
+        RoveComm.writeReliable(9600, 1, (uint8_t)1);
+        RoveComm.writeReliable(9600, 1, (uint8_t)2);
+        RoveComm.writeReliable(9600, 1, (uint8_t)3);
+
+        last_update_time = millis();
+    }
+} 

examples/RoveCommEthernetUDP/RoveCommUdp.ino

@@ -0,0 +1,85 @@
+//Example of receiving and writing rovecomm data for a mock driveboard
+
+#include <RoveComm.h>
+
+// create rovecomm context
+RoveCommEthernet RoveComm;
+// packets can be quite large so better to keep just one around
+RoveCommPacket packet;
+
+int16_t motorSpeeds[RC_DRIVEBOARD_DRIVESPEEDS_DATA_COUNT] = {-500, 200, 740, -720, 10, -182};
+
+// we will set up a timer to periodically send the current drive speeds (not required)
+void telemetry() {
+    // send over UDP to all subscribed IP addresses
+    RoveComm.write(RC_DRIVEBOARD_DRIVESPEEDS_DATA_ID, RC_DRIVEBOARD_DRIVESPEEDS_DATA_COUNT, motorSpeeds);
+}
+// the hardware timer that will call telemetry() using an interrupt
+#define TELEMETRY_PERIOD 150'000 // microseconds
+IntervalTimer Telemetry;
+
+void setup() {
+    // start up serial communication
+    Serial.begin(9600);
+
+    // initialize rovecomm
+    Serial.println("Initializing rovecomm...");
+    RoveComm.begin(RC_COREBOARD_IPADDRESS);
+    Serial.println("RoveComm initialized!");
+
+    Telemetry.begin(telemetry, TELEMETRY_PERIOD);
+}
+
+void loop() {
+    // read newest data into our packet variable
+    // if no data is available or there is no connection, the packet.dataId will be set to RC_ROVECOMM_NO_DATA_DATA_ID
+    // if a packet with an incorrect version makes it into the network, packet.dataID will be RC_ROVECOMM_INVALID_VERSION_DATA_ID
+    // as a convenience, RoveComm.read(packet) will return false if either of these error codes are returned
+    if (RoveComm.read(packet)) {
+        // Print the data id
+        Serial.printf("Received a packet with data id: %d\n", packet.dataId);
+    }
+
+    // change behavior based on packet id
+    // you can find all known dataId's in RoveCommManifest.h
+    switch(packet.data_id) {
+        // because we declare a new variable inside this case block, we need to declare a scope for it
+        case RC_DRIVEBOARD_DRIVEINDIVIDUAL_DATA_ID:
+        {
+            // cast the packet to the correct data type
+            int16_t *speeds = (int16_t*)packet.data;
+
+            Serial.println("We received an individual wheel drive command:");
+            for (int i = 0; i < RC_DRIVEBOARD_DRIVESPEEDS_DATA_COUNT; i++) {
+                Serial.print(motorSpeeds[i]);
+                Serial.print(", ");
+            }
+
+            // set the motor speeds to the commanded speeds in RoveComm
+            motorSpeeds[0] = speeds[0];
+            motorSpeeds[1] = speeds[1];
+            motorSpeeds[2] = speeds[2];
+            motorSpeeds[3] = speeds[3];
+            motorSpeeds[4] = speeds[4];
+            motorSpeeds[5] = speeds[5];
+            break;
+        }
+        case RC_DRIVEBOARD_DRIVELEFTRIGHT_DATA_ID:
+        {
+            // cast the packet to the correct data type
+            int16_t *speeds = (int16_t*)packet.data;
+
+            Serial.println("We received a left/right drive command:");
+            Serial.printf("Left: %d, Right: %d\n", speeds[0], speeds[1]);
+
+            // set the motor speeds to the commanded speeds in RoveComm
+            motorSpeeds[0] = speeds[0];
+            motorSpeeds[1] = speeds[0];
+            motorSpeeds[2] = speeds[0];
+            motorSpeeds[3] = speeds[1];
+            motorSpeeds[4] = speeds[1];
+            motorSpeeds[5] = speeds[1];
+            break;
+        }
+    }
+}