Algorithm/pid.c

+/****************************************************************************
+ *  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;
+}

Algorithm/pid.h

+/****************************************************************************
+ *  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

Driver/bsp_can.c

+/****************************************************************************
+ *  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); 
+}

Driver/bsp_can.h

+/****************************************************************************
+ *  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

Driver/bsp_key.c

+/****************************************************************************
+ *  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

Driver/bsp_key.h

+/****************************************************************************
+ *  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

Driver/bsp_led.h

+/****************************************************************************
+ *  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

Driver/bsp_oled.h

@@ -11,7 +11,6 @@
 #ifndef __BSP_OLED__H
 #define __BSP_OLED__H
 
-#include "stm32f4xx.h"
 #include "spi.h"
 #include <stdint.h>
 

Driver/bsp_pwm.c

+#include "tim.h"
+#include "bsp_pwm.h"
+
+void PWM_init(void){
+	/* Open 4 sets of PWM waves respectively */
+		/**TIM2 GPIO Configuration    
+	PA8     ------> TIM2_CH1
+	PA9     ------> TIM2_CH2
+	PA10    ------> TIM2_CH3
+	PA11    ------> TIM2_CH4 
+	*/
+	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
+	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
+	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
+	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
+}
+
+void PWM_SetDuty(TIM_HandleTypeDef *tim,uint32_t tim_channel,float duty){
+	
+	switch(tim_channel){	
+		case TIM_CHANNEL_1: tim->Instance->CCR1 = (PWM_RESOLUTION*duty) - 1;break;
+		case TIM_CHANNEL_2: tim->Instance->CCR2 = (PWM_RESOLUTION*duty) - 1;break;
+		case TIM_CHANNEL_3: tim->Instance->CCR3 = (PWM_RESOLUTION*duty) - 1;break;
+		case TIM_CHANNEL_4: tim->Instance->CCR4 = (PWM_RESOLUTION*duty) - 1;break;
+	}
+	
+}
+

Driver/bsp_pwm.h

+#ifndef __BSP_PWM
+#define __BSP_PWM
+
+#include "main.h"
+
+void PWM_SetDuty(TIM_HandleTypeDef *tim,uint32_t tim_channel,float duty);
+void PWM_init(void);
+#endif

README.md

 # BSP
 
-Board Support Package
\ No newline at end of file
+This is our "Board Support Package" repository.
+
+In the "Driver" folder, you will find the layer of code that interacts directly with the HAL Libraries and the DJI Boards.
+
+In the "Device" folder, you will find a layer of code that interacts with the Driver layer, and that represents different devices in our robots' system : the OLED board, the DT7 controller, and more.
\ No newline at end of file