From bbdf967e6d2e88431cf8387bf4bfc7a811925326 Mon Sep 17 00:00:00 2001
From: lulko <keklolov7@gmail.com>
Date: Tue, 11 Mar 2025 17:49:50 +0300
Subject: [PATCH] Fix CAN

---
 controller/fw/embed/include/reg_cah.h |  4 +--
 controller/fw/embed/platformio.ini    |  2 ++
 controller/fw/embed/src/main.cpp      | 38 ++++++++++++++++++++-------
 3 files changed, 33 insertions(+), 11 deletions(-)

diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h
index 61d7eac..2a0c51d 100644
--- a/controller/fw/embed/include/reg_cah.h
+++ b/controller/fw/embed/include/reg_cah.h
@@ -1,8 +1,8 @@
 #ifndef REG_CAH_H_
 #define REG_CAH_H_
 
-#define APP_ADDR        0x08004000  // 16KB - Application
-#define ADDR_VAR        0x80400000
+#define APP_ADDR        0x0800400  // 16KB - Application
+#define ADDR_VAR        0x8040000
 #define ADDR_VAR_ID     ADDR_VAR
 #define ADDR_VAR_P      (ADDR_VAR + 1)
 #define ADDR_VAR_I      (ADDR_VAR + 2)
diff --git a/controller/fw/embed/platformio.ini b/controller/fw/embed/platformio.ini
index a759816..8b6a522 100644
--- a/controller/fw/embed/platformio.ini
+++ b/controller/fw/embed/platformio.ini
@@ -18,10 +18,12 @@ upload_protocol = stlink
 debug_tool = stlink
 monitor_speed = 19200
 monitor_parity = N
+
 build_flags = 
 	-DSTM32F446xx
 	-D HAL_CAN_MODULE_ENABLED
 	-D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH
+	
 lib_deps = 
 	askuric/Simple FOC@^2.3.4
 	pazi88/STM32_CAN@^1.1.2
diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index 28f5f32..31d81ef 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -1,5 +1,6 @@
 // clang-format off
 #include <Arduino.h>
+#include "stm32f446xx.h"
 #include <SimpleFOC.h>
 #include <STM32_CAN.h>
 #include <AS5045.h>
@@ -126,7 +127,8 @@ void send_motor_enabled() {
 }
 
 void send_foc_state() {
-  CAN_TX_msg.id = 1;
+  uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID;
+  CAN_TX_msg.id = FOC_STATE;
   CAN_TX_msg.buf[0] = 'F';
   memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.foc_state,
     sizeof(motor_control_inputs.foc_state));
@@ -135,13 +137,11 @@ Can.write(CAN_TX_msg);
 
 void send_id() {
   /*          data for reading of firmware      */
-  FLASH->KEYR = 0x45670123;  // Первый ключ
-  FLASH->KEYR = 0xCDEF89AB;  // Второй ключ
-  uint8_t id = *(volatile uint32_t*)ADDR_VAR_ID;
-  FLASH->CR |=  FLASH_CR_LOCK;
+ 
+  uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID;
   CAN_TX_msg.id = id;
   memcpy(&CAN_TX_msg.buf[0], &id, sizeof(id));
-Can.write(CAN_TX_msg);
+  Can.write(CAN_TX_msg);
 }
 
 void setup_id(uint8_t my_id) {
@@ -203,8 +203,8 @@ void send_data() {
 
 
 void listen_can() {
-  uint8_t reg_id = CAN_inMsg.id;
-  if (CAN_inMsg.buf[0] == *(volatile uint8_t*)ADDR_VAR_ID) {
+  uint8_t reg_id = CAN_inMsg.buf[0];  //reg id
+  if (CAN_inMsg.id == *(volatile uint8_t*)ADDR_VAR_ID) {
   if (CAN_inMsg.buf[1] == REG_WRITE) {
     switch (reg_id) {
     case REG_ID:
@@ -237,7 +237,22 @@ void listen_can() {
     case REG_ID:
       send_id();
       break;
+    case MOTOR_VELOCITY:
+      send_velocity();
+      break;
 
+    case MOTOR_ANGLE:
+      send_angle();
+      break;
+
+    case MOTOR_ENABLED:
+      send_motor_enabled();
+      break;
+
+    case FOC_STATE:
+      send_foc_state();
+      break;
+    
     default:
       break;
     }
@@ -277,7 +292,8 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
 
 void setup() {
   /*   bias for vector int    */
-  SCB->VTOR = APP_ADDR;
+  //SCB->VTOR = 0x08004000;
+
   
   Serial.setRx(HARDWARE_SERIAL_RX_PIN);
   Serial.setTx(HARDWARE_SERIAL_TX_PIN);
@@ -294,6 +310,10 @@ void setup() {
   SendTimer->setOverflow(100, HERTZ_FORMAT);  // 50 Hz
   SendTimer->attachInterrupt(send_data);
   SendTimer->resume();
+  for(int i = 0;i < 10;i++){
+    GPIOC->ODR ^= GPIO_ODR_OD10;
+    HAL_Delay(1000);
+  }
   setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);
 
   /*            Настройка параметров стирания         */