From 4bb6bbb21f61e56e42b63a921e65b79f56a41566 Mon Sep 17 00:00:00 2001
From: Charles <charles.sirois14@gmail.com>
Date: Sun, 16 Feb 2020 14:27:23 -0500
Subject: [PATCH] bsp motor
---
Algorithm/pid.c | 62 +++++++++++++++++++++++++++
Algorithm/pid.h | 50 ++++++++++++++++++++++
Device/motor.c | 0
Device/motor.h | 0
Driver/bsp_can.c | 106 +++++++++++++++++++++++++++++++++++++++++++++++
Driver/bsp_can.h | 40 ++++++++++++++++++
Driver/bsp_key.c | 40 ++++++++++++++++++
Driver/bsp_key.h | 24 +++++++++++
Driver/bsp_led.h | 31 ++++++++++++++
9 files changed, 353 insertions(+)
create mode 100644 Algorithm/pid.c
create mode 100644 Algorithm/pid.h
create mode 100644 Device/motor.c
create mode 100644 Device/motor.h
create mode 100644 Driver/bsp_can.c
create mode 100644 Driver/bsp_can.h
create mode 100644 Driver/bsp_key.c
create mode 100644 Driver/bsp_key.h
create mode 100644 Driver/bsp_led.h
diff --git a/Algorithm/pid.c b/Algorithm/pid.c
new file mode 100644
index 0000000..bd4e3f8
--- /dev/null
+++ b/Algorithm/pid.c
@@ -0,0 +1,62 @@
+/****************************************************************************
+ * Copyright (C) 2018 RoboMaster.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ ***************************************************************************/
+
+#include "pid.h"
+
+/**
+ * @brief init pid parameter
+ * @param pid struct
+ @param parameter
+ * @retval None
+ */
+void pid_init(pid_struct_t *pid,
+ float kp,
+ float ki,
+ float kd,
+ float i_max,
+ float out_max)
+{
+ pid->kp = kp;
+ pid->ki = ki;
+ pid->kd = kd;
+ pid->i_max = i_max;
+ pid->out_max = out_max;
+}
+
+/**
+ * @brief pid calculation
+ * @param pid struct
+ @param reference value
+ @param feedback value
+ * @retval calculation result
+ */
+float pid_calc(pid_struct_t *pid, float ref, float fdb)
+{
+ pid->ref = ref;
+ pid->fdb = fdb;
+ pid->err[1] = pid->err[0];
+ pid->err[0] = pid->ref - pid->fdb;
+
+ pid->p_out = pid->kp * pid->err[0];
+ pid->i_out += pid->ki * pid->err[0];
+ pid->d_out = pid->kd * pid->err[0] - pid->err[1];
+ LIMIT_MIN_MAX(pid->i_out, -pid->i_max, pid->i_max);
+
+ pid->output = pid->p_out + pid->i_out + pid->d_out;
+ LIMIT_MIN_MAX(pid->output, -pid->out_max, pid->out_max);
+ return pid->output;
+}
diff --git a/Algorithm/pid.h b/Algorithm/pid.h
new file mode 100644
index 0000000..69f6f47
--- /dev/null
+++ b/Algorithm/pid.h
@@ -0,0 +1,50 @@
+/****************************************************************************
+ * Copyright (C) 2018 RoboMaster.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ ***************************************************************************/
+
+#ifndef _PID_H
+#define _PID_H
+
+#include "main.h"
+
+typedef struct _pid_struct_t
+{
+ float kp;
+ float ki;
+ float kd;
+ float i_max;
+ float out_max;
+
+ float ref; // target value
+ float fdb; // feedback value
+ float err[2]; // error and last error
+
+ float p_out;
+ float i_out;
+ float d_out;
+ float output;
+}pid_struct_t;
+
+void pid_init(pid_struct_t *pid,
+ float kp,
+ float ki,
+ float kd,
+ float i_max,
+ float out_max);
+
+float pid_calc(pid_struct_t *pid, float ref, float fdb);
+
+#endif
diff --git a/Device/motor.c b/Device/motor.c
new file mode 100644
index 0000000..e69de29
diff --git a/Device/motor.h b/Device/motor.h
new file mode 100644
index 0000000..e69de29
diff --git a/Driver/bsp_can.c b/Driver/bsp_can.c
new file mode 100644
index 0000000..0df7811
--- /dev/null
+++ b/Driver/bsp_can.c
@@ -0,0 +1,106 @@
+/****************************************************************************
+ * Copyright (C) 2018 RoboMaster.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ ***************************************************************************/
+
+#include "bsp_can.h"
+#include "bsp_led.h"
+
+moto_info_t motor_info[MOTOR_MAX_NUM];
+uint16_t can_cnt;
+
+/**
+ * @brief init can filter, start can, enable can rx interrupt
+ * @param hcan pointer to a CAN_HandleTypeDef structure that contains
+ * the configuration information for the specified CAN.
+ * @retval None
+ */
+void can_user_init(CAN_HandleTypeDef* hcan )
+{
+ CAN_FilterTypeDef can_filter;
+
+ can_filter.FilterBank = 0; // filter 0
+ can_filter.FilterMode = CAN_FILTERMODE_IDMASK; // mask mode
+ can_filter.FilterScale = CAN_FILTERSCALE_32BIT;
+ can_filter.FilterIdHigh = 0;
+ can_filter.FilterIdLow = 0;
+ can_filter.FilterMaskIdHigh = 0;
+ can_filter.FilterMaskIdLow = 0; // set mask 0 to receive all can id
+ can_filter.FilterFIFOAssignment = CAN_RX_FIFO0; // assign to fifo0
+ can_filter.FilterActivation = ENABLE; // enable can filter
+ can_filter.SlaveStartFilterBank = 14; // only meaningful in dual can mode
+
+ HAL_CAN_ConfigFilter(hcan, &can_filter); // init can filter
+ HAL_CAN_Start(&hcan1); // start can1
+ HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); // enable can1 rx interrupt
+}
+
+/**
+ * @brief can rx callback, get motor feedback info
+ * @param hcan pointer to a CAN_HandleTypeDef structure that contains
+ * the configuration information for the specified CAN.
+ * @retval None
+ */
+void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
+{
+ CAN_RxHeaderTypeDef rx_header;
+ uint8_t rx_data[8];
+ if(hcan->Instance == CAN1)
+ {
+ HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data); //receive can data
+ }
+ if ((rx_header.StdId >= FEEDBACK_ID_BASE)
+ && (rx_header.StdId < FEEDBACK_ID_BASE + MOTOR_MAX_NUM)) // judge the can id
+ {
+ can_cnt ++;
+ uint8_t index = rx_header.StdId - FEEDBACK_ID_BASE; // get motor index by can_id
+ motor_info[index].rotor_angle = ((rx_data[0] << 8) | rx_data[1]);
+ motor_info[index].rotor_speed = ((rx_data[2] << 8) | rx_data[3]);
+ motor_info[index].torque_current = ((rx_data[4] << 8) | rx_data[5]);
+ motor_info[index].temp = rx_data[6];
+ }
+ if (can_cnt == 500)
+ {
+ can_cnt = 0;
+ LED_GREEN_TOGGLE(); // green led blink indicate can comunication successful
+ }
+}
+
+/**
+ * @brief send motor control message through can bus
+ * @param id_range to select can control id 0x1ff or 0x2ff
+ * @param motor voltage 1,2,3,4 or 5,6,7
+ * @retval None
+ */
+void set_motor_voltage(uint8_t id_range, int16_t v1, int16_t v2, int16_t v3, int16_t v4)
+{
+ CAN_TxHeaderTypeDef tx_header;
+ uint8_t tx_data[8];
+
+ tx_header.StdId = (id_range == 0)?(0x200):(0x1ff);
+ tx_header.IDE = CAN_ID_STD;
+ tx_header.RTR = CAN_RTR_DATA;
+ tx_header.DLC = 8;
+
+ tx_data[0] = (v1>>8)&0xff;
+ tx_data[1] = (v1)&0xff;
+ tx_data[2] = (v2>>8)&0xff;
+ tx_data[3] = (v2)&0xff;
+ tx_data[4] = (v3>>8)&0xff;
+ tx_data[5] = (v3)&0xff;
+ tx_data[6] = (v4>>8)&0xff;
+ tx_data[7] = (v4)&0xff;
+ HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0);
+}
diff --git a/Driver/bsp_can.h b/Driver/bsp_can.h
new file mode 100644
index 0000000..04cee92
--- /dev/null
+++ b/Driver/bsp_can.h
@@ -0,0 +1,40 @@
+/****************************************************************************
+ * Copyright (C) 2018 RoboMaster.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ ***************************************************************************/
+
+#ifndef __BSP_CAN
+#define __BSP_CAN
+
+#include "can.h"
+
+#define FEEDBACK_ID_BASE 0x201
+#define CAN_CONTROL_ID_BASE 0x200
+#define CAN_CONTROL_ID_EXTEND 0x1ff
+#define MOTOR_MAX_NUM 7
+
+typedef struct
+{
+ uint16_t can_id;
+ int16_t set_voltage;
+ uint16_t rotor_angle;
+ int16_t rotor_speed;
+ int16_t torque_current;
+ uint8_t temp;
+}moto_info_t;
+
+void can_user_init(CAN_HandleTypeDef* hcan);
+void set_motor_voltage(uint8_t id_range, int16_t v1, int16_t v2, int16_t v3, int16_t v4);
+#endif
diff --git a/Driver/bsp_key.c b/Driver/bsp_key.c
new file mode 100644
index 0000000..afbef13
--- /dev/null
+++ b/Driver/bsp_key.c
@@ -0,0 +1,40 @@
+/****************************************************************************
+ * Copyright (C) 2018 RoboMaster.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ ***************************************************************************/
+
+#include "bsp_key.h"
+
+/**
+ * @brief detect key presse down event
+ * @param None
+ * @retval event flag
+ */
+uint8_t key_scan(void)
+{
+ static uint8_t key_last = 0;
+ static uint8_t key = 0;
+
+ key_last = key;
+ key = HAL_GPIO_ReadPin(KEY_GPIO_Port,KEY_Pin);
+ if( (key == GPIO_PIN_RESET) && (key_last == GPIO_PIN_SET))
+ {
+ return 1;
+ }
+ else
+ {
+ return 0;
+ }
+}
\ No newline at end of file
diff --git a/Driver/bsp_key.h b/Driver/bsp_key.h
new file mode 100644
index 0000000..00ca3f2
--- /dev/null
+++ b/Driver/bsp_key.h
@@ -0,0 +1,24 @@
+/****************************************************************************
+ * Copyright (C) 2018 RoboMaster.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ ***************************************************************************/
+
+#ifndef __BSP_KEY
+#define __BSP_KEY
+
+#include "gpio.h"
+
+uint8_t key_scan(void);
+#endif
diff --git a/Driver/bsp_led.h b/Driver/bsp_led.h
new file mode 100644
index 0000000..b4181c4
--- /dev/null
+++ b/Driver/bsp_led.h
@@ -0,0 +1,31 @@
+/****************************************************************************
+ * Copyright (C) 2018 RoboMaster.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ ***************************************************************************/
+
+#ifndef __BSP_LED
+#define __BSP_LED
+
+#define LED_GREEN_ON() HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_RESET)
+#define LED_GREEN_OFF() HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_SET)
+#define LED_GREEN_TOGGLE() LED_GREEN_GPIO_Port->ODR ^= LED_GREEN_Pin
+
+#define LED_RED_ON() HAL_GPIO_WritePin(LED_RED_GPIO_Port, LED_RED_Pin, GPIO_PIN_RESET)
+#define LED_RED_OFF() HAL_GPIO_WritePin(LED_RED_GPIO_Port, LED_RED_Pin, GPIO_PIN_SET)
+#define LED_RED_TOGGLE() LED_RED_GPIO_Port->ODR ^= LED_RED_Pin
+
+#define LED_ALL_OFF LED_RED_OFF(); LED_RED_OFF();
+
+#endif
--
GitLab