diff --git a/fw/platformio.ini b/fw/platformio.ini index 5d09ee4..85f55f2 100644 --- a/fw/platformio.ini +++ b/fw/platformio.ini @@ -9,8 +9,9 @@ ; https://docs.platformio.org/page/projectconf.html [env:nilsdriverv1] -# platform = espressif32 -platform = https://github.com/cziter15/platform-espressif32.git#68ad40f6df654fe4b8d0a50982b810df5b49b677 +platform = espressif32 +# platform = https://github.com/cziter15/platform-espressif32.git#68ad40f6df654fe4b8d0a50982b810df5b49b677 +# platform = https://github.com/cziter15/platform-espressif32.git #platform = https://github.com/tasmota/platform-espressif32/releases/download/2023.10.10/platform-espressif32.zip # board = adafruit_feather_esp32s3 board = lolin_s3_mini @@ -19,12 +20,13 @@ 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 +# ;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/handmade0octopus/ESP32-TWAI-CAN + # https://github.com/handmade0octopus/ESP32-TWAI-CAN + https://github.com/tdk-invn-oss/motion.arduino.ICM42670P ; https://github.com/simplefoc/Arduino-FOC-dcmotor.git ; https://github.com/eric-wieser/packet-io.git ; https://git.nilsschulte.de/nils/simplesync.git diff --git a/fw/src/main.cpp b/fw/src/main.cpp index fdd972e..f516dd6 100644 --- a/fw/src/main.cpp +++ b/fw/src/main.cpp @@ -9,15 +9,17 @@ // #include #include "SimpleFOCDrivers.h" -#include "comms/streams/BinaryIO.h" -#include "encoders/esp32hwencoder/ESP32HWEncoder.h" -#include "encoders/as5600/MagneticSensorAS5600.h" -#include "encoders/calibrated/CalibratedSensor.h" +// #include "comms/streams/BinaryIO.h" +// #include "encoders/as5600/MagneticSensorAS5600.h" +// #include "encoders/calibrated/CalibratedSensor.h" +// #include "encoders/esp32hwencoder/ESP32HWEncoder.h" #include "DRV8323S.hpp" + #include "esp32-hal-adc.h" #include "esp32-hal-gpio.h" -#include "motors/HybridStepperMotor/HybridStepperMotor.h" +// #include "motors/HybridStepperMotor/HybridStepperMotor.h" +#include "HybridStepperMotor.h" #include // #include "encoders/sc60228/MagneticSensorSC60228.h" @@ -30,10 +32,12 @@ #define ENC_A 11 #define ENC_B 8 -#define I2C_SDA 18 -#define I2C_SCL 0 +// #define I2C_SDA 18 +// #define I2C_SCL 0 -#define DRVEN 15 +#define SPI_ACC_nCS 0 +#define DRV_nFAULT 15 +#define DRVEN 16 #define TERMISTOR_PCB 3 #define TERMISTOR_EXT 9 #define TEMP(v0, b, rt, r1, vdd) \ @@ -49,8 +53,8 @@ #define SOC 10 #define INHA 14 -#define INHB 13 -#define INHC 12 +#define INHB 12 +#define INHC 13 #define INLABC 46 #define CAN_TX 6 @@ -61,7 +65,7 @@ #define SPI_CLK 47 #define SPI_DRV_SC 21 -#define LED_PIN 16 +#define LED_PIN 38 #define VSENSE_PIN 5 #define CAL_PIN 17 #define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845) @@ -85,15 +89,20 @@ const int voltage_lpf = 50; // BLDC motor & driver instance // BLDCMotor motor = BLDCMotor(pole pair number); +// #include "ICM42670P.h" +// ICM42670 IMU(SPI, 0, 100000); + // ESP32HWEncoder encoder = -// ESP32HWEncoder(ENC_A, ENC_B, 2048 / 4); // The Index pin can be omitted + // ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted +Encoder encoder = Encoder(ENC_A, ENC_B, 4096 / 4); // CalibratedSensor encoder_calibrated = CalibratedSensor(encoder); -MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36); +// MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36); #ifdef MOTOR struct drv8323s_foc_driver drv8323s; // BLDCMotor motor = BLDCMotor(100); // 24N22P -HybridStepperMotor motor = HybridStepperMotor(50);//, 3.7, 10, 4.5 / 1000); +HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000); +// HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000); // DRV8316Driver3PWM driver = DRV8316Driver3PWM(INHA, INHB, INHC, SPI_DRV_SC); // BLDCDriver3PWM driver = BLDCDriver3PWM(INHA,INHB,INHC); #endif @@ -172,11 +181,17 @@ void comm_task(void *parameter) { } } +void doA1(){encoder.handleA();} +void doB1(){encoder.handleB();} + void setup() { pinMode(LED_PIN, OUTPUT); - digitalWrite(LED_PIN, 1); // enable - delay(1000); - digitalWrite(LED_PIN, 0); // enable + for (int i = 0; i < 1; i++) { + digitalWrite(LED_PIN, 0); // enable + delay(1000); + digitalWrite(LED_PIN, 1); // enable + delay(1000); + } mutex = xSemaphoreCreateMutex(); #ifdef SSIF_USBSERIAL SSYNCIF.begin(); @@ -191,7 +206,7 @@ void setup() { // SSYNCIF.print(__cplusplus); // Wire.setTimeOut(10); - Wire.begin(I2C_SDA, I2C_SCL, 1000000); + // Wire.begin(I2C_SDA, I2C_SCL, 1000000); pinMode(SPI_DRV_SC, OUTPUT); @@ -207,44 +222,66 @@ void setup() { digitalWrite(CAL_PIN, 0); // enable // write_i2c(0x36,0x07, 0b00000011); - write_i2c(0x36, 0x09, 0xFF); - encoder.init(&Wire); + // write_i2c(0x36, 0x09, 0xFF); + // encoder.init(&Wire); + encoder.pullup = Pullup::USE_INTERN; encoder.init(); -#ifdef MOTOR - SimpleFOCDebug::enable(&SSYNCIF); - // driver.voltage_power_supply = 15; + encoder.enableInterrupts(doA1,doB1); pinMode(SPI_MISO, INPUT_PULLUP); SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC); +#ifdef MOTOR + SimpleFOCDebug::enable(&SSYNCIF); drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &SPI); digitalWrite(INLABC, 1); // enable // driver.init(&SPI); // status(); motor.linkDriver(drv8323s.focdriver); motor.linkSensor(&encoder); - motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - motor.voltage_sensor_align = 4; - motor.voltage_limit = 7; - // motor.controller = MotionControlType::velocity_openloop; - motor.controller = MotionControlType::torque; - motor.torque_controller = TorqueControlType::voltage; - drv8323s.focdriver->voltage_power_supply = 12; + // motor.foc_modulation = FOCModulationType::SinePWM; + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + + motor.voltage_sensor_align = 6; + motor.voltage_limit = 3; + motor.controller = MotionControlType::velocity_openloop; + // motor.controller = MotionControlType::torque; + // motor.torque_controller = TorqueControlType::voltage; + drv8323s.focdriver->voltage_power_supply = vdrive_read; motor.init(); // // set voltage to run calibration // encoder_calibrated.voltage_calibration = 3; // // Running calibration - // encoder_calibrated.calibrate(motor); + // encoder_calibrated.calibrate(motor); // //Serial.println("Calibrating Sensor Done."); // // Linking sensor to motor object // motor.linkSensor(&encoder_calibrated); motor.initFOC(); + motor.move(1); // sensor.init(&SPI); // motors[i].linkSensor(&sensors[i]); #endif + // delay(5000); + // SSYNCIF.println("IMU.begin()"); + // int ret = IMU.begin(); + // if (ret != 0) { + // SSYNCIF.print("ICM42670 initialization failed: "); + // SSYNCIF.println(ret); + // while (1) { + // SSYNCIF.print("ICM42670 initialization failed: "); + // SSYNCIF.println(ret); + // delay(100); + // } + // } + // Accel ODR = 100 Hz and Full Scale Range = 16G + // IMU.startAccel(100, 16); + // Gyro ODR = 100 Hz and Full Scale Range = 2000 dps + // IMU.startGyro(100, 2000); + // Wait IMU to start + // delay(100); // 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); @@ -314,7 +351,7 @@ void setup() { // // 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 + // // prefs.get(pref_zero_key.c_str()); motors[i].sensor_direction // = // // (Direction)prefs.getInt(pref_dir_key.c_str()); // // } @@ -323,6 +360,7 @@ void setup() { // // !prefs.isKey(pref_dir_key.c_str())) { // // prefs.clear(); // // prefs.putFloat(pref_zero_key.c_str(), + // 7/ // motors[i].zero_electric_angle); // // prefs.putInt(pref_dir_key.c_str(), // (int)motors[i].sensor_direction); @@ -357,17 +395,47 @@ void setup() { void loop() { #ifdef MOTOR + // drv8323s.focdriver->voltage_power_supply = vdrive_read; + // digitalWrite(LED_PIN, 0); // enable motor.loopFOC(); // motor.move(3); - motor.move(3); + // digitalWrite(LED_PIN, 1); // enable + motor.move(); #else encoder.update(); // optional: Update manually if not using loopfoc() -#endif static float angle = 0; static float old_angle = 0; angle = encoder.getAngle(); - // SSYNCIF.print(angle * 180.0 / 3.1415); + SSYNCIF.print(angle * 180.0 / 3.1415); + SSYNCIF.println(); + + // inv_imu_sensor_event_t imu_event; + + // // Get last event + // IMU.getDataFromRegisters(imu_event); + + // // Format data for Serial Plotter + // SSYNCIF.print(imu_event.accel[0]); + // SSYNCIF.print(", "); + // SSYNCIF.print(imu_event.accel[1]); + // SSYNCIF.print(", "); + // SSYNCIF.print(imu_event.accel[2]); + // SSYNCIF.print(", "); + // SSYNCIF.print(imu_event.gyro[0]); + // SSYNCIF.print(", "); + // SSYNCIF.print(imu_event.gyro[1]); + // SSYNCIF.print(", "); + // SSYNCIF.print(imu_event.gyro[2]); + // SSYNCIF.print(", "); + // SSYNCIF.print(imu_event.temperature / 128 + 25); + // SSYNCIF.println(); + + // // Run @ ODR 100Hz + // delay(100); + // // SSYNCIF.print("V: "); + // // SSYNCIF.println(vdrive_read); +#endif #ifdef MOTOR // SSYNCIF.print(" "); // uint16_t result = 0; @@ -410,8 +478,8 @@ void loop() { // SSYNCIF.print(" "); // SSYNCIF.print(analogRead(SOC)); // SSYNCIF.print(" "); - static float t = temp_pcb; - t = t * 0.95 + 0.05 * temp_pcb; + // static float t = temp_pcb; + // t = t * 0.95 + 0.05 * temp_pcb; // SSYNCIF.print(t); // SSYNCIF.print(" "); // SSYNCIF.println(vdrive_read);