Merge branch 'firmware' of github.com:CMU-Robotics-Club/RoboBuggy2 in…

…to main

Merged firmware

.env.dev

@@ -2,3 +2,4 @@ GPS_PORT=/dev/null
 WEBCAM_PORT=/dev/null
 RLSENSE_PORT=/dev/null
 TEENSY_PORT=/dev/null
+FEATHER_PORT=/dev/null

firmware/README.md

@@ -0,0 +1,3 @@
+This folder has all of the firmware code (i.e. code that runs on the Teensy microcontrollers).
+
+Please create a subdirectory for each task - /ROS/TeensyEncoders, /RC-Controller, and so on.

firmware/libraries/Rosserial_Arduino_Library/Dockerfile

@@ -0,0 +1,28 @@
+FROM ros:noetic-ros-base
+
+RUN apt-get update &&\
+  apt-get install -y ros-$ROS_DISTRO-rosserial-arduino ros-$ROS_DISTRO-rosserial git &&\
+  apt-get -y clean &&\
+  apt-get -y purge &&\
+  rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/*
+
+# Create a Catkin Workspace
+SHELL ["/bin/bash", "-c"]
+ENV CATKIN_WS /catkin_ws
+RUN source /opt/ros/$ROS_DISTRO/setup.bash &&\
+  mkdir -p $CATKIN_WS/src &&\
+  cd $CATKIN_WS/ &&\
+  catkin_make
+
+# Build ROS Serial
+RUN source /opt/ros/$ROS_DISTRO/setup.bash &&\
+  cd $CATKIN_WS/src &&\
+  git clone https://github.com/ros-drivers/rosserial.git &&\
+  cd $CATKIN_WS &&\
+  catkin_make &&\
+  catkin_make install
+
+# Create ROS Serial Arduino builder
+RUN source /opt/ros/$ROS_DISTRO/setup.bash &&\
+  cd /tmp &&\
+  rosrun rosserial_arduino make_libraries.py .

firmware/libraries/Rosserial_Arduino_Library/examples/ADC/ADC.pde

@@ -0,0 +1,52 @@
+/* 
+ * rosserial ADC Example
+ * 
+ * This is a poor man's Oscilloscope.  It does not have the sampling 
+ * rate or accuracy of a commerical scope, but it is great to get
+ * an analog value into ROS in a pinch.
+ */
+
+#if (ARDUINO >= 100)
+ #include <Arduino.h>
+#else
+ #include <WProgram.h>
+#endif
+#include <ros.h>
+#include <rosserial_arduino/Adc.h>
+
+ros::NodeHandle nh;
+
+rosserial_arduino::Adc adc_msg;
+ros::Publisher p("adc", &adc_msg);
+
+void setup()
+{ 
+  pinMode(13, OUTPUT);
+  nh.initNode();
+
+  nh.advertise(p);
+}
+
+//We average the analog reading to elminate some of the noise
+int averageAnalog(int pin){
+  int v=0;
+  for(int i=0; i<4; i++) v+= analogRead(pin);
+  return v/4;
+}
+
+long adc_timer;
+
+void loop()
+{
+  adc_msg.adc0 = averageAnalog(0);
+  adc_msg.adc1 = averageAnalog(1);
+  adc_msg.adc2 = averageAnalog(2);
+  adc_msg.adc3 = averageAnalog(3);
+  adc_msg.adc4 = averageAnalog(4);
+  adc_msg.adc5 = averageAnalog(5);
+
+  p.publish(&adc_msg);
+
+  nh.spinOnce();
+}
+

firmware/libraries/Rosserial_Arduino_Library/examples/Blink/Blink.pde

@@ -0,0 +1,29 @@
+/* 
+ * rosserial Subscriber Example
+ * Blinks an LED on callback
+ */
+
+#include <ros.h>
+#include <std_msgs/Empty.h>
+
+ros::NodeHandle  nh;
+
+void messageCb( const std_msgs::Empty& toggle_msg){
+  digitalWrite(LED_BUILTIN, HIGH-digitalRead(LED_BUILTIN));   // blink the led
+}
+
+ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );
+
+void setup()
+{ 
+  pinMode(LED_BUILTIN, OUTPUT);
+  nh.initNode();
+  nh.subscribe(sub);
+}
+
+void loop()
+{  
+  nh.spinOnce();
+  delay(1);
+}
+

firmware/libraries/Rosserial_Arduino_Library/examples/BlinkM/BlinkM.pde

@@ -0,0 +1,162 @@
+/*
+*  RosSerial BlinkM Example
+*  This program shows how to control a blinkm
+*  from an arduino using RosSerial
+*/
+
+#include <stdlib.h>
+
+
+#include <ros.h>
+#include <std_msgs/String.h>
+
+
+//include Wire/ twi for the BlinkM
+#include <Wire.h>
+extern "C" { 
+#include "utility/twi.h" 
+}
+
+#include "BlinkM_funcs.h"
+const byte blinkm_addr = 0x09; //default blinkm address
+
+
+void setLED( bool solid,  char color)
+{
+
+       	if (solid)
+	{
+           switch (color)
+		{
+
+		case 'w':  // white
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0xff,0xff,0xff);  
+			break;
+
+		case 'r': //RED
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0xff,0,0);  
+			break;
+
+		case 'g':// Green
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0,0xff,0);
+			break;
+
+		case 'b':// Blue
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0,0,0xff);
+			break;
+
+		case 'c':// Cyan
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0,0xff,0xff);
+			break;
+
+		case 'm': // Magenta
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0xff,0,0xff);
+			break;
+
+		case 'y': // yellow
+                        BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0xff,0xff,0);
+			break;
+
+		default: // Black
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0,0,0);
+			break;
+		}
+	}
+
+
+	else
+	{
+               	switch (color)
+		{
+		case 'r':  // Blink Red
+                        BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 3,0,0 );
+			break;
+		case 'w':  // Blink white
+                        BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 2,0,0 );
+			break;
+		case 'g':  // Blink Green
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 4,0,0 );
+			break;
+
+		case 'b': // Blink Blue
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 5,0,0 );
+			break;
+
+		case 'c': //Blink Cyan
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 6,0,0 );
+			break;
+
+		case 'm': //Blink Magenta
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 7,0,0 );
+			break;
+
+		case 'y': //Blink Yellow
+                        BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 8,0,0 );
+			break;
+
+		default: //OFF
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 9,0,0 );
+			break;
+		}
+
+	}
+}
+
+void light_cb( const std_msgs::String& light_cmd){
+        bool solid =false;
+        char color; 
+        if (strlen( (const char* ) light_cmd.data) ==2 ){
+          solid  = (light_cmd.data[0] == 'S') || (light_cmd.data[0] == 's');
+          color = light_cmd.data[1];
+        }
+        else{
+          solid=  false;
+          color = light_cmd.data[0];
+        } 
+
+	setLED(solid, color);
+}
+
+
+
+ros::NodeHandle  nh;
+ros::Subscriber<std_msgs::String> sub("blinkm" , light_cb);
+
+
+void setup()
+{
+
+    pinMode(13, OUTPUT); //set up the LED
+
+	BlinkM_beginWithPower();
+	delay(100);
+	BlinkM_stopScript(blinkm_addr);  // turn off startup script
+	setLED(false, 0); //turn off the led
+
+        nh.initNode();
+        nh.subscribe(sub);
+
+}
+
+void loop()
+{
+ nh.spinOnce();
+ delay(1);
+}
+