commit bb4ba9df1e28bfbdd62de57cb29797119d5dc10a Author: Scott Bezek Date: Fri Oct 22 12:35:39 2021 -0700 initial checkin diff --git a/firmware/.gitignore b/firmware/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/firmware/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/firmware/.vscode/extensions.json b/firmware/.vscode/extensions.json new file mode 100644 index 0000000..0f0d740 --- /dev/null +++ b/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" + ] +} diff --git a/firmware/include/README b/firmware/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/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 diff --git a/firmware/lib/README b/firmware/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/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 +#include + +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 diff --git a/firmware/platformio.ini b/firmware/platformio.ini new file mode 100644 index 0000000..9e8184b --- /dev/null +++ b/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 diff --git a/firmware/shaftMount.scad b/firmware/shaftMount.scad new file mode 100644 index 0000000..122a959 --- /dev/null +++ b/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(); +} \ No newline at end of file diff --git a/firmware/src/display_task.cpp b/firmware/src/display_task.cpp new file mode 100644 index 0000000..258c850 --- /dev/null +++ b/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; +} diff --git a/firmware/src/display_task.h b/firmware/src/display_task.h new file mode 100644 index 0000000..f665180 --- /dev/null +++ b/firmware/src/display_task.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +#include "task.h" + +class DisplayTask : public Task { + friend class Task; // 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; +}; diff --git a/firmware/src/logger.h b/firmware/src/logger.h new file mode 100644 index 0000000..2ef2823 --- /dev/null +++ b/firmware/src/logger.h @@ -0,0 +1,7 @@ + +class Logger { + public: + Logger() {}; + virtual ~Logger() {}; + +}; diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp new file mode 100644 index 0000000..8a92251 --- /dev/null +++ b/firmware/src/main.cpp @@ -0,0 +1,28 @@ +#include +#include +#include +#include + +#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); +} \ No newline at end of file diff --git a/firmware/src/motor_task.cpp b/firmware/src/motor_task.cpp new file mode 100644 index 0000000..ed98cb5 --- /dev/null +++ b/firmware/src/motor_task.cpp @@ -0,0 +1,131 @@ +#include + +#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); + } +} \ No newline at end of file diff --git a/firmware/src/motor_task.h b/firmware/src/motor_task.h new file mode 100644 index 0000000..c2240a2 --- /dev/null +++ b/firmware/src/motor_task.h @@ -0,0 +1,20 @@ +#pragma once + +#include + +#include "task.h" +#include "display_task.h" + +class MotorTask : public Task { + friend class Task; // 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_; +}; diff --git a/firmware/src/semaphore_guard.h b/firmware/src/semaphore_guard.h new file mode 100644 index 0000000..0f9a2a1 --- /dev/null +++ b/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 + +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_; +}; diff --git a/firmware/src/task.h b/firmware/src/task.h new file mode 100644 index 0000000..b59c0d2 --- /dev/null +++ b/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 + +// 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 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(params); + t->run(); + } + + const char* name; + uint32_t stackDepth; + UBaseType_t priority; + TaskHandle_t taskHandle; + const BaseType_t coreId; +}; diff --git a/firmware/src/tlv_sensor.cpp b/firmware/src/tlv_sensor.cpp new file mode 100644 index 0000000..bea7fde --- /dev/null +++ b/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; +} diff --git a/firmware/src/tlv_sensor.h b/firmware/src/tlv_sensor.h new file mode 100644 index 0000000..cbcbb71 --- /dev/null +++ b/firmware/src/tlv_sensor.h @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +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_; +}; diff --git a/firmware/test/README b/firmware/test/README new file mode 100644 index 0000000..df5066e --- /dev/null +++ b/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