From 798b54aa91251f1022269f1ac4de8e16c737a767 Mon Sep 17 00:00:00 2001
From: vanyabeat <vanyabeat@protonmail.com>
Date: Tue, 5 Mar 2024 21:00:46 +0300
Subject: [PATCH] 	modified:   firmware/src/main.cpp

---
 firmware/src/main.cpp | 126 ++++++++++++++++++++++++------------------
 1 file changed, 71 insertions(+), 55 deletions(-)

diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp
index e2e8109..e1969f8 100644
--- a/firmware/src/main.cpp
+++ b/firmware/src/main.cpp
@@ -12,55 +12,16 @@
 #define AS5040_MISO PB14
 #define AS5040_MOSI PC1
 #define AS5040_SCLK PB10
-
-#define EN_W_Pin LL_GPIO_PIN_6
-#define EN_W_GPIO_Port GPIOC
-#define DRV_FAULT_Pin LL_GPIO_PIN_7
-#define DRV_FAULT_GPIO_Port GPIOC
-#define DRV_RESET_Pin LL_GPIO_PIN_8
-#define DRV_RESET_GPIO_Port GPIOC
-#define DRV_SLEEP_Pin LL_GPIO_PIN_9
-#define DRV_SLEEP_GPIO_Port GPIOC
-#define EN_U_Pin LL_GPIO_PIN_11
-#define EN_U_GPIO_Port GPIOA
-#define EN_V_Pin LL_GPIO_PIN_12
-#define EN_V_GPIO_Port GPIOA
-#define SENSE3_Pin GPIO_PIN_5
-#define SENSE3_GPIO_Port GPIOC
-#define SENSE2_Pin GPIO_PIN_0
-#define SENSE2_GPIO_Port GPIOB
-#define SENSE1_Pin GPIO_PIN_1
-#define SENSE1_GPIO_Port GPIOB
-#define AS5045_CS_Pin GPIO_PIN_15
-#define AS5045_CS_GPIO_Port GPIOB
-#define EN_W_Pin GPIO_PIN_6
-#define EN_W_GPIO_Port GPIOC
-#define DRV_FAULT_Pin GPIO_PIN_7
-#define DRV_FAULT_GPIO_Port GPIOC
-#define DRV_RESET_Pin GPIO_PIN_8
-#define DRV_RESET_GPIO_Port GPIOC
-#define DRV_SLEEP_Pin GPIO_PIN_9
-#define DRV_SLEEP_GPIO_Port GPIOC
-#define EN_U_Pin GPIO_PIN_11
-#define EN_U_GPIO_Port GPIOA
-#define EN_V_Pin GPIO_PIN_12
-#define EN_V_GPIO_Port GPIOA
-#define LED1_Pin GPIO_PIN_10
-#define LED1_GPIO_Port GPIOC
-#define LED2_Pin GPIO_PIN_11
-#define LED2_GPIO_Port GPIOC
-#define LED3_Pin GPIO_PIN_12
-#define LED3_GPIO_Port GPIOC
-#define spi1_cs_Pin GPIO_PIN_2
-#define spi1_cs_GPIO_Port GPIOD
-
+#define CURRENT_SENSOR_1 PB1 // INA180A
+#define CURRENT_SENSOR_2 PB0
+#define CURRENT_SENSOR_3 PC5
 #pragma endregion
 
 AS5040Sensor sensor(AS5040_CS, AS5040_MOSI, AS5040_MISO, AS5040_SCLK);
 // Init iPower 6208
 BLDCMotor motor = BLDCMotor(12);
 BLDCDriver3PWM driver = BLDCDriver3PWM(PA10, PA11, PA12, PC6, PA11, PA12);
-
+InlineCurrentSense current_sense = InlineCurrentSense(0.51, 50, PB1, PB0, PC5);
 Commander command = Commander(Serial);
 void onMotor(char *cmd) { command.motor(&motor, cmd); }
 
@@ -83,32 +44,87 @@ void setup() {
   digitalWrite(PA11, HIGH);
   digitalWrite(PA12, HIGH);
   digitalWrite(PC6, HIGH);
-  Serial.begin(115200);
+
+  // arduino pin initialization INA180A current sensor
+  pinMode(CURRENT_SENSOR_1, OUTPUT);
+  pinMode(CURRENT_SENSOR_2, OUTPUT);
+  pinMode(CURRENT_SENSOR_3, OUTPUT);
+  analogReadResolution(12);
   sensor.init();
   motor.linkSensor(&sensor);
+
+  driver.pwm_frequency = 20000;
   driver.voltage_power_supply = 12;
+  driver.voltage_limit = 12;
   driver.init();
   motor.linkDriver(&driver);
+
+  current_sense.linkDriver(&driver);
+  // init current sense
+  current_sense.init();
+  // link the motor to current sense
+  motor.linkCurrentSense(&current_sense);
+  // control loop type and torque mode
+  motor.torque_controller = TorqueControlType::foc_current;
   motor.controller = MotionControlType::torque;
-  motor.PID_velocity.P = 0.2;
-  motor.PID_velocity.I = 20;
+  motor.motion_downsample = 0;
+
+  // velocity loop PID
+  motor.PID_velocity.P = 0;
+  motor.PID_velocity.I = 0;
   motor.PID_velocity.D = 0;
-  motor.voltage_limit = 12;
-  motor.LPF_velocity.Tf = 0.01;
-  motor.P_angle.P = 20;
-  motor.velocity_limit = 50;
-  Serial.begin(115200);
+  motor.PID_velocity.output_ramp = 0;
+  motor.PID_velocity.limit = 0;
+  // Low pass filtering time constant
+  motor.LPF_velocity.Tf = 0;
+  // angle loop PID
+  motor.P_angle.P = 0;
+  motor.P_angle.I = 0;
+  motor.P_angle.D = 0;
+  motor.P_angle.output_ramp = 0;
+  motor.P_angle.limit = 0;
+  // Low pass filtering time constant
+  motor.LPF_angle.Tf = 0;
+  // current q loop PID
+  motor.PID_current_q.P = 0;
+  motor.PID_current_q.I = 0;
+  motor.PID_current_q.D = 0;
+  motor.PID_current_q.output_ramp = 0;
+  motor.PID_current_q.limit = 0;
+  // Low pass filtering time constant
+  motor.LPF_current_q.Tf = 0;
+  // current d loop PID
+  motor.PID_current_d.P = 0;
+  motor.PID_current_d.I = 0;
+  motor.PID_current_d.D = 0;
+  motor.PID_current_d.output_ramp = 0;
+  motor.PID_current_d.limit = 0;
+  // Low pass filtering time constant
+  motor.LPF_current_d.Tf = 0;
+  // Limits
+  motor.velocity_limit = 0;
+  motor.voltage_limit = 0;
+  motor.current_limit = 0;
+  // sensor zero offset - home position
+  motor.sensor_offset = 0;
+  // general settings
+  // motor phase resistance
+  motor.phase_resistance = 0;
+  // pwm modulation settings
+  motor.foc_modulation = FOCModulationType::SinePWM;
+  motor.modulation_centered = 1;
+  command.add('A', onMotor, "motor");
+  // tell the motor to use the monitoring
   motor.useMonitoring(Serial);
+  motor.monitor_downsample = 0; // disable monitor at first - optional
+
   motor.init();
   motor.initFOC();
-  motor.target = 2;
-  command.add('A', onMotor, "motor");
-
-  _delay(1000);
 }
 
 void loop() {
   motor.loopFOC();
+  motor.monitor();
   motor.move();
   command.run();
 }
\ No newline at end of file