initial checkin

firmware/.gitignore

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

firmware/.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"
+    ]
+}

firmware/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

firmware/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

firmware/platformio.ini

@@ -0,0 +1,43 @@
+; 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:tdisplay]
+platform = espressif32
+board = esp32doit-devkit-v1
+framework = arduino
+monitor_speed = 115200
+monitor_flags = 
+	--eol=CRLF
+	--echo
+	--filter=esp32_exception_decoder
+lib_deps =
+    TFT_eSPI@2.3.59
+    fastled/FastLED @ ^3.4.0
+	askuric/Simple FOC @ ^2.2
+	infineon/TLV493D-Magnetic-Sensor @ ^1.0.3
+
+build_flags =
+  -Os
+  -DCORE_DEBUG_LEVEL=ARDUHAL_LOG_LEVEL_DEBUG
+  -DUSER_SETUP_LOADED=1
+  -DST7789_DRIVER=1
+  -DCGRAM_OFFSET=1
+  -DTFT_WIDTH=135
+  -DTFT_HEIGHT=240
+  -DTFT_MISO=-1
+  -DTFT_MOSI=19
+  -DTFT_SCLK=18
+  -DTFT_CS=5
+  -DTFT_DC=16
+  -DTFT_RST=23
+  -DTFT_BL=4
+  -DLOAD_GLCD=1
+  -DLOAD_GFXFF=1
+  -DSPI_FREQUENCY=40000000

firmware/shaftMount.scad

@@ -0,0 +1,61 @@
+$fn=30;
+
+
+module ic() {
+    linear_extrude(height=2) {
+        difference() {
+            square([7.75, 10.5], center=true);
+            circle(d=4.2);
+        }
+    }
+    translate([0, 0, 2]) {
+        linear_extrude(height=2) {
+            difference() {
+                square([7.75, 10.5], center=true);
+                square([5.25, 7], center=true);
+            }
+        }
+    }
+}
+
+module mountHoles(d=3.2) {
+    translate([19/2, 0]) {
+        circle(d=d);
+    }
+    translate([-19/2, 0]) {
+        circle(d=d);
+    }
+    translate([0, 19/2]) {
+        circle(d=d);
+    }
+    translate([0, -19/2]) {
+        circle(d=d);
+    }
+}
+
+module base() {
+    linear_extrude(height=2) {
+        difference() {
+            circle(d=24);
+            circle(d=3.4);
+            mountHoles();
+        }
+    }
+    linear_extrude(height=3) {
+        difference() {
+            mountHoles(4);
+            mountHoles();
+        }
+    }
+    linear_extrude(height=22) {
+        difference() {
+            circle(d=4.2);
+            circle(d=3.4);
+        }
+    }
+}
+
+base();
+translate([20, 0]) {
+    ic();
+}

firmware/src/display_task.cpp

@@ -0,0 +1,127 @@
+#include "display_task.h"
+#include "semaphore_guard.h"
+
+DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 8192, 1, task_core} {
+    semaphore_ = xSemaphoreCreateMutex();
+    assert(semaphore_ != NULL);
+    xSemaphoreGive(semaphore_);
+}
+
+DisplayTask::~DisplayTask() {
+    if (semaphore_ != NULL) {
+        vSemaphoreDelete(semaphore_);
+    }
+}
+
+static void HSV_to_RGB(float h, float s, float v, uint8_t *r, uint8_t *g, uint8_t *b)
+{
+  int i;
+  float f,p,q,t;
+  
+  h = fmax(0.0, fmin(360.0, h));
+  s = fmax(0.0, fmin(100.0, s));
+  v = fmax(0.0, fmin(100.0, v));
+  
+  s /= 100;
+  v /= 100;
+  
+  if(s == 0) {
+    // Achromatic (grey)
+    *r = *g = *b = round(v*255);
+    return;
+  }
+
+  h /= 60; // sector 0 to 5
+  i = floor(h);
+  f = h - i; // factorial part of h
+  p = v * (1 - s);
+  q = v * (1 - s * f);
+  t = v * (1 - s * (1 - f));
+  switch(i) {
+    case 0:
+      *r = round(255*v);
+      *g = round(255*t);
+      *b = round(255*p);
+      break;
+    case 1:
+      *r = round(255*q);
+      *g = round(255*v);
+      *b = round(255*p);
+      break;
+    case 2:
+      *r = round(255*p);
+      *g = round(255*v);
+      *b = round(255*t);
+      break;
+    case 3:
+      *r = round(255*p);
+      *g = round(255*q);
+      *b = round(255*v);
+      break;
+    case 4:
+      *r = round(255*t);
+      *g = round(255*p);
+      *b = round(255*v);
+      break;
+    default: // case 5:
+      *r = round(255*v);
+      *g = round(255*p);
+      *b = round(255*q);
+    }
+}
+
+void DisplayTask::run() {
+    tft_.begin();
+    tft_.invertDisplay(1);
+    tft_.setRotation(0);
+
+    spr_.setColorDepth(16);
+    spr_.createSprite(TFT_WIDTH, TFT_HEIGHT);
+    spr_.setFreeFont(&Roboto_Thin_24);
+    spr_.setTextColor(0xFFFF, TFT_BLACK);
+    
+    float angle;
+
+    int32_t pointer_center_x = TFT_WIDTH / 2;
+    int32_t pointer_center_y = TFT_HEIGHT * 2 / 3;
+    int32_t pointer_length_short = 10;
+    int32_t pointer_length_long = TFT_WIDTH / 2;
+    while(1) {
+
+        {
+            SemaphoreGuard lock(semaphore_);
+            angle = angle_;// < 0 ? angle_ + 2 * PI : angle_;
+        }
+        float degrees = angle * 360 / 2 / PI;
+
+        uint8_t r, g, b;
+        HSV_to_RGB(degrees, 80, 80, &r, &g, &b);
+
+        spr_.fillSprite(tft_.color565(r, g, b));
+        spr_.setCursor(40, 40);
+
+        spr_.printf("%.1f", degrees);
+
+        float pointer_angle = - angle;
+        spr_.fillTriangle(
+            pointer_center_x + pointer_length_short * cos(pointer_angle - PI * 3 /4),
+            pointer_center_y + pointer_length_short * sin(pointer_angle - PI * 3 /4),
+            pointer_center_x + pointer_length_short * cos(pointer_angle + PI * 3 /4),
+            pointer_center_y + pointer_length_short * sin(pointer_angle + PI * 3 /4),
+            pointer_center_x + pointer_length_long * cos(pointer_angle),
+            pointer_center_y + pointer_length_long * sin(pointer_angle),
+            TFT_WHITE
+        );
+
+        spr_.fillCircle(pointer_center_x, pointer_center_y, 3, TFT_RED);
+
+
+        spr_.pushSprite(0, 0);
+        delay(10);
+    }
+}
+
+void DisplayTask::set_angle(float angle) {
+    SemaphoreGuard lock(semaphore_);
+    angle_ = angle;
+}

firmware/src/display_task.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <Arduino.h>
+#include <TFT_eSPI.h>
+
+#include "task.h"
+
+class DisplayTask : public Task<DisplayTask> {
+    friend class Task<DisplayTask>; // Allow base Task to invoke protected run()
+
+    public:
+        DisplayTask(const uint8_t task_core);
+        ~DisplayTask();
+
+        void set_angle(float angle);
+
+    protected:
+        void run();
+
+    private:
+        TFT_eSPI tft_ = TFT_eSPI();
+
+        /** Full-size sprite used as a framebuffer */
+        TFT_eSprite spr_ = TFT_eSprite(&tft_);
+
+        SemaphoreHandle_t semaphore_;
+
+        float angle_ = 0;
+};

firmware/src/logger.h

@@ -0,0 +1,7 @@
+
+class Logger {
+    public:
+        Logger() {};
+        virtual ~Logger() {};
+    
+};

firmware/src/main.cpp

@@ -0,0 +1,28 @@
+#include <Arduino.h>
+#include <TFT_eSPI.h>
+#include <FastLED.h>
+#include <SimpleFOC.h>
+
+#include "display_task.h"
+#include "motor_task.h"
+#include "tlv_sensor.h"
+
+DisplayTask display_task = DisplayTask(1);
+MotorTask motor_task = MotorTask(0, display_task);
+
+CRGB leds[1];
+
+
+void setup() {
+  Serial.begin(115200);
+
+  display_task.begin();
+  motor_task.begin();
+
+  vTaskDelete(nullptr);
+}
+
+
+void loop() {
+  assert(false);
+}

firmware/src/motor_task.cpp

@@ -0,0 +1,131 @@
+#include <SimpleFOC.h>
+
+#include "motor_task.h"
+#include "tlv_sensor.h"
+
+MotorTask::MotorTask(const uint8_t task_core, DisplayTask& display_task) : Task{"Motor", 8192, 1, task_core}, display_task_(display_task) {
+}
+
+MotorTask::~MotorTask() {}
+
+
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(7, 8);
+BLDCDriver6PWM driver = BLDCDriver6PWM(27, 26, 25, 33, 32, 13);
+
+TlvSensor tlv = TlvSensor();
+
+
+Commander command = Commander(Serial);
+void doMotor(char* cmd) { command.motor(&motor, cmd); }
+
+
+void MotorTask::run() {
+    driver.voltage_power_supply = 5;
+    driver.init();
+
+    Wire.begin();
+    Wire.setClock(400000);
+    tlv.init();
+
+    motor.linkDriver(&driver);
+
+    motor.current_limit = 0.6;
+    motor.linkSensor(&tlv);
+
+    motor.LPF_angle = 0.01;
+
+    motor.LPF_velocity.Tf = 0.05;
+
+    motor.PID_velocity.P = 0.2;
+    motor.PID_velocity.I = 0;
+    motor.PID_velocity.D = 0;
+    motor.PID_velocity.output_ramp = 1000;
+
+    motor.P_angle.P = 0.1;
+    motor.P_angle.I = 0;
+    motor.P_angle.D = 0;
+
+    motor.init();
+
+
+
+
+    // float pp_search_voltage = 2; // maximum power_supply_voltage/2
+    // float pp_search_angle = 28*PI; // search electrical angle to turn
+
+    // // move motor to the electrical angle 0
+    // motor.controller = MotionControlType::angle_openloop;
+    // motor.voltage_limit=pp_search_voltage;
+    // motor.move(0);
+    // _delay(1000);
+    // // read the sensor angle
+    // tlv.update();
+    // float angle_begin = tlv.getAngle();
+    // _delay(50);
+
+    // // move the motor slowly to the electrical angle pp_search_angle
+    // float motor_angle = 0;
+    // while(motor_angle <= pp_search_angle){
+    //   motor_angle += 0.01f;
+    //   tlv.update(); // keep track of the overflow
+    //   motor.move(motor_angle);
+    // }
+    // _delay(1000);
+    // // read the sensor value for 180
+    // tlv.update(); 
+    // float angle_end = tlv.getAngle();
+    // _delay(50);
+    // // turn off the motor
+    // motor.move(0);
+    // _delay(1000);
+
+    // // calculate the pole pair number
+    // int pp = round((pp_search_angle)/(angle_end-angle_begin));
+
+    // Serial.print(F("Estimated PP : "));
+    // Serial.println(pp);
+    // Serial.println(F("PP = Electrical angle / Encoder angle "));
+    // Serial.print(pp_search_angle*180/PI);
+    // Serial.print(F("/"));
+    // Serial.print((angle_end-angle_begin)*180/PI);
+    // Serial.print(F(" = "));
+    // Serial.println((pp_search_angle)/(angle_end-angle_begin));
+    // Serial.println();
+
+
+    // // a bit of monitoring the result
+    // if(pp <= 0 ){
+    //   Serial.println(F("PP number cannot be negative"));
+    //   Serial.println(F(" - Try changing the search_voltage value or motor/sensor configuration."));
+    //   return;
+    // }else if(pp > 30){
+    //   Serial.println(F("PP number very high, possible error."));
+    // }else{
+    //   Serial.println(F("If PP is estimated well your motor should turn now!"));
+    //   Serial.println(F(" - If it is not moving try to relaunch the program!"));
+    //   Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!"));
+    // }
+
+
+
+
+
+    motor.initFOC(0.602, Direction::CW);
+
+
+    // add the motor to the commander interface
+    // The letter (here 'M') you will provide to the SimpleFOCStudio
+    command.add('M', &doMotor, "foo");
+    // tell the motor to use the monitoring
+    motor.useMonitoring(Serial);
+    motor.monitor_downsample = 0; // disable monitor at first - optional
+
+    while (1) {
+        motor.move();
+        motor.loopFOC();
+        motor.monitor();
+        command.run();
+        display_task_.set_angle(motor.shaft_angle);
+    }
+}

firmware/src/motor_task.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <Arduino.h>
+
+#include "task.h"
+#include "display_task.h"
+
+class MotorTask : public Task<MotorTask> {
+    friend class Task<MotorTask>; // Allow base Task to invoke protected run()
+
+    public:
+        MotorTask(const uint8_t task_core, DisplayTask& display_task);
+        ~MotorTask();
+
+    protected:
+        void run();
+
+    private:
+        DisplayTask& display_task_;
+};

firmware/src/semaphore_guard.h

@@ -0,0 +1,33 @@
+/*
+   Copyright 2020 Scott Bezek and the splitflap contributors
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+*/
+#pragma once
+
+#include <Arduino.h>
+
+class SemaphoreGuard {
+    public:
+        SemaphoreGuard(SemaphoreHandle_t handle) : handle_{handle} {
+            xSemaphoreTake(handle_, portMAX_DELAY);
+        }
+        ~SemaphoreGuard() {
+            xSemaphoreGive(handle_);
+        }
+        SemaphoreGuard(SemaphoreGuard const&)=delete;
+        SemaphoreGuard& operator=(SemaphoreGuard const&)=delete;
+
+    private:
+        SemaphoreHandle_t handle_;
+};

firmware/src/task.h

@@ -0,0 +1,54 @@
+/*
+   Copyright 2020 Scott Bezek and the splitflap contributors
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+*/
+#pragma once
+
+#include<Arduino.h>
+
+// Static polymorphic abstract base class for a FreeRTOS task using CRTP pattern. Concrete implementations
+// should implement a run() method.
+// Inspired by https://fjrg76.wordpress.com/2018/05/23/objectifying-task-creation-in-freertos-ii/
+template<class T>
+class Task {
+    public:
+        Task(const char* name, uint32_t stackDepth, UBaseType_t priority, const BaseType_t coreId = tskNO_AFFINITY) : 
+                name { name },
+                stackDepth {stackDepth},
+                priority { priority },
+                coreId { coreId }
+        {}
+        virtual ~Task() {};
+
+        TaskHandle_t getHandle() {
+            return taskHandle;
+        }
+
+        void begin() {
+            BaseType_t result = xTaskCreatePinnedToCore(taskFunction, name, stackDepth, this, priority, &taskHandle, coreId);
+            assert("Failed to create task" && result == pdPASS);
+        }
+
+    private:
+        static void taskFunction(void* params) {
+            T* t = static_cast<T*>(params);
+            t->run();
+        }
+
+        const char* name;
+        uint32_t stackDepth;
+        UBaseType_t priority;
+        TaskHandle_t taskHandle;
+        const BaseType_t coreId;
+};

firmware/src/tlv_sensor.cpp

@@ -0,0 +1,23 @@
+#include "tlv_sensor.h"
+
+static const float ALPHA = 0.8;
+
+TlvSensor::TlvSensor() {}
+
+void TlvSensor::init() {
+  tlv_.begin();
+  tlv_.setAccessMode(Tlv493d::AccessMode_e::MASTERCONTROLLEDMODE);
+  tlv_.disableInterrupt();
+  tlv_.disableTemp();
+}
+
+float TlvSensor::getSensorAngle() {
+    tlv_.updateData();
+    x_ = tlv_.getX() * ALPHA + x_ * (1-ALPHA);
+    y_ = tlv_.getY() * ALPHA + y_ * (1-ALPHA);
+    float rad = atan2f(y_, x_);
+    if (rad < 0) {
+        rad += 2*PI;
+    }
+    return rad;
+}

firmware/src/tlv_sensor.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <SimpleFOC.h>
+#include <Tlv493d.h>
+
+class TlvSensor : public Sensor {
+    public:
+        TlvSensor();
+
+        // initialize the sensor hardware
+        void init();
+
+        // Get current shaft angle from the sensor hardware, and 
+        // return it as a float in radians, in the range 0 to 2PI.
+        //  - This method is pure virtual and must be implemented in subclasses.
+        //    Calling this method directly does not update the base-class internal fields.
+        //    Use update() when calling from outside code.
+        float getSensorAngle();
+    private:
+        Tlv493d tlv_ = Tlv493d();
+        float x_;
+        float y_;
+};

firmware/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