diff --git a/Algorithm/pid.c b/Algorithm/pid.c index fd82c7a816215e3d8053244dfc8fa9b4a7294099..7e31acf467bb10167c9372e7a30ae093b8c3d005 100644 --- a/Algorithm/pid.c +++ b/Algorithm/pid.c @@ -30,13 +30,15 @@ void pid_init(pid_struct_t *pid, float ki, float kd, float i_max, - float out_max) + float out_max, + int mode) { pid->kp = kp; pid->ki = ki; pid->kd = kd; pid->i_max = i_max; pid->out_max = out_max; + pid->mode = mode; } /** @@ -52,9 +54,12 @@ float pid_calc(pid_struct_t *pid, float ref, float fdb) 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;} - + if (pid->mode == 0) + { + 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]; diff --git a/Algorithm/pid.h b/Algorithm/pid.h index 69f6f47fe7fb8c3919d3e5cbe04127db84a6a511..ed449ac13f8635b2faa6e5c9f3c6be96f91e142f 100644 --- a/Algorithm/pid.h +++ b/Algorithm/pid.h @@ -22,6 +22,7 @@ typedef struct _pid_struct_t { + int mode; // mode = 0 : position, 1 : speed float kp; float ki; float kd; @@ -43,7 +44,8 @@ void pid_init(pid_struct_t *pid, float ki, float kd, float i_max, - float out_max); + float out_max, + int mode); float pid_calc(pid_struct_t *pid, float ref, float fdb); 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 index 1a000a3b2012f4a85c5e9815b07a4eae276d0ede..b18aa65b5578db64c7874cb8013e7d0441a202b8 100644 --- a/Device/list_motor.c +++ b/Device/list_motor.c @@ -3,18 +3,23 @@ #include "list_motor.h" motor_t gm6020; -const pid_struct_t M3508_pid_speed = { - 2, 0.005, 2, 16000, 16000 +motor_t m3508_frontleft; +motor_t m3508_frontright; +motor_t m3508_backleft; +motor_t m3508_backright; + +const pid_struct_t M3508_pid_speed = { //A VERIFIER + 1, 2, 0.005, 2, 16000, 16000 }; -const pid_struct_t M3508_pid_pos = { - 0.1, 0.005, 0, 16000, 16000 +const pid_struct_t M3508_pid_pos = { //A VERIFIER + 0, 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_speed = { //A VERIFIER + 1, 40, 10, 0, 30000, 30000 }; -const pid_struct_t GM6020_pid_pos = { - 30, 4, 15, 10000, 10000 +const pid_struct_t GM6020_pid_pos = { //A VERIFIER + 0, 12, 3, 5, 10000, 10000 }; void motors_init(void){ @@ -24,5 +29,33 @@ void motors_init(void){ gm6020.can_info.rx_id = 0x205; gm6020.pid_pos = GM6020_pid_pos; gm6020.pid_speed = GM6020_pid_speed; + + m3508_frontleft.type = M3508; + m3508_frontleft.can_info.can_id = 0; + m3508_frontleft.can_info.tx_id = 0; + m3508_frontleft.can_info.rx_id = 0x201; + m3508_frontleft.pid_pos = M3508_pid_pos; + m3508_frontleft.pid_speed = M3508_pid_speed; + + m3508_frontright.type = M3508; + m3508_frontright.can_info.can_id = 0; + m3508_frontright.can_info.tx_id = 0; + m3508_frontright.can_info.rx_id = 0x202; + m3508_frontright.pid_pos = M3508_pid_pos; + m3508_frontright.pid_speed = M3508_pid_speed; + + m3508_backleft.type = M3508; + m3508_backleft.can_info.can_id = 0; + m3508_backleft.can_info.tx_id = 0; + m3508_backleft.can_info.rx_id = 0x203; + m3508_backleft.pid_pos = M3508_pid_pos; + m3508_backleft.pid_speed = M3508_pid_speed; + + m3508_backright.type = M3508; + m3508_backright.can_info.can_id = 0; + m3508_backright.can_info.tx_id = 0; + m3508_backright.can_info.rx_id = 0x204; + m3508_backright.pid_pos = M3508_pid_pos; + m3508_backright.pid_speed = M3508_pid_speed; } diff --git a/Device/list_motor.h b/Device/list_motor.h index e0ea4d0b6a0c27ce464c840372ff066f9b448314..20905453c9c5605a2678b2f3e385fc121747aff9 100644 --- a/Device/list_motor.h +++ b/Device/list_motor.h @@ -9,7 +9,9 @@ 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; diff --git a/Device/motor.c b/Device/motor.c index 522851f1a2af350cf28f7f2e39a1e42c1be467fa..eae071c85d8d8dd949f9b6ff578aedcab94cd505 100644 --- a/Device/motor.c +++ b/Device/motor.c @@ -1,7 +1,6 @@ #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; @@ -23,42 +22,37 @@ static int16_t correct_output(motor_t *motor) { } -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_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]); } -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]); +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) { - uint8_t buf[CAN_DATA_SIZE]; - can1_read(motor->can_info.rx_id, buf); + 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, buf); + get_6020_data(motor, motor_buf); return 1; case M3508: - get_3508_data(motor, buf); + get_3508_data(motor, motor_buf); return 1; default: return 0; diff --git a/Driver/bsp_can.h b/Driver/bsp_can.h index 7f8b125909ff05b403ebb1c1a3f672c84ba126da..8a796c4874f5a0689faff7652e5fa8099fdef9c7 100644 --- a/Driver/bsp_can.h +++ b/Driver/bsp_can.h @@ -22,7 +22,7 @@ #include <stdio.h> #include <string.h> -#define FEEDBACK_ID_BASE 0x204 +#define FEEDBACK_ID_BASE 0x201 #define CAN_CONTROL_ID_BASE 0x200 #define CAN_CONTROL_ID_EXTEND 0x1ff #define MOTOR_MAX_NUM 7 diff --git a/Driver/bsp_pwm.c b/Driver/bsp_pwm.c index 36d4cf4e4218db957fe37a7a08e5b4bd1c2fa7d3..dee836b4614b3527e63d33cdd6726289a6860b9b 100644 --- a/Driver/bsp_pwm.c +++ b/Driver/bsp_pwm.c @@ -68,18 +68,14 @@ void PWM_SetAllDuty(TIM_HandleTypeDef *tim, float duty_ch1, float duty_ch2, floa * @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_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); - 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; - + PWM_SetAllDuty(&htim1,0,0,0,0); HAL_Delay(500); } diff --git a/Driver/bsp_uart.c b/Driver/bsp_uart.c new file mode 100644 index 0000000000000000000000000000000000000000..7d618236713847e0bac016c2c7408e65e8f44b47 --- /dev/null +++ b/Driver/bsp_uart.c @@ -0,0 +1,165 @@ +/** + ***************************************(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; + + +/** + * @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->ch4 = (buff[0] | buff[1] << 8) & 0x07FF; //0x07FF met la commande sur 11 bits (donc 2048 max) + rc->ch4 -= 1024; //on centre sur 0 + rc->ch3 = (buff[1] >> 3 | buff[2] << 5) & 0x07FF; + rc->ch3 -= 1024; + rc->ch2 = (buff[2] >> 6 | buff[3] << 2 | buff[4] << 10) & 0x07FF; + rc->ch2 -= 1024; + rc->ch1 = (buff[4] >> 1 | buff[5] << 7) & 0x07FF; + rc->ch1 -= 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 (__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 +