diff --git a/Algorithm/pid.c b/Algorithm/pid.c
new file mode 100644
index 0000000000000000000000000000000000000000..fd82c7a816215e3d8053244dfc8fa9b4a7294099
--- /dev/null
+++ b/Algorithm/pid.c
@@ -0,0 +1,68 @@
+/****************************************************************************
+ * 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"
+#include "motor.h"
+//#include <stdlib.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;
+ if(pid->err[0]>4096){pid->err[0] = pid->err[0] - 8192;}
+ if(pid->err[0]<-4096){pid->err[0] = pid->err[0] + 8192;}
+
+ 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 0000000000000000000000000000000000000000..69f6f47fe7fb8c3919d3e5cbe04127db84a6a511
--- /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/Remote_Control.c b/Device/Remote_Control.c
new file mode 100644
index 0000000000000000000000000000000000000000..d6c3aaaee758eeee7b0fc906be215892de307c3e
--- /dev/null
+++ b/Device/Remote_Control.c
@@ -0,0 +1,90 @@
+/**
+ ******************************************************************************
+ * @file Remote_Control.c
+ * @author DJI
+ * @version V1.0.0
+ * @date 2015/11/15
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#include "Remote_Control.h"
+#define LED2_Pin GPIO_PIN_7
+#define LED2_GPIO_Port GPIOE
+RC_Type remote_control;
+uint32_t Latest_Remote_Control_Pack_Time = 0;
+uint32_t LED_Flash_Timer_remote_control = 0;
+/*******************************************************************************************
+ * @Func void Callback_RC_Handle(RC_Type* rc, uint8_t* buff)
+ * @Brief DR16接收机协议解码程序
+ * @Param RC_Type* rc 存储遥控器数据的结构体 uint8_t* buff 用于解码的缓存
+ * @Retval None
+ * @Date
+ *******************************************************************************************/
+void Callback_RC_Handle(RC_Type* rc, uint8_t* buff)
+{
+// rc->ch1 = (*buff | *(buff+1) << 8) & 0x07FF; offset = 1024
+ rc->ch1 = (buff[0] | buff[1]<<8) & 0x07FF;
+ rc->ch1 -= 1024;
+ rc->ch2 = (buff[1]>>3 | buff[2]<<5 ) & 0x07FF;
+ rc->ch2 -= 1024;
+ rc->ch3 = (buff[2]>>6 | buff[3]<<2 | buff[4]<<10) & 0x07FF;
+ rc->ch3 -= 1024;
+ rc->ch4 = (buff[4]>>1 | buff[5]<<7) & 0x07FF;
+ rc->ch4 -= 1024;
+
+ rc->switch_left = ( (buff[5] >> 4)& 0x000C ) >> 2;
+ rc->switch_right = (buff[5] >> 4)& 0x0003 ;
+
+ rc->mouse.x = buff[6] | (buff[7] << 8); //x axis
+ rc->mouse.y = buff[8] | (buff[9] << 8);
+ rc->mouse.z = buff[10]| (buff[11] << 8);
+
+ rc->mouse.press_left = buff[12]; // is pressed?
+ rc->mouse.press_right = buff[13];
+
+ rc->keyBoard.key_code = buff[14] | buff[15] << 8; //key borad code
+
+ Latest_Remote_Control_Pack_Time = HAL_GetTick();
+
+ if(Latest_Remote_Control_Pack_Time - LED_Flash_Timer_remote_control>500){
+
+ HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
+
+ LED_Flash_Timer_remote_control = Latest_Remote_Control_Pack_Time;
+
+
+ }
+
+}
+
+extern uint16_t TIM_COUNT[];
+int16_t HighTime;
+
+
+/*******************************************************************************************
+ * @Func void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
+ * @Brief PWM接收机脉宽计算
+ * @Param TIM_HandleTypeDef *htim 用于测量PWM脉宽的定时器。
+ * @Retval None
+ * @Date
+ *******************************************************************************************/
+void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
+{
+
+ HighTime = (TIM_COUNT[1] - TIM_COUNT[0])>0?(TIM_COUNT[1] - TIM_COUNT[0]):((TIM_COUNT[1] - TIM_COUNT[0])+10000);
+
+ Latest_Remote_Control_Pack_Time = HAL_GetTick();
+
+ remote_control.ch4 = (HighTime - 4000)*660/4000;
+
+ if(Latest_Remote_Control_Pack_Time - LED_Flash_Timer_remote_control>500){
+
+ HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
+ LED_Flash_Timer_remote_control = Latest_Remote_Control_Pack_Time;
+
+ }
+
+}
diff --git a/Device/Remote_Control.h b/Device/Remote_Control.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b3096eb5666b52930403f79cf8c661e754ce7a5
--- /dev/null
+++ b/Device/Remote_Control.h
@@ -0,0 +1,70 @@
+/**
+ ******************************************************************************
+ * @file Remote_Control.h
+ * @author Ginger
+ * @version V1.0.0
+ * @date 2015/11/14
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+
+#ifndef __RC__
+#define __RC__
+/* Includes ------------------------------------------------------------------*/
+#include "stm32f4xx_hal.h"
+
+#define RC_Frame_Lentgh 18
+
+typedef struct {
+ int16_t ch1; //each ch value from -364 -- +364
+ int16_t ch2;
+ int16_t ch3;
+ int16_t ch4;
+
+ uint8_t switch_left; //3 value
+ uint8_t switch_right;
+
+ struct {
+ int16_t x;
+ int16_t y;
+ int16_t z;
+
+ uint8_t press_left;
+ uint8_t press_right;
+ }mouse;
+
+ struct {
+ uint16_t key_code;
+/**********************************************************************************
+ * 键盘通道:15 14 13 12 11 10 9 8 7 6 5 4 3 2 1
+ * V C X Z G F R E Q CTRL SHIFT D A S W
+************************************************************************************/
+
+ }keyBoard;
+
+
+}RC_Type;
+
+
+
+enum{
+ Switch_Up = 1,
+ Switch_Middle = 3,
+ Switch_Down = 2,
+};
+
+enum{
+ Key_W,
+ Key_S,
+ //...
+};
+
+extern RC_Type remote_control;
+extern uint32_t Latest_Remote_Control_Pack_Time ;
+void Callback_RC_Handle(RC_Type* rc, uint8_t* buff);
+#endif
+
+
diff --git a/Device/list_motor.c b/Device/list_motor.c
new file mode 100644
index 0000000000000000000000000000000000000000..781a9561f8081916d3c1a8396eff8b351980168d
--- /dev/null
+++ b/Device/list_motor.c
@@ -0,0 +1,35 @@
+#include "motor.h"
+#include "oled.h"
+#include "list_motor.h"
+
+motor_t gm6020;
+motor_t m3508;
+const pid_struct_t M3508_pid_speed = {
+ 2, 0.005, 2, 16000, 16000
+ };
+const pid_struct_t M3508_pid_pos = {
+ 0.1, 0.005, 0, 16000, 16000
+ };
+
+const pid_struct_t GM6020_pid_speed = {
+ 40, 3, 0, 30000, 30000
+ };
+const pid_struct_t GM6020_pid_pos = {
+ 30, 4, 15, 10000, 10000
+ };
+
+void motors_init(void){
+ gm6020.type = GM6020;
+ gm6020.can_info.can_id = 0;
+ gm6020.can_info.tx_id = 1;
+ gm6020.can_info.rx_id = 0x205;
+ gm6020.pid_pos = GM6020_pid_pos;
+ gm6020.pid_speed = GM6020_pid_speed;
+ m3508.type = M3508;
+ m3508.can_info.can_id = 0;
+ m3508.can_info.tx_id = 0;
+ m3508.can_info.rx_id = 0x201;
+ m3508.pid_pos = M3508_pid_pos;
+ m3508.pid_speed = M3508_pid_speed;
+}
+
diff --git a/Device/list_motor.h b/Device/list_motor.h
new file mode 100644
index 0000000000000000000000000000000000000000..c802f7d3e759e33862d84684d4ee43252fd20bd1
--- /dev/null
+++ b/Device/list_motor.h
@@ -0,0 +1,20 @@
+#include "motor.h"
+#include "oled.h"
+
+
+extern motor_t m3508_frontleft;
+extern motor_t m3508_frontright;
+extern motor_t m3508_backleft;
+extern motor_t m3508_backright;
+extern motor_t gm6020_down;
+extern motor_t gm6020_up;
+extern motor_t m2006;
+extern motor_t gm6020;
+extern motor_t m3508;
+
+extern const pid_struct_t M3508_pid_speed;
+extern const pid_struct_t M3508_pid_pos;
+extern const pid_struct_t GM6020_pid_speed;
+extern const pid_struct_t GM6020_pid_pos;
+
+void motors_init(void);
diff --git a/Device/motor.c b/Device/motor.c
new file mode 100644
index 0000000000000000000000000000000000000000..5c2d009b55d0f105e3c777cf1372afa924c8752a
--- /dev/null
+++ b/Device/motor.c
@@ -0,0 +1,95 @@
+#include "motor.h"
+#include "oled.h"
+#include "pid.h"
+//#include "list_motor.h"
+
+void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *motor3, motor_t *motor4) {
+ int16_t sp1, sp2, sp3, sp4;
+ uint16_t tx_id; //can_id 閠ait d閏lar� l� mais pas utilis�
+ sp1 = sp2 = sp3 = sp4 = 0;
+ tx_id = id;
+
+ if (motor1) { sp1 = correct_output(motor1); }
+ if (motor2) { sp2 = correct_output(motor2); }
+ if (motor3) { sp3 = correct_output(motor3); }
+ if (motor4) { sp4 = correct_output(motor4); }
+
+ can1_transmit(tx_id, sp1, sp2, sp3, sp4);
+}
+
+
+static int16_t correct_output(motor_t *motor) {
+ return motor->voltage;
+}
+
+
+static void get_6020_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
+ motor->info.GM6020_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
+ motor->info.GM6020_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
+ motor->info.GM6020_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
+ motor->info.GM6020_info.temp = (int8_t)(motor_buf[6]);
+ /*
+ oled_clear(Pen_Clear);
+ oled_printf(1,1,"Val: %i", motor->info.GM6020_info.angle);
+ */
+}
+
+static void get_3508_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
+ motor->info.M3508_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
+ motor->info.M3508_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
+ motor->info.M3508_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
+ motor->info.M3508_info.temp = (int8_t)(motor_buf[6]);
+}
+
+void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb)
+{
+
+ motor->voltage = pid_calc(&pid_param, target, fdb);
+}
+
+
+uint8_t get_motor_data(motor_t *motor) {
+ static uint8_t motor_buf[CAN_DATA_SIZE];
+ can1_read(motor->can_info.rx_id, motor_buf);
+
+
+ switch (motor->type) {
+ case GM6020:
+ get_6020_data(motor, motor_buf);
+ return 1;
+ case M3508:
+ get_3508_data(motor, motor_buf);
+ return 1;
+ default:
+ return 0;
+ }
+}
+
+
+void set_motor_position(motor_t *motor)
+{
+ float target = motor->target;
+ switch(motor->type){
+ case M3508:
+ run_motor_from_command(motor, motor->pid_pos, target, motor->info.M3508_info.angle);
+ case GM6020:
+ run_motor_from_command(motor, motor->pid_pos, target, motor->info.GM6020_info.angle);
+ /*default:
+ bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
+ }
+
+}
+
+
+void set_motor_speed(motor_t *motor)
+{
+ float target = motor->target;
+ switch(motor->type){
+ case M3508:
+ run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);;
+ case GM6020:
+ run_motor_from_command(motor, motor->pid_speed, target, motor->info.GM6020_info.speed);;
+ /*default:
+ bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
+ }
+}
diff --git a/Device/motor.h b/Device/motor.h
new file mode 100644
index 0000000000000000000000000000000000000000..53b8dfc0c7c045a5d13418e2e7438ac2e9b1bd94
--- /dev/null
+++ b/Device/motor.h
@@ -0,0 +1,88 @@
+#ifndef _MOTOR_H_
+#define _MOTOR_H_
+
+#include "bsp_can.h"
+#include "bsp_pwm.h"
+#include "pid.h"
+
+
+#define CAN_TX1_ID 0x200
+#define CAN_TX2_ID 0x1FF
+
+#define CAN_RX1_START 0x201
+#define CAN_RX2_START 0x205
+
+enum motors {
+ /* can motors */
+ M3508,
+ GM6020,
+ M2006,
+ general_motor,
+ /* pwm motors */
+ M2305,
+};
+//
+
+typedef struct {
+ uint16_t rx_id;
+ uint16_t tx_id;
+ uint8_t can_id;
+} can_info_t;
+
+
+
+/**
+ * @struct GM6020_info_t
+ * @brief store GM6020 motor data
+ * @var rx_id sensor CAN receive id
+ * @var tx_id sensor CAN transmit id
+ * @var can_id CAN id chosen from [CAN1, CAN2]
+ * @var angle most recent angle data
+ * @var current_get actual current / torque output
+ * @var current_set target current / torque output
+ */
+typedef struct {
+ int16_t angle;
+ int16_t speed;
+ int16_t current;
+ int8_t temp;
+} GM6020_info_t;
+
+typedef struct {
+ int16_t angle;
+ int16_t speed;
+ int16_t current;
+ int8_t temp;
+} M3508_info_t;
+
+typedef union{
+ GM6020_info_t GM6020_info;
+ M3508_info_t M3508_info;
+} motor_info_t;
+
+
+// Une struct par moteur
+typedef struct
+{
+ can_info_t can_info; // Can ID information
+ //M3508_info_t info;
+ motor_info_t info; // Specific info about the motor
+ int type; // Type du moteur, ex:GM6020
+ pid_struct_t pid_pos; // Controler
+ pid_struct_t pid_speed; // Controler
+ float target;
+ float voltage;
+} motor_t;
+
+//id:0 -> 0x200, 1 -> 0x1ff
+void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *motor3, motor_t *motor4);
+
+static int16_t correct_output(motor_t *motor);
+uint8_t get_motor_data(motor_t *motor);
+static void get_6020_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]);
+
+void set_motor_position(motor_t *motor);
+void set_motor_speed(motor_t *motor);
+void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb);
+
+#endif
diff --git a/Driver/bsp_can.c b/Driver/bsp_can.c
new file mode 100644
index 0000000000000000000000000000000000000000..9fdc845f2cbffa822fa962db42092f4cdc45155c
--- /dev/null
+++ b/Driver/bsp_can.c
@@ -0,0 +1,116 @@
+/****************************************************************************
+ * 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"
+
+uint16_t can_cnt;
+uint8_t can1_rx_buffer[MOTOR_MAX_NUM][CAN_DATA_SIZE];
+
+/**
+ * @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;
+ /* get device id ahead of time */
+ rx_header.StdId = (CAN_RI0R_STID & hcan->Instance->sFIFOMailBox[CAN_RX_FIFO0].RIR) >> CAN_TI0R_STID_Pos;
+
+ if(hcan->Instance == CAN1)
+ {
+ 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
+ HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, can1_rx_buffer[index]); //receive can data
+ }
+ }
+ if (can_cnt == 500)
+ {
+ can_cnt = 0;
+ LED_GREEN_TOGGLE(); // green led blink indicate can comunication successful
+ }
+}
+
+
+
+void can1_transmit(uint16_t id, int16_t msg1, int16_t msg2, int16_t msg3, int16_t msg4) {
+ can_transmit(id, msg1, msg2, msg3, msg4);
+}
+
+void can1_read(uint16_t id, uint8_t buf[CAN_DATA_SIZE]) {
+ uint8_t idx = id - FEEDBACK_ID_BASE;
+ memcpy(buf, can1_rx_buffer[idx], CAN_DATA_SIZE);
+}
+
+/**
+ * @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 can_transmit(uint8_t id, int16_t val1, int16_t val2, int16_t val3, int16_t val4)
+{
+ CAN_TxHeaderTypeDef tx_header;
+ uint8_t tx_data[8];
+
+ tx_header.StdId = (id == 0)?(0x200):(0x1ff);
+ tx_header.IDE = CAN_ID_STD;
+ tx_header.RTR = CAN_RTR_DATA;
+ tx_header.DLC = 8;
+ tx_header.TransmitGlobalTime = DISABLE;
+
+ tx_data[0] = (val1>>8)&0xff;
+ tx_data[1] = (val1)&0xff;
+ tx_data[2] = (val2>>8)&0xff;
+ tx_data[3] = (val2)&0xff;
+ tx_data[4] = (val3>>8)&0xff;
+ tx_data[5] = (val3)&0xff;
+ tx_data[6] = (val4>>8)&0xff;
+ tx_data[7] = (val4)&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 0000000000000000000000000000000000000000..7f8b125909ff05b403ebb1c1a3f672c84ba126da
--- /dev/null
+++ b/Driver/bsp_can.h
@@ -0,0 +1,45 @@
+/****************************************************************************
+ * 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"
+#include <stdio.h>
+#include <string.h>
+
+#define FEEDBACK_ID_BASE 0x204
+#define CAN_CONTROL_ID_BASE 0x200
+#define CAN_CONTROL_ID_EXTEND 0x1ff
+#define MOTOR_MAX_NUM 7
+#define CAN_DATA_SIZE 8
+
+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 can1_transmit(uint16_t id, int16_t val1, int16_t val2, int16_t val3, int16_t val4);
+void can1_read(uint16_t id, uint8_t buf[CAN_DATA_SIZE]);
+void can_transmit(uint8_t id_range, int16_t val1, int16_t val2, int16_t val3, int16_t val4);
+#endif
diff --git a/Driver/bsp_key.c b/Driver/bsp_key.c
new file mode 100644
index 0000000000000000000000000000000000000000..a3e90b674993326bbd96685f5b67bdd9d6cb0877
--- /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;
+ }
+}
diff --git a/Driver/bsp_key.h b/Driver/bsp_key.h
new file mode 100644
index 0000000000000000000000000000000000000000..00ca3f2885854b421d7a23fd521d218dd65cb7aa
--- /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 0000000000000000000000000000000000000000..b4181c4af0461618cab65cfc32e282aa8eed1577
--- /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
diff --git a/Driver/bsp_oled.h b/Driver/bsp_oled.h
index 68c571fb856f519a45c5b0122a6d329c2675ba16..f59d9a959e8db3036a7cc31e58c9149bc2ca25d8 100644
--- a/Driver/bsp_oled.h
+++ b/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>
diff --git a/Driver/bsp_pwm.c b/Driver/bsp_pwm.c
new file mode 100644
index 0000000000000000000000000000000000000000..970ef37a991a1634880937e885929c6a9e9d76d8
--- /dev/null
+++ b/Driver/bsp_pwm.c
@@ -0,0 +1,81 @@
+/****************************************************************************
+ * 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_pwm.h"
+
+/**
+ * @brief start pwm output
+ * @param None
+ * @retval None
+ */
+void pwm_init(void)
+{
+ HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); // start pwm output
+ HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
+ HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
+ HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
+}
+
+/**
+ * @brief sets PWM duty cycle (= speed) for a particular channel
+ * @param Timer, channel (1 to 4), duty cycle (between 0 and 1)
+ * @retval None
+ */
+void PWM_SetDuty(TIM_HandleTypeDef *tim,uint32_t tim_channel,float duty){
+ duty = duty/10 + 0.10f; //rescale before send
+ 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;
+ }
+}
+
+/**
+ * @brief sets PWM duty cycle (= speed) for a all channels
+ * @param Timer, duty cycles (between 0 and 1)
+ * @retval None
+ */
+void PWM_SetAllDuty(TIM_HandleTypeDef *tim, float duty_ch1, float duty_ch2, float duty_ch3, float duty_ch4){
+ duty_ch1 = duty_ch1/10 + 0.10f; //rescale before send
+ duty_ch2 = duty_ch2/10 + 0.10f;
+ duty_ch3 = duty_ch3/10 + 0.10f;
+ duty_ch4 = duty_ch4/10 + 0.10f;
+ tim->Instance->CCR1 = (PWM_RESOLUTION*duty_ch1) - 1;
+ tim->Instance->CCR2 = (PWM_RESOLUTION*duty_ch2) - 1;
+ tim->Instance->CCR3 = (PWM_RESOLUTION*duty_ch3) - 1;
+ tim->Instance->CCR4 = (PWM_RESOLUTION*duty_ch4) - 1;
+
+}
+
+/**
+ * @brief scales all PWM duty cycles between 0 and 1
+ * @param Timer
+ * @retval None
+ */
+void PWM_ScaleAll(TIM_HandleTypeDef *tim){ //il faudrait jouer sur l'allumage des ports d'alimentation des snails (voir Nathan pour plus de d閠ails)
+ HAL_GPIO_WritePin(GPIOH, POWER1_CTRL_Pin, GPIO_PIN_RESET); // switch off 24v power
+ PWM_SetAllDuty(&htim1,1,1,1,1);
+ HAL_Delay(10);
+
+ HAL_GPIO_WritePin(GPIOH, POWER1_CTRL_Pin, GPIO_PIN_SET); // switch on 24v power
+ HAL_Delay(3500);
+
+ PWM_SetAllDuty(&htim1,0,0,0,0);
+ HAL_Delay(500);
+
+}
diff --git a/Driver/bsp_pwm.h b/Driver/bsp_pwm.h
new file mode 100644
index 0000000000000000000000000000000000000000..da42cf6d0019e5dcfcb2f6bef0ddb4932ba0a0aa
--- /dev/null
+++ b/Driver/bsp_pwm.h
@@ -0,0 +1,28 @@
+/****************************************************************************
+ * 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_PWM
+#define __BSP_PWM
+
+#include "tim.h"
+
+void pwm_init(void);
+void PWM_SetDuty(TIM_HandleTypeDef *tim,uint32_t tim_channel,float duty);
+void PWM_SetAllDuty(TIM_HandleTypeDef *tim, float duty_ch1, float duty_ch2, float duty_ch3, float duty_ch4);
+void PWM_ScaleAll(TIM_HandleTypeDef *tim);
+
+#endif
diff --git a/Driver/bsp_uart.c b/Driver/bsp_uart.c
new file mode 100644
index 0000000000000000000000000000000000000000..ac5a8ddd192207b14a161bba9d43474c2af12bc2
--- /dev/null
+++ b/Driver/bsp_uart.c
@@ -0,0 +1,179 @@
+/**
+ ***************************************(C) COPYRIGHT 2018 DJI***************************************
+ * @file bsp_uart.c
+ * @brief this file contains rc data receive and processing function
+ * @note
+ * @Version V1.0.0
+ * @Date Jan-30-2018
+ ***************************************(C) COPYRIGHT 2018 DJI***************************************
+ */
+
+#include "string.h"
+#include "stdlib.h"
+#include "bsp_uart.h"
+#include "usart.h"
+#include "main.h"
+#include "oled.h"
+uint8_t dbus_buf[DBUS_BUFLEN];
+rc_info_t rc;
+
+int is_working = 0;
+
+
+/**
+ * @brief enable global uart it and do not use DMA transfer done it
+ * @param[in] huart: uart IRQHandler id
+ * @param[in] pData: receive buff
+ * @param[in] Size: buff size
+ * @retval set success or fail
+ */
+static int uart_receive_dma_no_it(UART_HandleTypeDef* huart, uint8_t* pData, uint32_t Size)
+{
+ uint32_t tmp1 = 0;
+
+ tmp1 = huart->RxState;
+
+ if (tmp1 == HAL_UART_STATE_READY)
+ {
+ if ((pData == NULL) || (Size == 0))
+ {
+ return HAL_ERROR;
+ }
+
+ huart->pRxBuffPtr = pData;
+ huart->RxXferSize = Size;
+ huart->ErrorCode = HAL_UART_ERROR_NONE;
+
+ /* Enable the DMA Stream */
+ HAL_DMA_Start(huart->hdmarx, (uint32_t)&huart->Instance->DR, (uint32_t)pData, Size);
+
+ /*
+ * Enable the DMA transfer for the receiver request by setting the DMAR bit
+ * in the UART CR3 register
+ */
+ SET_BIT(huart->Instance->CR3, USART_CR3_DMAR);
+
+ return HAL_OK;
+ }
+ else
+ {
+ return HAL_BUSY;
+ }
+}
+
+/**
+ * @brief returns the number of remaining data units in the current DMAy Streamx transfer.
+ * @param[in] dma_stream: where y can be 1 or 2 to select the DMA and x can be 0
+ * to 7 to select the DMA Stream.
+ * @retval The number of remaining data units in the current DMAy Streamx transfer.
+ */
+uint16_t dma_current_data_counter(DMA_Stream_TypeDef *dma_stream)
+{
+ /* Return the number of remaining data units for DMAy Streamx */
+ return ((uint16_t)(dma_stream->NDTR));
+}
+
+
+
+/**
+ * @brief handle received rc data
+ * @param[out] rc: structure to save handled rc data
+ * @param[in] buff: the buff which saved raw rc data
+ * @retval
+ */
+void rc_callback_handler(rc_info_t *rc, uint8_t *buff)
+{
+
+ rc->ch1 = (buff[4]);// & 0x07FF; //0x07FF met la commande sur 11 bits (donc 2048 max)
+ rc->ch2 = (buff[5]);// & 0x07FF;
+ rc->ch3 = (buff[6]);// & 0x07FF;
+ rc->ch4 = (buff[7]);// & 0x07FF;
+
+/*
+ rc->ch1 = (buff[0] | buff[1] << 8) & 0x07FF; //0x07FF met la commande sur 11 bits (donc 2048 max)
+ rc->ch1 -= 1024; //on centre sur 0
+ rc->ch2 = (buff[1] >> 3 | buff[2] << 5) & 0x07FF;
+ //rc->ch2 -= 1024;
+ rc->ch3 = (buff[2] >> 6 | buff[3] << 2 | buff[4] << 10) & 0x07FF;
+ rc->ch3 -= 1024;
+ rc->ch4 = (buff[4] >> 1 | buff[5] << 7) & 0x07FF;
+ //rc->ch4 -= 1024;
+ rc->sw1 = ((buff[5] >> 4) & 0x000C) >> 2;
+ rc->sw2 = (buff[5] >> 4) & 0x0003;
+ *//*
+ if ((abs(rc->ch1) > 660) || \
+ (abs(rc->ch2) > 660) || \
+ (abs(rc->ch3) > 660) || \
+ (abs(rc->ch4) > 660))
+ {
+ memset(rc, 0, sizeof(rc_info_t));
+ } */
+}
+
+/**
+ * @brief clear idle it flag after uart receive a frame data
+ * @param[in] huart: uart IRQHandler id
+ * @retval
+ */
+static void uart_rx_idle_callback(UART_HandleTypeDef* huart)
+{
+ /* clear idle it flag avoid idle interrupt all the time */
+ __HAL_UART_CLEAR_IDLEFLAG(huart);
+
+
+
+ /* handle received data in idle interrupt */
+ if (huart == &DBUS_HUART)
+ {
+ /* clear DMA transfer complete flag */
+ __HAL_DMA_DISABLE(huart->hdmarx);
+
+ /* handle dbus data dbus_buf from DMA */
+ if ((DBUS_MAX_LEN - dma_current_data_counter(huart->hdmarx->Instance)) == DBUS_BUFLEN)
+ {
+ rc_callback_handler(&rc, dbus_buf);
+ }
+
+ /* restart dma transmission */
+ __HAL_DMA_SET_COUNTER(huart->hdmarx, DBUS_MAX_LEN);
+ __HAL_DMA_ENABLE(huart->hdmarx);
+ }
+}
+
+/**
+ * @brief callback this function when uart interrupt
+ * @param[in] huart: uart IRQHandler id
+ * @retval
+ */
+void uart_receive_handler(UART_HandleTypeDef *huart)
+{
+ if(!is_working) {
+// oled_clear(Pen_Clear);
+ oled_printf(1, 1,"working");
+ oled_refresh_gram();
+ is_working = 1;
+ }
+ if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE) &&
+ __HAL_UART_GET_IT_SOURCE(huart, UART_IT_IDLE))
+ {
+ uart_rx_idle_callback(huart);
+ }
+
+}
+
+/**
+ * @brief initialize dbus uart device
+ * @param
+ * @retval
+ */
+void dbus_uart_init(void)
+{
+ /* open uart idle it */
+ __HAL_UART_CLEAR_IDLEFLAG(&DBUS_HUART);
+ __HAL_UART_ENABLE_IT(&DBUS_HUART, UART_IT_IDLE);
+
+ uart_receive_dma_no_it(&DBUS_HUART, dbus_buf, DBUS_MAX_LEN);
+}
+
+
+
diff --git a/Driver/bsp_uart.h b/Driver/bsp_uart.h
new file mode 100644
index 0000000000000000000000000000000000000000..0064e20de05b413846229e0c2b9db05277cb6a43
--- /dev/null
+++ b/Driver/bsp_uart.h
@@ -0,0 +1,41 @@
+/**
+ ***************************************(C) COPYRIGHT 2018 DJI***************************************
+ * @file bsp_uart.h
+ * @brief this file contains the common defines and functions prototypes for
+ * the bsp_uart.c driver
+ * @note
+ * @Version V1.0.0
+ * @Date Jan-30-2018
+ ***************************************(C) COPYRIGHT 2018 DJI***************************************
+ */
+
+#ifndef __BSP_UART_H__
+#define __BSP_UART_H__
+
+#include "usart.h"
+
+#define UART_RX_DMA_SIZE (1024)
+#define DBUS_MAX_LEN (50)
+#define DBUS_BUFLEN (18)
+#define DBUS_HUART huart1 /* for dji remote controler reciever */
+/**
+ * @brief remote control information
+ */
+typedef __packed struct
+{
+ /* rocker channel information */
+ int16_t ch1;
+ int16_t ch2;
+ int16_t ch3;
+ int16_t ch4;
+
+ /* left and right lever information */
+ uint8_t sw1;
+ uint8_t sw2;
+} rc_info_t;
+
+void uart_receive_handler(UART_HandleTypeDef *huart);
+void dbus_uart_init(void);
+void USART_SendData(USART_TypeDef* USARTx, uint16_t Data);
+#endif
+
diff --git a/README.md b/README.md
index 43f36de6ed0a0c1459aeebe041717a9281b50bfa..1fce13fac4ab71844dcf00f3539fcf1b1ad3b76b 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,7 @@
# 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