initial commit

.gitignore

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

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "lib/PCA9685-Arduino"]
+	path = lib/PCA9685-Arduino
+	url = https://github.com/NachtRaveVL/PCA9685-Arduino.git

.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

.vscode/extensions.json

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

.vscode/settings.json

@@ -0,0 +1,11 @@
+{
+    "files.associations": {
+        "array": "cpp",
+        "deque": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "initializer_list": "cpp"
+    }
+}

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

include/body.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <vector>
+#include <map>
+
+class Leg
+{
+public:
+
+  enum Joint
+  {
+    Hipp,
+    Knee,
+  };
+
+  Leg(uint8_t hippIndex, uint8_t kneeIndex);
+
+  //! @brief positive trim in deg to center this joint
+  void setTrim(Joint, uint8_t angle);
+
+  //! @brief position in deg
+  void setPos(Joint, double angle);
+
+private:
+  struct JointInfo {
+    uint8_t index;
+    uint8_t trim;
+    double pos;
+  };
+
+  std::map<Joint, JointInfo> _joints;
+};
+
+typedef std::vector<Leg> Legs;
+
+class Body
+{
+public:
+  static Body& instance()
+  {
+      static Body instance;
+      return instance;
+  }
+
+  //! @brief Nescessary to configure the driver
+  void init();
+
+  //! @brief Check if driver has frozen or lost contact/sync
+  //!        Resets the driver if nescesarry
+  void healthCheck(); 
+  
+  Legs legs;
+
+private:
+  Body();
+
+};

lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

platformio.ini

@@ -0,0 +1,14 @@
+; 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:featheresp32]
+platform = espressif32
+board = esp32dev
+framework = arduino

src/body.cpp

@@ -0,0 +1,105 @@
+#include "body.h"
+
+#include <Arduino.h>
+#include <Wire.h>
+#include <Adafruit_PWMServoDriver.h>
+#include <SPI.h>
+
+
+#define SERVO_IIC_ADDR  (0x40)
+
+// Depending on your servo make, the pulse width min and max may vary, you 
+// want these to be as small/large as possible without hitting the hard stop
+// for max range. You'll have to tweak them as necessary to match the servos you
+// have!
+#define SERVOMIN  190 // This is the 'minimum' pulse length count (out of 4096)
+#define SERVOMAX  540 // This is the 'maximum' pulse length count (out of 4096)
+#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 190
+#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 540
+#define SERVO_FREQ 60 // Analog servos run at ~50 Hz updates
+
+Adafruit_PWMServoDriver servoDriver = Adafruit_PWMServoDriver(0x40);
+
+unsigned long freqWatchDog = 0;
+unsigned long SuppressScamperUntil = 0;  // if we had to wake up the servos, suppress the power hunger scamper mode for a while
+
+inline void resetServoDriver() {
+  servoDriver.begin(); 
+  servoDriver.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~60 Hz updates
+}
+
+inline void checkForServoSleep() {
+
+  if (millis() > freqWatchDog) 
+  {
+
+    // See if the servo driver module went to sleep, probably due to a short power dip
+    Wire.beginTransmission(SERVO_IIC_ADDR);
+    Wire.write(0);  // address 0 is the MODE1 location of the servo driver, see documentation on the PCA9685 chip for more info
+    Wire.endTransmission();
+    Wire.requestFrom((uint8_t)SERVO_IIC_ADDR, (uint8_t)1);
+    int mode1 = Wire.read();
+    if (mode1 & 16) 
+    { // the fifth bit up from the bottom is 1 if controller was asleep
+        // wake it up!
+        resetServoDriver();
+        //beep(1200,100);  // chirp to warn user of brown out on servo controller
+        SuppressScamperUntil = millis() + 10000;  // no scamper for you! (for 10 seconds because we ran out of power, give the battery
+                                                // a bit of time for charge migration and let the servos cool down)
+    }
+    freqWatchDog = millis() + 100;
+  }
+}
+
+double modifiedMap(double x, double in_min, double in_max, double out_min, double out_max)
+{
+ double temp = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+ temp = (int) (4*temp + .5);
+ return (double) temp/4;
+}
+
+Body::Body()
+: legs()
+{
+  uint8_t numOfLegs = 6;
+  for (uint8_t i = 0; i < numOfLegs; ++i)
+  {
+    legs.push_back(Leg(i, i+6));
+  }
+}
+
+void Body::init()
+{
+  resetServoDriver();
+}
+void Body::healthCheck()
+{
+  checkForServoSleep();
+}
+
+Leg::Leg(uint8_t hipp, uint8_t knee)
+: _joints({{Joint::Hipp, {.index=hipp, .trim=0, .pos=0}},
+           {Joint::Knee, {.index=knee, .trim=0, .pos=0}}})
+{
+}
+
+void Leg::setTrim(Joint j, uint8_t angle) 
+{
+  _joints[j].trim = angle;
+}
+
+void Leg::setPos(Joint j, double angle)
+{
+  //! @todo Trying to evaluate if one of these has better performance on the servo
+
+  auto angleToPulse = [](double a)
+  { return modifiedMap(a, 0.0, 180.0, SERVOMIN, SERVOMAX); };
+
+  auto angleToMicroPulse = [](double a)
+  { return modifiedMap(a, 0.0, 180.0, USMIN, USMAX); };
+
+  //servoDriver.setPWM(_joints[j].index, 0, angleToPulse(angle));
+  servoDriver.writeMicroseconds(_joints[j].index, angleToMicroPulse(angle));
+
+  _joints[j].pos = angle;
+}

src/main.cpp

@@ -0,0 +1,48 @@
+#include <Arduino.h>
+
+#include "body.h"
+
+Body body = Body::instance();
+
+void servoTest() {
+// Drive each PWM in a 'wave'
+  for (uint8_t legnum = 0; legnum < body.legs.size(); legnum++)
+  {
+    body.legs[legnum].setPos(Leg::Hipp, 90);
+    body.legs[legnum].setPos(Leg::Knee, 45);
+  }
+}
+
+void setup() {
+  Serial.begin(9600);
+
+  body.init();
+  delay(250);
+
+  servoTest();
+}
+
+
+
+void loop() {
+  Serial.println("Hejsan");
+  sleep(1);
+  
+  static uint8_t d = 0;
+  if (d < 10)
+  {
+    for (long angle = 0; angle < 180*5; angle++)
+    {
+      body.legs[1].setPos(Leg::Knee, double(angle/5));
+      //delay(1);
+    }
+    for (long angle = 180*5; angle > 0; angle--)
+    {
+      body.legs[1].setPos(Leg::Knee, double(angle/5));
+      //delay(1);
+    }
+  }
+  d++;
+
+  body.healthCheck();
+}

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