diff --git a/fw/README.md b/fw/README.md new file mode 100644 index 0000000..70c3e36 --- /dev/null +++ b/fw/README.md @@ -0,0 +1,5 @@ +## Develop +create `compile_commands.json`: +´´´ +pio run -t compiledb +´´´ diff --git a/fw/platformio.ini b/fw/platformio.ini new file mode 100644 index 0000000..b8f7c82 --- /dev/null +++ b/fw/platformio.ini @@ -0,0 +1,35 @@ +; 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:nilsdriverv1] +platform = espressif32 +#platform = https://github.com/tasmota/platform-espressif32/releases/download/2023.10.10/platform-espressif32.zip +board = adafruit_feather_esp32s3 +# board = lolin_s3_mini +framework = arduino +#platform_packages = +# platformio/framework-arduinoespressif32 @ https://github.com/imwhocodes/arduino-esp32#master +upload_protocol = esptool +upload_port = /dev/ttyACM* +# board_build.mcu = esp32s3 +lib_archive = false +;platform_packages = espressif/toolchain-xtensa-esp32@8.4.0+2021r2-patch5 +monitor_speed = 115200 +lib_deps = + https://github.com/simplefoc/Arduino-FOC.git#dev + https://github.com/simplefoc/Arduino-FOC-drivers.git#dev +; https://github.com/simplefoc/Arduino-FOC-dcmotor.git +; https://github.com/eric-wieser/packet-io.git +; https://git.nilsschulte.de/nils/simplesync.git +; https://github.com/Tinyu-Zhao/INA3221.git + +build_flags = -DRADIOLIB_EEPROM_UNSUPPORTED +;-DSIMPLEFOC_ESP32_USELEDC + diff --git a/fw/src/main.cpp b/fw/src/main.cpp new file mode 100644 index 0000000..39cddea --- /dev/null +++ b/fw/src/main.cpp @@ -0,0 +1,311 @@ +// #include + +#include "Arduino.h" +#include "SPI.h" +#include "USB.h" +// #include "Wire.h" +// #include "Wire.h" +#include +// #include + +#include "SimpleFOCDrivers.h" +#include "encoders/esp32hwencoder/ESP32HWEncoder.h" + +#include + +// #include "encoders/sc60228/MagneticSensorSC60228.h" +// #include "esp32-hal-adc.h" +// #include "esp32-hal-gpio.h" +// #include "esp32-hal-uart.h" +// #include "esp32-hal.h" +// #include "simplesync.hpp" + +#define ENC_A 17 +#define ENC_B 8 + +#define I2C_SDA 18 +#define I2C_SCL 0 + +#define DRVOFF 46 +#define TERP_PCB 2 +#define TERP_EXT 1 + +#define SOA 3 +#define SOB 9 +#define SOC 10 + +#define INLA 21 +#define INHA 47 +#define INLB 13 +#define INHB 14 +#define INLC 11 +#define INHC 12 + +#define CAN_TX 45 +#define CAN_RX 48 + +#define SPI_MISO 6 +#define SPI_MOSI 7 +#define SPI_CLK 15 +#define SPI_DRV_SC 16 + + + +#define MOTOR +#define SSIF_USBSERIAL +#define FW_NO_WATCHDOG + +#ifndef FW_NO_WATCHDOG +#include +#define WDT_TIMEOUT_s 1 +// struct esp_task_wdt_config_t wd_config = { +// .timeout_ms = 100, +// .idle_core_mask = ~0x00000000, +// .trigger_panic = true +// }; +#endif + +const float voltage_factor = 19.31; +const int voltage_lpf = 50; +const float anglefactor = 10000.0; +// BLDC motor & driver instance +// BLDCMotor motor = BLDCMotor(pole pair number); +ESP32HWEncoder encoder = ESP32HWEncoder(ENC_A, ENC_B, 2048); // The Index pin can be omitted + +#ifdef MOTOR +BLDCMotor motor = BLDCMotor(11); // 24N22P +BLDCDriver3PWM driver = BLDCDriver3PWM(INHA,INHB,INHC); +#endif +// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional)); + +#ifdef SSIF_USBSERIAL +USBCDC usbserial; +#define SSYNCIF usbserial +#else +#define SSYNCIF Serial1 +#endif +// #define SSYNCIF Serial + +void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) { + Wire.beginTransmission(addr); + Wire.write(reg); // MAN:0x2E + int error = Wire.endTransmission(); + if (error != 0) { + // Serial.println("read error!"); + return; + } + delayMicroseconds(50); + int ret = Wire.requestFrom(addr, len); + // Serial.print("0x"); + // Serial.print(reg, HEX); + // Serial.print(": "); + Wire.readBytes(buf, ret); + // for (int i = 0; i < ret; ++i) { + // Serial.print("0x"); + // Serial.print(Wire.read(), HEX); + // Serial.print(" "); + // } + // Serial.println(); + return; +} + +void write_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val) { + Wire.beginTransmission(addr); + uint8_t data[] = {reg_lsb, val}; + Wire.write(data, sizeof data); + int error = Wire.endTransmission(); + if (error != 0) { + // Serial.println("write error!"); + return; + } +} +void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t +val2) { + Wire.beginTransmission(addr); + uint8_t data[] = {reg_lsb, val1, val2}; + Wire.write(data, sizeof data); + int error = Wire.endTransmission(); + if (error != 0) { + // Serial.println("write error!"); + return; + } +} + +// Preferences prefs; + +SemaphoreHandle_t mutex; +struct sync_params {}; + +TaskHandle_t taskFocHandle; +void foc_task(void *parameter) { + long last_comm = 0; + while (true) { +#ifdef MOTOR + // drivers[0].voltage_power_supply = s_foc.vcc_voltage_mV / 1000.0f; + // for (int i = 1; i < NUM_MOTORS; i += 1) + // drivers[i].voltage_power_supply = drivers[0].voltage_power_supply; +#endif + +#ifdef MOTOR + // motors.target = s_foc.vt[i] / anglefactor; + // motors.loopFOC(); + // motors.move(); +#else + // sensors.update(); +#endif + vTaskDelay(1); + } +} +TaskHandle_t taskCommHandle; +void comm_task(void *parameter) { + while (true) { + long now = millis(); + + vTaskDelay(1); + } +} + +void setup() { + mutex = xSemaphoreCreateMutex(); +#ifdef SSIF_USBSERIAL + SSYNCIF.begin(); + USB.begin(); +#else + SSYNCIF.begin(460800, SERIAL_8N1, 44, 43); +#endif + // prefs.begin("foc"); + // SSYNCIF.print(__cplusplus); + + // Wire.setTimeOut(10); + // SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_CS1); + Wire.begin(I2C_SDA, I2C_SCL, 400000); + + // write_i2c(0x36,0x07, 0b00000011); + write_i2c(0x36,0x09, 0b1000); + + encoder.init(); + + + // sensor.init(&SPI); +#ifdef MOTOR + // motors[i].linkSensor(&sensors[i]); +#endif + + // ina.getCurrent(INA3221_CH1) * 1000; ina.getVoltage(INA3221_CH1); + // ina.getCurrent(INA3221_CH2) * 1000; ina.getVoltage(INA3221_CH2); + // ina.getCurrent(INA3221_CH3) * 1000; ina.getVoltage(INA3221_CH3); + + // initialise magnetic sensor hardware + +#ifdef MOTOR + // pinMode(nRESET, OUTPUT); + // digitalWrite(nRESET, 1); +#endif + // power supply voltage [V] + // s_exchange.vcc_voltage_mV = GET_VCC; + // delay(100); + // s_exchange.vcc_voltage_mV = GET_VCC; + // drivers[0].voltage_power_supply = s_exchange.vcc_voltage_mV / 1000.0f; + // for (int i = 1; i < NUM_MOTORS; i += 1) + // drivers[i].voltage_power_supply = drivers[0].voltage_power_supply; + // s_foc = s_exchange; + + // for (int i = 0; i < NUM_MOTORS; i += 1) { +#ifdef MOTOR + // drivers[i].init(); + // link driver + // motors[i].linkDriver(&drivers[i]); + // motors[i].voltage_sensor_align = 5; + + // set control loop type to be used + // motors[i].torque_controller = TorqueControlType::voltage; + // motors[i].controller = MotionControlType::velocity; + + // contoller configuration based on the control type + // motors[i].PID_velocity.P = 0.35; + // motors[i].PID_velocity.I = 8; + // motors[i].PID_velocity.D = 0; + + // default voltage_power_supply + // motors[i].voltage_limit = 3.4 * 3; + + // velocity low pass filtering time constant + // motors[i].LPF_velocity.Tf = 0.01; +#endif + // } +#ifdef MOTOR + // drivers[2].init(); + // drivers[2].voltage_limit = 3.4 * 3; + // drivers[2].setPwm(s_exchange.vcc_voltage_mV,s_exchange.vcc_voltage_mV,s_exchange.vcc_voltage_mV); + + // pinMode(4, OUTPUT); + // digitalWrite(4, 1); + // pinMode(5, OUTPUT); + // digitalWrite(5, 1); + // pinMode(6, OUTPUT); + // digitalWrite(6, 1); +#endif + // angle loop controller + // motor.P_angle.P = 20; + // angle loop velocity limit + // motor.velocity_limit = 150; + + // initialise motor + // align encoder and start FOC +#ifdef MOTOR + // for (int i = 0; i < NUM_MOTORS; i += 1) { + // motors[i].init(); + // // std::string pref_zero_key = "m" + std::to_string(i) + ".zero"; + // // std::string pref_dir_key = "m" + std::to_string(i) + ".dir"; + // // if (prefs.isKey(pref_zero_key.c_str()) && + // // prefs.isKey(pref_dir_key.c_str())) { + // // motors[i].zero_electric_angle = + // // prefs.getFloat(pref_zero_key.c_str()); motors[i].sensor_direction = + // // (Direction)prefs.getInt(pref_dir_key.c_str()); + // // } + // motors[i].initFOC(); + // // if (!prefs.isKey(pref_zero_key.c_str()) || + // // !prefs.isKey(pref_dir_key.c_str())) { + // // prefs.clear(); + // // prefs.putFloat(pref_zero_key.c_str(), motors[i].zero_electric_angle); + // // prefs.putInt(pref_dir_key.c_str(), (int)motors[i].sensor_direction); + // // } + // } +#endif + // prefs.end(); + // set the inital target value +#ifdef MOTOR + // for (int i = 0; i < NUM_MOTORS; i += 1) + // motors[i].target = 0; +#endif + + // xTaskCreatePinnedToCore(&comm_task, // Function name of the task + // "Comm", // Name of the task (e.g. for debugging) + // 4096, // Stack size (bytes) + // NULL, // Parameter to pass + // 1, // Task priority + // &taskCommHandle, // Assign task handle + // 1 // Run on the non-Arduino (1) core + // ); + // xTaskCreatePinnedToCore(&foc_task, // Function name of the task + // "foc", // Name of the task (e.g. for debugging) + // 2048, // Stack size (bytes) + // NULL, // Parameter to pass + // 1, // Task priority + // &taskFocHandle, // Assign task handle + // 0 // Run on the non-Arduino (1) core + // ); + // esp_task_wdt_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts + // esp_task_wdt_add(NULL); // add current thread to WDT watch +} + +int i = 0; +void loop() { + + encoder.update(); // optional: Update manually if not using loopfoc() + + // Access the sensor value + SSYNCIF.println(encoder.getAngle()); + delay(10); +} +// esp_task_wdt_reset();