From 631cadca269ddeef919bb4391f5b7832ebe4583f Mon Sep 17 00:00:00 2001
From: sosiskovich <sosiskovich@HIHOME>
Date: Wed, 12 Feb 2025 21:57:27 +0800
Subject: [PATCH] New PID coefficients; remove legacy

---
 controller/fw/embed/src/main.cpp              | 58 +++++++--------
 .../fw/embed/test/python_send_velocity.py     | 70 +++++++++++++++----
 2 files changed, 81 insertions(+), 47 deletions(-)

diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index 36d46d5..c7640d8 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -48,40 +48,37 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
                DRV8313Driver *driver, LowsideCurrentSense *current_sense,
                Commander *commander, CommandCallback callback) {
   encoder->init(&spi);
-  motor->linkSensor(encoder);
+
   driver->pwm_frequency = 20000;
-  driver->voltage_power_supply = 20;
-  driver->voltage_limit = 40;
-  driver->init();
-  motor->linkDriver(driver);
+  driver->voltage_power_supply = 24;
+  driver->voltage_limit = 24;
+  driver->init();  
+
   current_sense->linkDriver(driver);
   current_sense->init();
+
+  motor->linkSensor(encoder);
+  motor->linkDriver(driver);
   motor->linkCurrentSense(current_sense);
-  // SimpleFOCDebug::enable(nullptr);
-  commander->add('M', callback, "motor");
   motor->useMonitoring(Serial);
-  // motor->monitor_start_char = 'M';
-  // motor->monitor_end_char = 'M';
-  // motor->monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE;
-  motor->monitor_downsample = 10; // default 10
-  // commander->verbose = VerboseMode::user_friendly;
+  motor->monitor_downsample = 5000; // default 0
   motor->controller = MotionControlType::angle;
   motor->torque_controller = TorqueControlType::voltage;
-  motor->foc_modulation = FOCModulationType::SinePWM;
-  motor->PID_velocity.P = 0.3;
-  motor->PID_velocity.I = 15;
-  motor->PID_velocity.D = 0.001;
-  motor->PID_velocity.output_ramp = 1000;
-  motor->LPF_velocity.Tf = 0.005;
-  motor->LPF_angle.Tf = 0.005;
-  motor->P_angle.P = 15;
-  // motor->P_angle.I = 15;
-  // motor->P_angle.3 = 15;
-  motor->velocity_limit = 40;
-  motor->voltage_limit = 40;
+  motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
+
+  //PID start
+  motor->PID_velocity.P = 0.75;
+  motor->PID_velocity.I = 20;
+  motor->LPF_velocity.Tf = 0.005; 
+  motor->P_angle.P = 0.5 ;
+  motor->LPF_angle.Tf = 0.001;  
+  //PID end
+
+  motor->velocity_limit = 40; // Ограничение по скорости вращения rad/s (382 rpm)
+  motor->voltage_limit = 24;
   motor->current_limit = 0.5;
+  
   motor->sensor_direction = Direction::CCW;
-  current_sense->skip_align = false;
   motor->init();
   motor->initFOC();
 }
@@ -113,7 +110,6 @@ void send_motor_enabled() {
 }
 
 void send_data() {
-  // TODO(vanyabeat) add can functionability for ROS packets
   send_velocity();
   send_angle();
   send_motor_enabled();
@@ -124,9 +120,11 @@ void send_data() {
 void read_can_step() {
   char flag = CAN_inMsg.buf[0];
   if (flag == 'V') {
+    motor.enable();
     memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[1],
            sizeof(motor_control_inputs.target_velocity));
   } else if (flag == 'A') {
+    motor.enable();
     memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1],
            sizeof(motor_control_inputs.target_angle));
   } else if (flag == 'E') {
@@ -141,7 +139,6 @@ void read_can_step() {
       motor.disable();
     }
   }
-  // motor.target = angle;
   digitalWrite(PC10, !digitalRead(PC10));
 }
 
@@ -152,6 +149,7 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
       motor->controller = MotionControlType::velocity;
     }
     motor->target = motor_control_inputs.target_velocity;
+
   } else {
     if (motor->controller != MotionControlType::angle) {
       motor->controller = MotionControlType::angle;
@@ -168,7 +166,7 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
 void setup() {
   Serial.setRx(HARDWARE_SERIAL_RX_PIN);
   Serial.setTx(HARDWARE_SERIAL_TX_PIN);
-  Serial.begin(19200);
+  Serial.begin(115200);
   // setup led pin
   pinMode(PC11, OUTPUT);
   pinMode(PC10, OUTPUT);
@@ -182,14 +180,10 @@ void setup() {
   SendTimer->attachInterrupt(send_data);
   SendTimer->resume();
   setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);
-  // _delay(1000);
 }
 
-// float current_angle = 0.0;
 void loop() {
   foc_step(&motor, &command);
-  // current_angle = encoder.getAngle();
-  // memcpy(&CAN_TX_msg.buf[1], &current_angle, sizeof(current_angle));
   while (Can.read(CAN_inMsg)) {
     read_can_step();
   }
diff --git a/controller/fw/embed/test/python_send_velocity.py b/controller/fw/embed/test/python_send_velocity.py
index e01f24f..dd46c6c 100644
--- a/controller/fw/embed/test/python_send_velocity.py
+++ b/controller/fw/embed/test/python_send_velocity.py
@@ -1,6 +1,7 @@
 import can
 import struct
-import argparse
+import time
+import sys
 
 # Function to send the target speed
 def send_target_speed(bus, target_speed):
@@ -8,29 +9,68 @@ def send_target_speed(bus, target_speed):
     msg.arbitration_id = 1  # Message ID
     msg.is_extended_id = False
     msg.dlc = 5  # Message length
-    msg.data = [ord('V')] + list(struct.pack('<f', target_speed))  # 'V' for the command identifier, followed by the speed in float format
+    msg.data = bytearray([ord('V')] + list(struct.pack('<f', target_speed)))  # 'V' for the command identifier, followed by the speed in float format
 
     try:
         bus.send(msg)
-        print(f"Sent message with target speed: {target_speed} m/s")
-        print(f"Message data: {msg.data}")
+        print(f"Sent message with target speed: {target_speed} rad/s")
     except can.CanError:
         print("Message failed to send")
 
-# Main function
+# Function to send the motor enable/disable command
+def send_motor_enable(bus, enable):
+    """
+    Sends a command to enable or disable the motor.
+    
+    :param bus: The CAN bus
+    :param enable: 1 to enable the motor, 0 to disable it
+    """
+    msg = can.Message()
+    msg.arbitration_id = 1  # Message ID
+    msg.is_extended_id = False
+    msg.dlc = 2  # Message length (flag + 1 byte of data)
+    msg.data = bytearray([ord('E'), enable])  # 'E' for the command, followed by 0 or 1
+
+    try:
+        bus.send(msg)
+        state = "enabled" if enable else "disabled"
+        print(f"Sent message to {state} motor")
+    except can.CanError as e:
+        print(f"Message failed to send: {e}")
+        sys.exit(1)  # Exit the program on failure
+
+    send_target_speed(bus,0.0)
+
 def main():
-    parser = argparse.ArgumentParser(description="Send target speed over CAN bus.")
-    parser.add_argument("--speed", type=float, required=True, help="Target speed to send over the CAN bus (in m/s)")
-    args = parser.parse_args()
-
-    target_speed = args.speed
-
     # CAN interface setup
-    bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000)  # Ensure the bitrate matches the microcontroller settings
-    print("CAN bus initialized, sending target speed...")
+    bus = None  # Define outside the try block for proper shutdown
+    try:
+        bus = can.interface.Bus(channel='COM4', bustype='slcan', bitrate=1000000)  # Ensure the bitrate matches the microcontroller settings
+        print("CAN bus initialized.")
 
-    # Send the message once
-    send_target_speed(bus, target_speed)
+        while True:
+            user_input = input("Enter target speed: ")
+            if user_input.lower() == 'exit':
+                print("Exiting...")
+                break
+            try:
+                target_speed = float(user_input)
+                send_target_speed(bus, target_speed)
+            except ValueError:
+                print("Invalid input. Please enter a valid number.")
+
+        # Disable motor before exiting
+        send_motor_enable(bus, 0)
+        print("Motor disabled.")
+    
+    except Exception as e:
+        print(f"Error initializing1 CAN bus: {e}")
+        sys.exit(1)
+
+    finally:
+        if bus is not None:
+            bus.shutdown()
+            print("CAN bus shut down.")
 
 if __name__ == '__main__':
     main()