Arduino comms (#32)

* init commit: raspberry to arduino communication added

* added baud rate and simplified names

* added orientationMsg.proto and start with $

* added in functionality to read in from Arduino

* fixed missing message

* fixed import error

* fixed more bugs and added $ check to stringToMessage

* fixes

* added message

* pi-arduino comms and pi-base comms

* fixes

* navball works

* getting output from pi

* joystick fixes

* testin ploulu servo ctrl

* updated comms

* maestro y u no work

* add controller skeleton

Author: Timothy Tamm

arduinoCommsModule.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+
+import os
+import robomodules as rm
+import serial
+from messages import *
+
+ADDRESS = os.environ.get("BIND_ADDRESS","localhost")
+PORT = os.environ.get("BIND_PORT", 11297)
+
+FREQUENCY = 10
+
+class ArduinoCommsModule(rm.ProtoModule):
+    def __init__(self, addr, port):
+        self.subscriptions = [MsgType.CTRL_MSG]
+        super().__init__(addr, port, message_buffers, MsgType, FREQUENCY, self.subscriptions)
+        try:
+            self.serialConnection = serial.Serial('/dev/ttyACM0', 9600, timeout=0)
+        except Exception:
+            raise RuntimeError("serial connection to Arduino failed")
+
+    def msg_received(self, msg, msg_type):
+        if msg_type == MsgType.CTRL_MSG:
+            arduino_msg = self._stringToBinary(self._messageToString(msg))
+            print(arduino_msg)
+            self.serialConnection.write(arduino_msg)
+
+    def tick(self):
+        line = self.serialConnection.readline()
+        if len(line) > 0:
+            msg = self._stringToMessage(self._binaryToString(line))
+            if msg:
+                self.write(msg.SerializeToString(), MsgType.ORIENTATION_MSG)
+
+    def _messageToString(self, m):
+        ans = "$"
+        for prop in ["x", "y", "z", "roll", "pitch", "yaw", "cameraTilt", "cameraPan"]:
+            ans += (str(getattr(m, prop)) + ";")
+        ans += "\0"
+        return ans
+        
+    def _stringToBinary(self, s):
+        return bytes(s, "ascii")
+        
+    def _binaryToString(self, b):
+        return b.decode('ascii')
+        
+    def _stringToMessage(self, s):
+        ans = OrientationMsg()
+        if s[0] == '$':
+            s = s[1:]
+        else:
+            return None
+        numbers = (s.split(";"))[0:-1]
+        if len(numbers) != 3:
+            return None
+        # assumes the values are coming in in that order
+        for (number, name) in zip(numbers, ["roll", "pitch", "yaw"]):
+            setattr(ans, name, float(number))
+        return ans
+            
+
+def main():
+    module = ArduinoCommsModule(ADDRESS, PORT)
+    module.run()
+
+if __name__ == "__main__":
+    main()

guiModule.py

@@ -13,14 +13,14 @@
 PORT = os.environ.get("BIND_PORT", 11297)
 
 FREQUENCY = 25
-NAVBALL_FREQ = 1
+NAVBALL_FREQ = 5
 FRAME_WIDTH = 640
 SCREEN_SIZE = (FRAME_WIDTH*3, 900)
 NAVBALL_SIZE = 300
 
 class GuiModule(rm.ProtoModule):
     def __init__(self, addr, port):
-        self.subscriptions = [MsgType.CAMERA_FRAME_MSG, MsgType.CTRL_MSG, MsgType.HUMIDITY_MSG]
+        self.subscriptions = [MsgType.CAMERA_FRAME_MSG, MsgType.CTRL_MSG, MsgType.HUMIDITY_MSG, MsgType.ORIENTATION_MSG]
         super().__init__(addr, port, message_buffers, MsgType, FREQUENCY, self.subscriptions)
         self.frames = {}
         pygame.init()
@@ -41,11 +41,13 @@ def msg_received(self, msg, msg_type):
         if msg_type == MsgType.CAMERA_FRAME_MSG:
             self.frames[msg.id] = msg.cameraFrame
         elif msg_type == MsgType.CTRL_MSG:
-            self.roll = msg.roll * 90
-            self.pitch = msg.pitch * 90
-            self.yaw = msg.yaw * 90
+            return 
         elif msg_type == MsgType.HUMIDITY_MSG:
             self.humidity = msg.humidity
+        elif msg_type == MsgType.ORIENTATION_MSG:
+            self.roll = msg.roll
+            self.yaw = msg.yaw + 180
+            self.pitch = msg.pitch + 180
 
     def tick(self):
         for event in pygame.event.get():

ino/client/client.ino

@@ -0,0 +1,210 @@
+#include <Wire.h>
+#include <Servo.h> 
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+#include <utility/imumaths.h>
+
+#define TICKS_PER_WRITE 200
+
+#define PANSERVO 2
+#define TILTSERVO 3
+#define T1 4
+#define T2 5
+#define T3 6
+#define T4 7
+#define T5 8
+#define T6 9
+
+#define FRONT_LEFT 0
+#define FRONT_RIGHT 1
+#define BACK_LEFT 2
+#define BACK_RIGHT 3
+#define SIDE_LEFT 4
+#define SIDE_RIGHT 5
+
+#define THRUSTER_BASE 1500
+#define THRUSTER_MAX 400
+
+struct message {
+    float x;
+    float y;
+    float z;
+    float roll;
+    float pitch;
+    float yaw;
+    float cameraTilt;
+    float cameraPan;
+};
+
+Adafruit_BNO055 bno = Adafruit_BNO055(55);
+int ticks = 0;
+
+Servo panServo;
+Servo tiltServo;
+
+Servo thrusters[6];
+
+int panPos = 90;
+int tiltPos = 90;
+
+struct message curMsg;
+
+void setup(void)
+{
+    Serial.begin(9600);
+  
+    // Initialise the IMU 
+    if(!bno.begin()) {
+        Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+        while(1);
+    }
+  
+    // initialize servos
+    panServo.attach(PANSERVO);
+    tiltServo.attach(TILTSERVO);
+
+    // initialize thrusters
+    thrusters[0].attach(T1);
+    thrusters[1].attach(T2);
+    thrusters[2].attach(T3);
+    thrusters[3].attach(T4);
+    thrusters[4].attach(T5);
+    thrusters[5].attach(T6);
+    
+    thrusters[0].writeMicroseconds(1500);
+    thrusters[1].writeMicroseconds(1500);
+    thrusters[2].writeMicroseconds(1500);
+    thrusters[3].writeMicroseconds(1500);
+    thrusters[4].writeMicroseconds(1500);
+    thrusters[5].writeMicroseconds(1500);
+    
+    delay(1000);
+  
+    bno.setExtCrystalUse(true);
+}
+
+void print_orientation(sensors_event_t event) {
+    Serial.print('$');
+    Serial.print(event.orientation.x);
+    Serial.print(';');
+    Serial.print(event.orientation.y);
+    Serial.print(';');
+    Serial.print(event.orientation.z);
+    Serial.print(';');
+    Serial.print('\n');
+}
+
+int read_message(struct message *msg) {
+    if (Serial.available() == 0) {
+        return 0;
+    }
+    int field = 0;
+    int inChr;
+    String inString = "";
+    // Find start
+    while(true) {
+        inChr = Serial.read();
+        if ((char)inChr == '$') {
+            break;
+        }
+        if(Serial.available() == 0) {
+            return 0;
+        }
+    }
+
+    while (field < 8) {
+        while(Serial.available() == 0);
+        inChr = Serial.read();
+        if ((char)inChr == ';') {
+            inString += '\0';
+            switch(field) {
+                case 0:
+                    msg->x = inString.toFloat();
+                    break;
+                case 1:
+                    msg->y = inString.toFloat();
+                    break;
+                case 2:
+                    msg->z = inString.toFloat();
+                    break;
+                case 3:
+                    msg->roll = inString.toFloat();
+                    break;
+                case 4:
+                    msg->pitch = inString.toFloat();
+                    break;
+                case 5:
+                    msg->yaw = inString.toFloat();
+                    break;
+                case 6:
+                    msg->cameraTilt = inString.toFloat();
+                    break;
+                case 7:
+                    msg->cameraPan = inString.toFloat();
+                    break;
+            }
+            field++;
+            inString = "";
+        } else {
+            inString += (char) inChr;
+        }
+    }
+    return 1;
+}
+
+void simpleController(struct message *msg) {
+    // TODO: ADD controller    
+}
+
+void loop(void)
+{
+    // get sensor event
+    sensors_event_t event;
+    bno.getEvent(&event);
+  
+    // TODO: run PID
+
+    /* Sample: Turn all thrusters on. value has to be in between 1100 and 1900
+    thrusters[0].writeMicroseconds(1700);
+    thrusters[1].writeMicroseconds(1700);
+    thrusters[2].writeMicroseconds(1700);
+    thrusters[3].writeMicroseconds(1700);
+    thrusters[4].writeMicroseconds(1700);
+    thrusters[5].writeMicroseconds(1700); */
+    
+    if (ticks % TICKS_PER_WRITE == 0) {
+        //print_orientation(event);
+    }
+    // read from Serial
+    if (read_message(&curMsg)) {
+        if (curMsg.cameraTilt > 0) {
+            tiltPos+= 10;
+        } else if (curMsg.cameraTilt < 0) {
+            tiltPos-= 10;
+        }
+        if (tiltPos > 360) {
+            tiltPos = 360;
+        } else if (tiltPos < -180) {
+            tiltPos = -180;
+        }
+        
+        if (curMsg.cameraPan > 0) {
+            panPos+= 10;
+        } else if (curMsg.cameraPan < 0) {
+            panPos-= 10;
+        }
+        if (panPos > 360) {
+            panPos = 360;
+        } else if (panPos < -180) {
+            panPos = -180;
+        }
+  
+    }
+    Serial.print(panPos);
+    Serial.print(" ");
+    Serial.println(tiltPos);
+    panServo.writeMicroseconds(1100 + panPos*5);
+    //tiltServo.write(tiltPos);
+    delay(500);
+    ticks++;
+}

ino/Adafruit_BNO055/.github/ISSUE_TEMPLATE.md ino/libs/Adafruit_BNO055/.github/ISSUE_TEMPLATE.md

@@ -9,7 +9,7 @@
 ADDRESS = os.environ.get("BIND_ADDRESS","localhost")
 PORT = os.environ.get("BIND_PORT", 11297)
 
-FREQUENCY = 20
+FREQUENCY = 5
 
 class JoystickModule(rm.ProtoModule):
     def __init__(self, addr, port):
@@ -20,6 +20,8 @@ def __init__(self, addr, port):
         self.pitch = 0.
         self.yaw = 0.
         self.roll = 0.
+        self.camera_tilt = 0.
+        self.camera_pan = 0.
         self.logitech = None
         pygame.init()
         pygame.joystick.init()
@@ -57,6 +59,8 @@ def tick(self):
         msg.pitch = self.pitch
         msg.yaw = self.yaw
         msg.roll = self.roll
+        msg.cameraTilt = self.camera_tilt
+        msg.cameraPan = self.camera_pan
         msg = msg.SerializeToString()
         self.write(msg, MsgType.CTRL_MSG)
 
@@ -72,6 +76,10 @@ def _get_input(self):
         buttonRB = joy.get_button(5)
         buttonLT = joy.get_button(6)
         buttonRT = joy.get_button(7)
+        tpl = joy.get_hat(0)
+
+        self.camera_tilt = tpl[0]
+        self.camera_pan = tpl[1]
 
         # -1 <= up < 0 < down <= +1
         if buttonLB: