initial checkin

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_;
+};