From d26f6e3478ee0fe99b62c1fd51e7c47792a83ec9 Mon Sep 17 00:00:00 2001
From: Nils Schulte <git@nilsschulte.de>
Date: Thu, 15 May 2025 17:16:52 +0200
Subject: [PATCH] working velocitiy closed loop (but slow?)

---
 fw/platformio.ini |  4 ++--
 fw/src/main.cpp   | 32 ++++++++++++++++++++++----------
 2 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/fw/platformio.ini b/fw/platformio.ini
index 85f55f2..ffb7456 100644
--- a/fw/platformio.ini
+++ b/fw/platformio.ini
@@ -23,8 +23,8 @@ 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/schnilz/Arduino-FOC.git#dev
+	https://github.com/schnilz/Arduino-FOC-drivers.git#dev
 	# https://github.com/handmade0octopus/ESP32-TWAI-CAN
 	https://github.com/tdk-invn-oss/motion.arduino.ICM42670P
 ;	https://github.com/simplefoc/Arduino-FOC-dcmotor.git
diff --git a/fw/src/main.cpp b/fw/src/main.cpp
index f516dd6..7130542 100644
--- a/fw/src/main.cpp
+++ b/fw/src/main.cpp
@@ -11,7 +11,7 @@
 #include "SimpleFOCDrivers.h"
 // #include "comms/streams/BinaryIO.h"
 // #include "encoders/as5600/MagneticSensorAS5600.h"
-// #include "encoders/calibrated/CalibratedSensor.h"
+#include "encoders/calibrated/CalibratedSensor.h"
 // #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
 
 #include "DRV8323S.hpp"
@@ -95,7 +95,7 @@ const int voltage_lpf = 50;
 // ESP32HWEncoder encoder =
     // 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);
+CalibratedSensor encoder_calibrated = CalibratedSensor(encoder);
 // MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36);
 
 #ifdef MOTOR
@@ -236,30 +236,33 @@ void setup() {
   digitalWrite(INLABC, 1); // enable
   // driver.init(&SPI);
   // status();
-  motor.linkDriver(drv8323s.focdriver);
   motor.linkSensor(&encoder);
+  motor.linkDriver(drv8323s.focdriver);
   // 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.voltage_sensor_align = 3;
+  motor.voltage_limit = 5;
+  // motor.controller = MotionControlType::velocity_openloop;
+  motor.controller = MotionControlType::velocity;
   // motor.controller = MotionControlType::torque;
   // motor.torque_controller = TorqueControlType::voltage;
+  motor.useMonitoring(SSYNCIF);
   drv8323s.focdriver->voltage_power_supply = vdrive_read;
   motor.init();
+  delay(1000);
 
   // // set voltage to run calibration
-  // encoder_calibrated.voltage_calibration = 3;
+  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);
+  motor.move(0);
   // sensor.init(&SPI);
   // motors[i].linkSensor(&sensors[i]);
 #endif
@@ -393,6 +396,7 @@ void setup() {
   // esp_task_wdt_add(NULL);                  // add current thread to WDT watch
 }
 
+int i = 0;
 void loop() {
 #ifdef MOTOR
   // drv8323s.focdriver->voltage_power_supply = vdrive_read;
@@ -404,11 +408,20 @@ void loop() {
   motor.move();
 #else
   encoder.update(); // optional: Update manually if not using loopfoc()
+#endif
+if(i++ % 100 == 0) {
   static float angle = 0;
   static float old_angle = 0;
   angle = encoder.getAngle();
+  
+  SSYNCIF.print("enc: ");
   SSYNCIF.print(angle * 180.0 / 3.1415);
+  SSYNCIF.print("mec: ");
+  SSYNCIF.print(encoder.getMechanicalAngle() * 180.0 / 3.1415);
+  SSYNCIF.print("\tcal: ");
+  SSYNCIF.print(encoder_calibrated.getAngle() * 180.0 / 3.1415);
   SSYNCIF.println();
+}
 
   // inv_imu_sensor_event_t imu_event;
 
@@ -435,7 +448,6 @@ void loop() {
   // delay(100);
   // // SSYNCIF.print("V: ");
   // // SSYNCIF.println(vdrive_read);
-#endif
 #ifdef MOTOR
   // SSYNCIF.print(" ");
   // uint16_t result = 0;