Commit 6d918f1b authored by Sarra Ayed's avatar Sarra Ayed
Browse files

Ajout du dossier controle telecommande de Baptiste

parent 8fe107a8
[PreviousGenFiles]
HeaderPath=D:/Documents/ST/Polystar/Contrle tlcommande/Inc
HeaderFiles=gpio.h;stm32f4xx_it.h;stm32f4xx_hal_conf.h;main.h;dma.h;usart.h;adc.h;spi.h;can.h;tim.h;
SourcePath=D:/Documents/ST/Polystar/Contrle tlcommande/Src
SourceFiles=gpio.c;stm32f4xx_it.c;stm32f4xx_hal_msp.c;main.c;dma.c;usart.c;adc.c;spi.c;can.c;tim.c;
[PreviousLibFiles]
LibFiles=Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ramfunc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal.h;Drivers/STM32F4xx_HAL_Driver/Inc/Legacy/stm32_hal_legacy.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_def.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_can.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_spi.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ramfunc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal.h;Drivers/STM32F4xx_HAL_Driver/Inc/Legacy/stm32_hal_legacy.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_def.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_can.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_spi.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f427xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Drivers/CMSIS/Include/cmsis_armcc.h;Drivers/CMSIS/Include/cmsis_armclang.h;Drivers/CMSIS/Include/cmsis_compiler.h;Drivers/CMSIS/Include/cmsis_gcc.h;Drivers/CMSIS/Include/cmsis_iccarm.h;Drivers/CMSIS/Include/cmsis_version.h;Drivers/CMSIS/Include/core_armv8mbl.h;Drivers/CMSIS/Include/core_armv8mml.h;Drivers/CMSIS/Include/core_cm0.h;Drivers/CMSIS/Include/core_cm0plus.h;Drivers/CMSIS/Include/core_cm1.h;Drivers/CMSIS/Include/core_cm23.h;Drivers/CMSIS/Include/core_cm3.h;Drivers/CMSIS/Include/core_cm33.h;Drivers/CMSIS/Include/core_cm4.h;Drivers/CMSIS/Include/core_cm7.h;Drivers/CMSIS/Include/core_sc000.h;Drivers/CMSIS/Include/core_sc300.h;Drivers/CMSIS/Include/mpu_armv7.h;Drivers/CMSIS/Include/mpu_armv8.h;Drivers/CMSIS/Include/tz_context.h;
[PreviousUsedKeilFiles]
SourceFiles=..\Src\main.c;..\Src\gpio.c;..\Src\adc.c;..\Src\can.c;..\Src\dma.c;..\Src\spi.c;..\Src\tim.c;..\Src\usart.c;..\Src\stm32f4xx_it.c;..\Src\stm32f4xx_hal_msp.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;..\\Src/system_stm32f4xx.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;..\Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;..\\Src/system_stm32f4xx.c;..\Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;;
HeaderPath=..\Drivers\STM32F4xx_HAL_Driver\Inc;..\Drivers\STM32F4xx_HAL_Driver\Inc\Legacy;..\Drivers\CMSIS\Device\ST\STM32F4xx\Include;..\Drivers\CMSIS\Include;..\Inc;
CDefines=USE_HAL_DRIVER;STM32F427xx;USE_HAL_DRIVER;USE_HAL_DRIVER;
/****************************************************************************
* 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;
}
/****************************************************************************
* 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
/**
******************************************************************************
* @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;
}
}
/**
******************************************************************************
* @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
#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;
}
#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);
#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 dclar 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");*/
}
}
#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
#include "oled.h"
#include "bsp_oled.h"
oled_button_val_t oled_button_val(void){
int val = oled_button_get_volt();
if (abs(BUTTON_DOWN - val) <= BUTTON_RANGE)
return B_DOWN;
if (abs(BUTTON_LEFT - val) <= BUTTON_RANGE)
return B_LEFT;
if (abs(BUTTON_RIGHT - val) <= BUTTON_RANGE)
return B_RIGHT;
if (abs(BUTTON_UP - val) <= BUTTON_RANGE)
return B_UP;
if (abs(BUTTON_PRESSED - val) <= BUTTON_RANGE)
return B_PRESSED;
if (abs(BUTTON_STATIC - val) <= BUTTON_RANGE)
return B_STATIC;
else
return B_ERROR;
}
void oled_print_button(uint8_t row, uint8_t col){
oled_clear(Pen_Clear);
oled_button_val_t pos= oled_button_val();
switch(pos){
case(B_DOWN):
oled_printf(row, col, "DOWN");
break;
case(B_LEFT):
oled_printf(row, col, "LEFT");
break;
case(B_RIGHT):
oled_printf(row, col, "RIGHT");
break;
case(B_UP):
oled_printf(row, col, "UP");
break;
case(B_PRESSED):
oled_printf(row, col, "PRESSED");
break;
case(B_STATIC):
oled_printf(row, col, "STATIC");
break;
case(B_ERROR):
oled_printf(row, col, "ERROR");
break;
}
oled_refresh_gram();