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/list_motor.c b/Device/list_motor.c
new file mode 100644
index 0000000000000000000000000000000000000000..1a000a3b2012f4a85c5e9815b07a4eae276d0ede
--- /dev/null
+++ b/Device/list_motor.c
@@ -0,0 +1,28 @@
+#include "motor.h"
+#include "oled.h"
+#include "list_motor.h"
+
+motor_t gm6020;
+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;
+}
+
diff --git a/Device/list_motor.h b/Device/list_motor.h
new file mode 100644
index 0000000000000000000000000000000000000000..e0ea4d0b6a0c27ce464c840372ff066f9b448314
--- /dev/null
+++ b/Device/list_motor.h
@@ -0,0 +1,19 @@
+#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 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..522851f1a2af350cf28f7f2e39a1e42c1be467fa
--- /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 était déclaré 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 buf[CAN_DATA_SIZE]){
+ motor->info.GM6020_info.angle = (int16_t)(buf[0] << 8 | buf[1]);
+ motor->info.GM6020_info.speed = (int16_t)(buf[2] << 8 | buf[3]);
+ motor->info.GM6020_info.current = (int16_t)(buf[4] << 8 | buf[5]);
+ motor->info.GM6020_info.temp = (int8_t)(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 buf[CAN_DATA_SIZE]){
+ motor->info.M3508_info.angle = (int16_t)(buf[0] << 8 | buf[1]);
+ motor->info.M3508_info.speed = (int16_t)(buf[2] << 8 | buf[3]);
+ motor->info.M3508_info.current = (int16_t)(buf[4] << 8 | buf[5]);
+ motor->info.M3508_info.temp = (int8_t)(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) {
+ uint8_t buf[CAN_DATA_SIZE];
+ can1_read(motor->can_info.rx_id, buf);
+
+
+ switch (motor->type) {
+ case GM6020:
+ get_6020_data(motor, buf);
+ return 1;
+ case M3508:
+ get_3508_data(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_pwm.c b/Driver/bsp_pwm.c
index f1b8811504419addcc63721740ecc215648de187..36d4cf4e4218db957fe37a7a08e5b4bd1c2fa7d3 100644
--- a/Driver/bsp_pwm.c
+++ b/Driver/bsp_pwm.c
@@ -1,28 +1,85 @@
-#include "tim.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/>.
+ ***************************************************************************/
+
#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);
+/**
+ * @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étails)
+ tim->Instance->CCR1 = (PWM_RESOLUTION*0.2) - 1;
+ tim->Instance->CCR2 = (PWM_RESOLUTION*0.2) - 1;
+ tim->Instance->CCR3 = (PWM_RESOLUTION*0.2) - 1;
+ tim->Instance->CCR4 = (PWM_RESOLUTION*0.2) - 1;
+
+ HAL_Delay(3500);
+
+ tim->Instance->CCR1 = (PWM_RESOLUTION*0.1) - 1;
+ tim->Instance->CCR2 = (PWM_RESOLUTION*0.1) - 1;
+ tim->Instance->CCR3 = (PWM_RESOLUTION*0.1) - 1;
+ tim->Instance->CCR4 = (PWM_RESOLUTION*0.1) - 1;
+
+ HAL_Delay(500);
+
+}
diff --git a/Driver/bsp_pwm.h b/Driver/bsp_pwm.h
index 003055602ac6c9fe83b916bc3ffadb7b68a1a476..0beb2398c189d8430ccd5b9efd375b2d775cd144 100644
--- a/Driver/bsp_pwm.h
+++ b/Driver/bsp_pwm.h
@@ -1,8 +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 "main.h"
+#include "tim.h"
+void pwm_init(void);
void PWM_SetDuty(TIM_HandleTypeDef *tim,uint32_t tim_channel,float duty);
-void PWM_init(void);
+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