Add IMU, Servo, Neopixel

esp/Imu/.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

esp/Imu/.travis.yml

@@ -0,0 +1,67 @@
+# Continuous Integration (CI) is the practice, in software
+# engineering, of merging all developer working copies with a shared mainline
+# several times a day < https://docs.platformio.org/page/ci/index.html >
+#
+# Documentation:
+#
+# * Travis CI Embedded Builds with PlatformIO
+#   < https://docs.travis-ci.com/user/integration/platformio/ >
+#
+# * PlatformIO integration with Travis CI
+#   < https://docs.platformio.org/page/ci/travis.html >
+#
+# * User Guide for `platformio ci` command
+#   < https://docs.platformio.org/page/userguide/cmd_ci.html >
+#
+#
+# Please choose one of the following templates (proposed below) and uncomment
+# it (remove "# " before each line) or use own configuration according to the
+# Travis CI documentation (see above).
+#
+
+
+#
+# Template #1: General project. Test it using existing `platformio.ini`.
+#
+
+# language: python
+# python:
+#     - "2.7"
+#
+# sudo: false
+# cache:
+#     directories:
+#         - "~/.platformio"
+#
+# install:
+#     - pip install -U platformio
+#     - platformio update
+#
+# script:
+#     - platformio run
+
+
+#
+# Template #2: The project is intended to be used as a library with examples.
+#
+
+# language: python
+# python:
+#     - "2.7"
+#
+# sudo: false
+# cache:
+#     directories:
+#         - "~/.platformio"
+#
+# env:
+#     - PLATFORMIO_CI_SRC=path/to/test/file.c
+#     - PLATFORMIO_CI_SRC=examples/file.ino
+#     - PLATFORMIO_CI_SRC=path/to/test/directory
+#
+# install:
+#     - pip install -U platformio
+#     - platformio update
+#
+# script:
+#     - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N

esp/Imu/.vscode/extensions.json

@@ -0,0 +1,10 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}

esp/Imu/README.md

@@ -0,0 +1,22 @@
+Postup kalibrace imu esp32
+
+1. Zkompilovat a nahrát testovací fw do esp32 (#define CALIBRATE)
+2. Přípojit k terminálu (115200 bd)
+3. Postavit na vodorovnou podložku a nepohybovat deskou (robotem)
+4. Resetovat esp (druhé tlačítko od usb konektoru)
+5. Cca po 3 sekundách se objeví kalibrační hodnoty akcelerometru a gyra
+6. Zapsat do config.h (pro daného robota)
+   např.:
+    #define ACC_BIAS    {-0.60, -58.37, -13.21}
+    #define GYRO_BIAS   {-2.15, 0.25, -0.16}
+7. Spustit aplikaci MotionCal
+    https://learn.adafruit.com/adafruit-sensorlab-magnetometer-calibration/magnetic-calibration-with-motioncal
+8. Připojit COMx a Clear
+9. Otáčet robotem tak, aby se pokryla celá koule (viz. popis výše)
+10. Zapsat do config.h
+    např.:
+    #define MAG_BIAS    {3.99, 9.87, 6.42}
+    #define MAG_CORECT  {{0.999, 0.008, 0.002}, {0.008, 1.027, 0.009}, {0.002, 0.009, 0.974}}
+11. Zkompilovat a nahrát testovací fw do esp32 bez kalibrace(// #define CALIBRATE)
+12. Připojit k terminálu a otestovat
+

esp/Imu/data/imu.dat

@@ -0,0 +1,4 @@
+-0.60, -58.37, -13.21
+-2.15, 0.25, -0.16
+3.99, 9.87, 6.42
+0.999, 0.008, 0.002, 0.008, 1.027, 0.009, 0.002, 0.009, 0.974

esp/Imu/include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

esp/Imu/platformio.ini

@@ -0,0 +1,18 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:esp32doit-devkit-v1]
+platform = espressif32
+board = esp32doit-devkit-v1
+framework = arduino
+lib_extra_dirs = ../lib
+build_flags = -I../common
+; upload_port = COM8
+monitor_speed = 115200

esp/Imu/src/main.cpp

@@ -0,0 +1,47 @@
+#include <Wire.h>
+#include "IMU.h"
+
+#define S_SCL   33
+#define S_SDA   32
+
+#define CALIBRATION
+
+IMU imu;
+
+void setup() { 
+  Wire2.begin(S_SDA, S_SCL, 400000);
+  Serial.begin(115200);
+  delay(1000);
+
+  #ifdef CALIBRATION
+    imu.init(1);
+    imu.autoCalibrate();
+  #else
+    imu.init();
+  #endif  
+  Serial.println("/-------------------------------------------------------------/");
+}
+
+#define PERIOD  100
+uint32_t timeStamp;
+
+float roll, pitch, yaw;
+
+uint32_t lastTime = 0;
+
+void loop() {
+  if (millis() - timeStamp >= PERIOD) {
+    imu.eulerAngles(roll, pitch, yaw);
+    timeStamp = millis();
+#ifndef CALIBRATION
+    Serial.printf("Roll: %6.2f Pitch: %6.2f Yaw: %6.2f Mag: %6.2f\n\r", roll, pitch, yaw, imu.mg);
+#else
+//  Data for magnetometer calibration by MotionCal
+//  "Raw:0,0,0,0,0,0,mx,my,mz\n\r" mx, my, mz 0.1uT
+    uint8_t x = imu.readData();
+    if (x) {
+      Serial.printf("Raw:0,0,0,0,0,0,%d,%d,%d\n\r", (int)(imu.m[0]*10), (int)(imu.m[1]*10), (int)(imu.m[2]*10));
+    }
+#endif
+  }
+}

esp/Imu/test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PIO Unit Testing and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PIO Unit Testing:
+- https://docs.platformio.org/page/plus/unit-testing.html

esp/Robot/lib/Ina219/INA219_WE.cpp

@@ -220,18 +220,22 @@ void INA219_WE::powerUp(){
 
 #ifndef USE_TINY_WIRE_M_
 uint8_t INA219_WE::writeRegister(uint8_t reg, uint16_t val){
+    _wire->lock();
     _wire->beginTransmission(i2cAddress);
     uint8_t lVal = val & 255;
     uint8_t hVal = val >> 8;
     _wire->write(reg);
     _wire->write(hVal);
     _wire->write(lVal);
-    return _wire->endTransmission();
+    uint8_t ret = _wire->endTransmission(); 
+    _wire->unlock();
+    return ret;
 }
 
 uint16_t INA219_WE::readRegister(uint8_t reg){
     uint8_t MSByte = 0, LSByte = 0;
     uint16_t regValue = 0;
+    _wire->lock();
     _wire->beginTransmission(i2cAddress);
     _wire->write(reg);
     _wire->endTransmission(false);
@@ -240,6 +244,7 @@ uint16_t INA219_WE::readRegister(uint8_t reg){
         MSByte = _wire->read();
         LSByte = _wire->read();
     }
+    _wire->unlock();
     regValue = (MSByte<<8) + LSByte;
     return regValue;
 }

esp/Robot/lib/Ina219/INA219_WE.h

@@ -26,12 +26,7 @@
 #endif
 #include "ina219_config.h"
 
-#ifdef USE_TINY_WIRE_M_
- #include <TinyWireM.h>
-#endif
-#ifndef USE_TINY_WIRE_M_
- #include <Wire.h>
-#endif
+#include "i2c.h"
 
 typedef enum INA219_ADC_MODE{
     BIT_MODE_9      = 0b00000000,   
@@ -85,8 +80,8 @@ class INA219_WE
 
         // Constructors: if not passed 0x40 / Wire will be set as address / wire object
 #ifndef USE_TINY_WIRE_M_   
-        INA219_WE(const uint8_t addr = 0x40) : _wire{&Wire}, i2cAddress{addr} {}
-        INA219_WE(TwoWire *w, const uint8_t addr = 0x40) : _wire{w}, i2cAddress{addr} {}
+        INA219_WE(const uint8_t addr = 0x40) : _wire{&Wire2}, i2cAddress{addr} {}
+        INA219_WE(I2C *w, const uint8_t addr = 0x40) : _wire{w}, i2cAddress{addr} {}
 #else
         INA219_WE(const uint8_t addr = 0x40) : i2cAddress{addr} {}
 #endif   
@@ -118,7 +113,7 @@ class INA219_WE
         INA219_PGAIN devicePGain;
         INA219_BUS_RANGE deviceBusRange;
 #ifndef USE_TINY_WIRE_M_    
-        TwoWire *_wire;
+        I2C *_wire;
 #endif
         uint8_t i2cAddress;
         uint16_t calVal;

esp/Robot/src/communication.cpp

@@ -21,7 +21,7 @@ void Comm::send(uint8_t message, void* data, uint8_t length) {
   transmit(&tx, length + 2);
 }
 
-void Comm::send(uint8_t status, uint8_t mode, uint16_t voltage, uint16_t current, int16_t speed, float angle, int32_t encoder[4]) {
+void Comm::send(uint8_t status, uint8_t mode, uint16_t voltage, uint16_t current, int16_t speed, float angle, int32_t encoder[4], float roll, float pitch, float yaw) {
   txData.counter = infoCounter++;
   txData.message = 'I';
   txData.status = status;
@@ -34,5 +34,8 @@ void Comm::send(uint8_t status, uint8_t mode, uint16_t voltage, uint16_t current
   txData.encoder[1] = encoder[1] * STEP;
   txData.encoder[2] = encoder[2] * STEP;
   txData.encoder[3] = encoder[3] * STEP;
+  txData.roll  =  round(roll * 100);
+  txData.pitch =  round(pitch * 100);
+  txData.yaw   =  round(yaw * 100);
   transmit(&txData, sizeof(TransmitPacket));
 }

esp/Robot/src/communication.h

@@ -11,7 +11,7 @@ class Comm : public Robbus {
     uint8_t   get(); 
     void      confirm(char message);
     void      send(uint8_t message, void* data = NULL, uint8_t length = 0);
-    void      send(uint8_t status, uint8_t mode, uint16_t voltage, uint16_t current, int16_t speed, float angle, int32_t encoder[4]);
+    void      send(uint8_t status, uint8_t mode, uint16_t voltage, uint16_t current, int16_t speed, float angle, int32_t encoder[4], float roll, float pitch, float yaw);
     ReceivePacket   rxData;
   private:
     uint8_t   infoCounter = 0;

esp/Robot/src/data.h

@@ -30,6 +30,11 @@ struct __attribute__((packed)) ReceivePacket {
       uint16_t period;    // ms
       uint16_t timeout;   // ms
     };
+    struct {
+      int index:8;
+      int rgb:24;
+    };
+    uint32_t data;
   };
 };
 
@@ -43,6 +48,7 @@ struct __attribute__((packed)) TransmitPacket {
   int16_t  speed;         // mm/s
   int16_t  angle;         // 0,01°
   uint16_t encoder[4];    // mm
+  int16_t  roll, pitch, yaw;
 };
 
 struct __attribute__((packed)) TransmitData {

esp/Robot/src/main.cpp

@@ -65,17 +65,23 @@ void receiveCommand() {
                   break;
         case 'P': gps.config(comm.rxData.mode);
                   break;
-        case 'V': {
-          // send current version
-          uint8_t buffer[2];
-          buffer[0] = MATTY;  // serial number
-          buffer[1] = ROBOT_FW_VERSION;
-          uint8_t length = 2;
-          comm.send('V', buffer, length);
-          break;
-        }
         case 'R': robot.reset();
                   break;
+        case 'V': { // send serial number, current version
+                    uint8_t buffer[] = {MATTY, ROBOT_FW_VERSION};
+                    comm.send('V', buffer, 2);
+                  }
+                  break;
+#ifdef SERVO_PIN                  
+        case 'C': robot.setServo(comm.rxData.period); // container
+                  break;
+#endif                  
+#ifdef LED_PIN                  
+        case 'D': robot.led(comm.rxData.index, comm.rxData.rgb); // neopixel led
+                  break;
+#endif                  
+        case 'X': robot.writePosition((uint8_t)comm.rxData.period, comm.rxData.timeout, 1000);
+                  break;
       }
       if (robot.mode == AUTONOMOUS) {
         switch (comm.rxData.command) {
@@ -113,13 +119,14 @@ void loop() {
   receiveCommand();
 
   if (robot.process()) { // periodicke odesilani dat
-    comm.send(robot.status, robot.mode, robot.voltage, robot.current, robot.actualSpeed, robot.joint, robot.encoder);
+    comm.send(robot.status, robot.mode, robot.voltage, robot.current, robot.actualSpeed, robot.joint, robot.encoder, robot.roll, robot.pitch, robot.yaw);
   }
 
   if (gps.process()) {
     uint8_t buffer[80];
     uint8_t length = gps.get(buffer);
-    comm.send('P', buffer, length);
-  }
-
+    if (length != 0) {
+      comm.send('P', buffer, length);
+    }
+  }  
 }