velocity working?

```
y r 125 reg.pid_velocity 0.001 2.5 0.0 0.01 1000.0 3.0 100.0 \
  && y cmd 125 restart
y pub --period=0.1 \
    10:reg.udral.service.common.readiness '!$ "B(1,1) * 3"' \
    50:reg.udral.physics.dynamics.rotation.planar '{torque: 4, kinematics: {angular_position: NAN, angular_velocity: !$ "A(1,1) * 10"}}'
```

fw/src/.clang-format

@@ -0,0 +1,3 @@
+Language: Cpp
+BasedOnStyle:  google
+ColumnLimit: 120

fw/src/main.cpp

@@ -1,8 +1,9 @@
 // #include <Preferences.h>
 
+#include <cmath>
+
 #include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
 #include "esp32-hal.h"
-#include <cmath>
 #define NODE_NAME "N17BLDC"
 
 #define VERSION_MAJOR 0
@@ -12,7 +13,6 @@
 #include "Arduino.h"
 #include "pin_def.h"
 
-
 #define USE_USBSERIAL
 #ifdef USE_USBSERIAL
 #include "USB.h"
@@ -26,36 +26,29 @@ USBCDC usbserial;
 #endif
 // #define Serial Serial
 
-#include "canard.c"
-
 #include <SPI.h>
+
+#include "canard.c"
+#include "udral_servo.hpp"
 #define spi_dev SPI
 // #include "Wire.h"
 #include <SimpleFOC.h>
 
 #include "SimpleFOCDrivers.h"
-#include "encoders/calibrated/CalibratedSensor.h"
 #include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
 #include "encoders/as5047/MagneticSensorAS5047.h"
+#include "encoders/calibrated/CalibratedSensor.h"
 // #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
 
-#include "DRV8323S.hpp"
+#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
 
+#include "DRV8323S.hpp"
 #include "HybridStepperMotor.h"
 #include "esp32-hal-adc.h"
 #include "esp32-hal-gpio.h"
 
-#include "udral_servo.hpp"
+#define TEMP(v0, b, rt, r1, vdd) ((1.0 / ((1.0 / 298.15) + ((1.0 / b) * log((v0 * r1) / (rt * (vdd - v0)))))) - 273.15)
-
+#define temp_pcb (TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, 3.3))
-#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
-
-#define TEMP(v0, b, rt, r1, vdd)                                               \
-  ((1.0 /                                                                      \
-    ((1.0 / 298.15) + ((1.0 / b) * log((v0 * r1) / (rt * (vdd - v0)))))) -     \
-   273.15)
-#define temp_pcb                                                               \
-  (TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0,    \
-        3.3))
 
 #define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
 //* 20.8f/ 21033.75)
@@ -74,6 +67,7 @@ USBCDC usbserial;
 #endif
 
 const int voltage_lpf = 50;
+const float max_voltage_limit = 6;
 
 // ESP32HWEncoder encoder =
 // ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted
@@ -83,19 +77,20 @@ Encoder encoder(ENC_A, ENC_B, 4096 / 4, 0);
 AbsIncCombineSensor abs_inc_sensor(encoder, encoder_absolute);
 
 float encoder_calibration_lut[90];
-CalibratedSensor encoder_calibrated(encoder_absolute, sizeof(encoder_calibration_lut) /
+CalibratedSensor encoder_calibrated(encoder_absolute,
-                                  sizeof(encoder_calibration_lut[0]), encoder_calibration_lut);
+                                    sizeof(encoder_calibration_lut) / sizeof(encoder_calibration_lut[0]),
+                                    encoder_calibration_lut);
 // 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);
 #endif
 
 void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
   Wire.beginTransmission(addr);
-  Wire.write(reg); // MAN:0x2E
+  Wire.write(reg);  // MAN:0x2E
   int error = Wire.endTransmission();
   if (error != 0) {
     return;
@@ -129,14 +124,32 @@ void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) {
 SemaphoreHandle_t mutex;
 struct UdralServoState servo_state = {0};
 
+static bool init_done = false;
+
 void mutexed_state_sync(UdralServoState *const state, const float hz) {
   xSemaphoreTake(mutex, portMAX_DELAY);
+  if (!init_done) {
+    init_done = true;
+    state->pid_position.value.count = 3;
+    state->pid_position.value.elements[0] = motor.P_angle.P;
+    state->pid_position.value.elements[1] = motor.P_angle.I;
+    state->pid_position.value.elements[2] = motor.P_angle.D;
+    state->pid_velocity.value.count = 7;
+    state->pid_velocity.value.elements[0] = motor.PID_velocity.P;
+    state->pid_velocity.value.elements[1] = motor.PID_velocity.I;
+    state->pid_velocity.value.elements[2] = motor.PID_velocity.D;
+    state->pid_velocity.value.elements[3] = motor.LPF_velocity.Tf;
+    state->pid_velocity.value.elements[4] = motor.PID_velocity.output_ramp;
+    state->pid_velocity.value.elements[5] = motor.PID_velocity.limit;
+    motor.motion_downsample = state->pid_velocity.value.elements[6] = 0;
+  }
   servo_state.target_force = state->target_force;
   servo_state.target_acceleration = state->target_acceleration;
   servo_state.target_velocity = state->target_velocity;
   servo_state.target_position = state->target_position;
   servo_state.arming = state->arming;
-  servo_state.pid = state->pid;
+  servo_state.pid_position = state->pid_position;
+  servo_state.pid_velocity = state->pid_velocity;
 
   float curr_angle = servo_state.curr_position;
   static float curr_angle_last = curr_angle;
@@ -144,7 +157,7 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
   curr_angle_last = curr_angle;
 
   static float curr_vel_last = curr_vel;
-  float curr_accel =(curr_vel - curr_vel_last) * hz;
+  float curr_accel = (curr_vel - curr_vel_last) * hz;
   curr_vel_last = curr_vel;
 
   servo_state.curr_acceleration = curr_accel;
@@ -166,15 +179,14 @@ void setup() {
 #ifdef USE_USBSERIAL
   Serial.begin();
   USB.begin();
-  Serial.setDebugOutput(
+  Serial.setDebugOutput(true);  // sends all log_e(), log_i() messages to USB HW CDC
-      true); // sends all log_e(), log_i() messages to USB HW CDC
   Serial.setTxTimeoutMs(0);
 #else
   Serial.begin(460800, SERIAL_8N1, 44, 43);
 #endif
-  digitalWrite(LED_PIN, 0); // enable
+  digitalWrite(LED_PIN, 0);  // enable
   delay(500);
-  digitalWrite(LED_PIN, 1); // disable
+  digitalWrite(LED_PIN, 1);  // disable
   delay(100);
 
   Serial.println("Start");
@@ -189,14 +201,14 @@ void setup() {
   pinMode(INHC, OUTPUT);
   // digitalWrite(INHC, 0); // enable
   pinMode(INLABC, OUTPUT);
-  digitalWrite(INLABC, 0); // enable
+  digitalWrite(INLABC, 0);  // enable
   pinMode(CAL_PIN, OUTPUT);
-  digitalWrite(CAL_PIN, 0); // enable
+  digitalWrite(CAL_PIN, 0);  // enable
 
   pinMode(SPI_MISO, INPUT_PULLUP);
   SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
-  //SPI.setFrequency(100000);
+  // SPI.setFrequency(100000);
-  // initialise magnetic sensor hardware
+  //  initialise magnetic sensor hardware
   encoder.pullup = Pullup::USE_INTERN;
   encoder.init();
   encoder.enableInterrupts(doA1, doB1);
@@ -208,7 +220,7 @@ void setup() {
 #ifdef MOTOR
   SimpleFOCDebug::enable(&Serial);
   drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &spi_dev);
-  digitalWrite(INLABC, 1); // enable
+  digitalWrite(INLABC, 1);  // enable
   // driver.init(&SPI);
   // status();
   motor.linkSensor(&encoder_calibrated);
@@ -217,7 +229,7 @@ void setup() {
   motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
 
   motor.voltage_sensor_align = 4;
-  motor.voltage_limit = 6;
+  motor.voltage_limit = max_voltage_limit;
   // motor.controller = MotionControlType::velocity_openloop;
   // motor.controller = MotionControlType::velocity;
   motor.controller = MotionControlType::torque;
@@ -239,18 +251,14 @@ void setup() {
   const char *sensor_direction_str = "enc_dir_str";
   const int settle_time_ms = 150;
   if (!pref.isKey(encoder_calibration_lut_str) ||
-      pref.getBytesLength(encoder_calibration_lut_str) !=
+      pref.getBytesLength(encoder_calibration_lut_str) != sizeof(encoder_calibration_lut)) {
-          sizeof(encoder_calibration_lut)) {
     encoder_calibrated.calibrate(motor, settle_time_ms);
-    pref.putBytes(encoder_calibration_lut_str,
+    pref.putBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
-                  &encoder_calibration_lut[0],
-                  sizeof(encoder_calibration_lut));
     pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
     pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
 
   } else {
-    pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0],
+    pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
-                  sizeof(encoder_calibration_lut));
     Serial.println("Skipping calibration");
   }
 
@@ -266,16 +274,16 @@ void setup() {
   pref.end();
 
   motor.move(0);
-  digitalWrite(INLABC, 0); // enable
+  digitalWrite(INLABC, 0);  // enable
 #endif
 
-  xTaskCreatePinnedToCore(&comm_task, // Function name of the task
+  xTaskCreatePinnedToCore(&comm_task,       // Function name of the task
-                          "comm",     // Name of the task (e.g. for debugging)
+                          "comm",           // Name of the task (e.g. for debugging)
-                          65536,      // Stack size (bytes)
+                          65536,            // Stack size (bytes)
-                          NULL,       // Parameter to pass
+                          NULL,             // Parameter to pass
-                          10,          // Task priority
+                          10,               // Task priority
-                          &taskCommHandle, // Assign task handle
+                          &taskCommHandle,  // Assign task handle
-                          0                // Run on the non-Arduino (1) core
+                          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
@@ -284,6 +292,10 @@ void setup() {
 int i = 0;
 void loop() {
 #ifdef MOTOR
+
+  while (!init_done) {
+    delay(10);
+  }
   bool armed = false;
 
   /* adc reading */
@@ -292,17 +304,67 @@ void loop() {
 
   xSemaphoreTake(mutex, portMAX_DELAY);
   armed = servo_state.arming.armed;
-  float target = servo_state.target_force;
+
+  float target;
+  if (!std::isnan(servo_state.target_position)) {
+    if (motor.controller != MotionControlType::angle) {
+      motor.controller = MotionControlType::angle;
+      Serial.println("MotionControlType::angle");
+    }
+    target = servo_state.target_position;
+
+    motor.velocity_limit = std::isnan(servo_state.target_velocity) ? 10000 : servo_state.target_velocity;
+    motor.voltage_limit = std::isnan(servo_state.target_force)
+                              ? max_voltage_limit
+                              : std::min(max_voltage_limit, std::abs(servo_state.target_force));
+  } else if (!std::isnan(servo_state.target_velocity)) {
+    if (motor.controller != MotionControlType::velocity) {
+      motor.controller = MotionControlType::velocity;
+      Serial.println("MotionControlType::velocity");
+    }
+    target = servo_state.target_velocity;
+    motor.voltage_limit = std::isnan(servo_state.target_force)
+                              ? max_voltage_limit
+                              : std::min(max_voltage_limit, std::abs(servo_state.target_force));
+  } else {
+    motor.voltage_limit = std::isnan(servo_state.target_force)
+                              ? max_voltage_limit
+                              : std::min(max_voltage_limit, std::abs(servo_state.target_force));
+    target = servo_state.target_force;
+    if (motor.controller != MotionControlType::torque) {
+      motor.controller = MotionControlType::torque;
+      Serial.printf("MotionControlType::torque %f\n", target);
+    }
+  }
 
   servo_state.controller_temperature = curr_pcb_temp_kelvin;
   servo_state.motor_temperature = NAN;
 
   float curr_angle = encoder_calibrated.getAngle();
   servo_state.curr_position = curr_angle;
-  servo_state.curr_force = servo_state.target_force;
+  servo_state.curr_force = motor.voltage.d;
 
   servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
-  servo_state.current = NAN;
+  servo_state.current = motor.current.d;
+
+  motor.PID_velocity.P = servo_state.pid_velocity.value.count <= 0 ? 0 : servo_state.pid_velocity.value.elements[0];
+  motor.PID_velocity.I = servo_state.pid_velocity.value.count <= 1 ? 0 : servo_state.pid_velocity.value.elements[1];
+  motor.PID_velocity.D = servo_state.pid_velocity.value.count <= 2 ? 0 : servo_state.pid_velocity.value.elements[2];
+  motor.LPF_velocity.Tf = servo_state.pid_velocity.value.count <= 3 ? 0.01 : servo_state.pid_velocity.value.elements[3];
+  motor.PID_velocity.output_ramp =
+      servo_state.pid_velocity.value.count <= 4 ? 300 : servo_state.pid_velocity.value.elements[4];
+  motor.PID_velocity.limit =
+      servo_state.pid_velocity.value.count <= 5 ? 1000 : servo_state.pid_velocity.value.elements[5];
+  motor.motion_downsample = servo_state.pid_velocity.value.count <= 6 ? 0 : servo_state.pid_velocity.value.elements[6];
+
+  motor.P_angle.P = servo_state.pid_position.value.count <= 0 ? 0 : servo_state.pid_position.value.elements[0];
+  motor.P_angle.I = servo_state.pid_position.value.count <= 1 ? 0 : servo_state.pid_position.value.elements[1];
+  motor.P_angle.D = servo_state.pid_position.value.count <= 2 ? 0 : servo_state.pid_position.value.elements[2];
+  // motor.P_angle.output_ramp = 10000; // default 1e6 rad/s^2
+  /* angle low pass filtering, use only for very noisy position sensors - try to avoid and keep the values very small */
+  motor.LPF_angle.Tf = 0;  // default 0[=disabled]
+  // setting the limits
+
   xSemaphoreGive(mutex);
 
   if (!armed) {
@@ -323,6 +385,6 @@ void loop() {
 
   motor.loopFOC();
 #else
-  encoder.update(); // optional: Update manually if not using loopfoc()
+  encoder.update();  // optional: Update manually if not using loopfoc()
 #endif
 }