ADD: Wii Nunchuck control
This commit is contained in:
parent
30f8d97694
commit
fd49c006f7
12
Inc/config.h
12
Inc/config.h
|
@ -14,12 +14,18 @@
|
||||||
#define DEAD_TIME 32 // PWM deadtime
|
#define DEAD_TIME 32 // PWM deadtime
|
||||||
|
|
||||||
//#define DC_CUR_LIMIT 34 // Motor DC current limit in amps
|
//#define DC_CUR_LIMIT 34 // Motor DC current limit in amps
|
||||||
#define DC_CUR_LIMIT 24 // Motor DC current limit in amps
|
#define DC_CUR_LIMIT 35 // Motor DC current limit in amps
|
||||||
|
|
||||||
//#define DEBUG_SERIAL_SERVOTERM
|
//#define DEBUG_SERIAL_SERVOTERM
|
||||||
// #define DEBUG_SERIAL_ASCII
|
#define DEBUG_SERIAL_ASCII
|
||||||
|
#define DEBUG_SERIAL_USART2
|
||||||
#define DEBUG_BAUD 115200 // UART baud rate
|
#define DEBUG_BAUD 115200 // UART baud rate
|
||||||
//#define DEBUG_I2C_LCD
|
//#define DEBUG_I2C_LCD
|
||||||
|
|
||||||
// #define CONTROL_PPM // use PPM CONTROL_PPM
|
//#define CONTROL_PPM // use PPM CONTROL_PPM
|
||||||
#define PPM_NUM_CHANNELS 6 // number of PPM channels to receive
|
#define PPM_NUM_CHANNELS 6 // number of PPM channels to receive
|
||||||
|
|
||||||
|
// #define CONTROL_ADC
|
||||||
|
#define CONTROL_NUNCHUCK
|
||||||
|
|
||||||
|
// #define BEEPS_BACKWARD
|
||||||
|
|
|
@ -45,7 +45,7 @@ extern "C" {
|
||||||
|
|
||||||
/* ########################## Module Selection ############################## */
|
/* ########################## Module Selection ############################## */
|
||||||
/**
|
/**
|
||||||
* @brief This is the list of modules to be used in the HAL driver
|
* @brief This is the list of modules to be used in the HAL driver
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define HAL_MODULE_ENABLED
|
#define HAL_MODULE_ENABLED
|
||||||
|
@ -60,7 +60,7 @@ extern "C" {
|
||||||
/*#define HAL_ETH_MODULE_ENABLED */
|
/*#define HAL_ETH_MODULE_ENABLED */
|
||||||
/*#define HAL_FLASH_MODULE_ENABLED */
|
/*#define HAL_FLASH_MODULE_ENABLED */
|
||||||
#define HAL_GPIO_MODULE_ENABLED
|
#define HAL_GPIO_MODULE_ENABLED
|
||||||
/*#define HAL_I2C_MODULE_ENABLED */
|
#define HAL_I2C_MODULE_ENABLED
|
||||||
/*#define HAL_I2S_MODULE_ENABLED */
|
/*#define HAL_I2S_MODULE_ENABLED */
|
||||||
/*#define HAL_IRDA_MODULE_ENABLED */
|
/*#define HAL_IRDA_MODULE_ENABLED */
|
||||||
/*#define HAL_IWDG_MODULE_ENABLED */
|
/*#define HAL_IWDG_MODULE_ENABLED */
|
||||||
|
@ -94,7 +94,7 @@ extern "C" {
|
||||||
/**
|
/**
|
||||||
* @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
|
* @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
|
||||||
* This value is used by the RCC HAL module to compute the system frequency
|
* This value is used by the RCC HAL module to compute the system frequency
|
||||||
* (when HSE is used as system clock source, directly or through the PLL).
|
* (when HSE is used as system clock source, directly or through the PLL).
|
||||||
*/
|
*/
|
||||||
#if !defined(HSE_VALUE)
|
#if !defined(HSE_VALUE)
|
||||||
#define HSE_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */
|
#define HSE_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */
|
||||||
|
@ -107,7 +107,7 @@ extern "C" {
|
||||||
/**
|
/**
|
||||||
* @brief Internal High Speed oscillator (HSI) value.
|
* @brief Internal High Speed oscillator (HSI) value.
|
||||||
* This value is used by the RCC HAL module to compute the system frequency
|
* This value is used by the RCC HAL module to compute the system frequency
|
||||||
* (when HSI is used as system clock source, directly or through the PLL).
|
* (when HSI is used as system clock source, directly or through the PLL).
|
||||||
*/
|
*/
|
||||||
#if !defined(HSI_VALUE)
|
#if !defined(HSI_VALUE)
|
||||||
#define HSI_VALUE ((uint32_t)8000000) /*!< Value of the Internal oscillator in Hz*/
|
#define HSI_VALUE ((uint32_t)8000000) /*!< Value of the Internal oscillator in Hz*/
|
||||||
|
@ -148,7 +148,7 @@ extern "C" {
|
||||||
|
|
||||||
/* ########################## Assert Selection ############################## */
|
/* ########################## Assert Selection ############################## */
|
||||||
/**
|
/**
|
||||||
* @brief Uncomment the line below to expanse the "assert_param" macro in the
|
* @brief Uncomment the line below to expanse the "assert_param" macro in the
|
||||||
* HAL drivers code
|
* HAL drivers code
|
||||||
*/
|
*/
|
||||||
/* #define USE_FULL_ASSERT 1U */
|
/* #define USE_FULL_ASSERT 1U */
|
||||||
|
@ -211,7 +211,7 @@ extern "C" {
|
||||||
|
|
||||||
/* Includes ------------------------------------------------------------------*/
|
/* Includes ------------------------------------------------------------------*/
|
||||||
/**
|
/**
|
||||||
* @brief Include module's header file
|
* @brief Include module's header file
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifdef HAL_RCC_MODULE_ENABLED
|
#ifdef HAL_RCC_MODULE_ENABLED
|
||||||
|
@ -345,7 +345,7 @@ extern "C" {
|
||||||
* @brief The assert_param macro is used for function's parameters check.
|
* @brief The assert_param macro is used for function's parameters check.
|
||||||
* @param expr: If expr is false, it calls assert_failed function
|
* @param expr: If expr is false, it calls assert_failed function
|
||||||
* which reports the name of the source file and the source
|
* which reports the name of the source file and the source
|
||||||
* line number of the call that failed.
|
* line number of the call that failed.
|
||||||
* If expr is true, it returns no value.
|
* If expr is true, it returns no value.
|
||||||
* @retval None
|
* @retval None
|
||||||
*/
|
*/
|
||||||
|
|
2
Makefile
2
Makefile
|
@ -33,6 +33,8 @@ Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_rcc_ex.c \
|
||||||
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal.c \
|
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal.c \
|
||||||
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_adc.c \
|
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_adc.c \
|
||||||
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_uart.c \
|
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_uart.c \
|
||||||
|
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_i2c.c \
|
||||||
|
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_dma.c \
|
||||||
Src/system_stm32f1xx.c \
|
Src/system_stm32f1xx.c \
|
||||||
Src/setup.c \
|
Src/setup.c \
|
||||||
Src/control.c \
|
Src/control.c \
|
||||||
|
|
64
Src/bldc.c
64
Src/bldc.c
|
@ -9,7 +9,10 @@ volatile int posl = 0;
|
||||||
volatile int posr = 0;
|
volatile int posr = 0;
|
||||||
volatile int pwml = 0;
|
volatile int pwml = 0;
|
||||||
volatile int pwmr = 0;
|
volatile int pwmr = 0;
|
||||||
int16_t pwmrl = 0;
|
volatile int weakl = 0;
|
||||||
|
volatile int weakr = 0;
|
||||||
|
|
||||||
|
extern volatile int pwmrl;
|
||||||
|
|
||||||
extern volatile adc_buf_t adc_buffer;
|
extern volatile adc_buf_t adc_buffer;
|
||||||
|
|
||||||
|
@ -128,7 +131,7 @@ int offsetrr2 = 2000;
|
||||||
int offsetdcl = 2000;
|
int offsetdcl = 2000;
|
||||||
int offsetdcr = 2000;
|
int offsetdcr = 2000;
|
||||||
|
|
||||||
float batteryVoltage;
|
float batteryVoltage = 40.0;
|
||||||
float adccmd1;
|
float adccmd1;
|
||||||
float adccmd2;
|
float adccmd2;
|
||||||
|
|
||||||
|
@ -157,32 +160,20 @@ void DMA1_Channel1_IRQHandler() {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
batteryVoltage = batteryVoltage * 0.999 + ((float)adc_buffer.batt1 * ADC_BATTERY_VOLT) * 0.001;
|
if (buzzerTimer % 100 == 0) {
|
||||||
adccmd1 = adccmd1 * 0.999 + (float)adc_buffer.l_rx2 * 0.001;
|
batteryVoltage = batteryVoltage * 0.999 + ((float)adc_buffer.batt1 * ADC_BATTERY_VOLT) * 0.001;
|
||||||
adccmd2 = adccmd2 * 0.999 + (float)adc_buffer.l_tx2 * 0.001;
|
|
||||||
|
|
||||||
//0-4096
|
|
||||||
//+-2000
|
|
||||||
|
|
||||||
pwmrl = 0;
|
|
||||||
if(adccmd1 - 700 > 0){
|
|
||||||
pwmrl -= adccmd1 - 700;
|
|
||||||
}
|
|
||||||
if(adccmd2 - 700 > 0){
|
|
||||||
pwmrl += adccmd2 - 700;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (pwmrl < -100 && enable == 1) {
|
|
||||||
buzzerFreq = 5;
|
|
||||||
buzzerPattern = 1;
|
|
||||||
} else if (enable == 1) {
|
|
||||||
buzzerFreq = 0;
|
|
||||||
buzzerPattern = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
pwmrl = powf((pwmrl/4), 3) / 255885;
|
#ifdef BEEPS_BACKWARD
|
||||||
pwml = -pwmrl;
|
if (pwmrl < -50 && enable == 1) {
|
||||||
pwmr = pwmrl;
|
buzzerFreq = 5;
|
||||||
|
buzzerPattern = 1;
|
||||||
|
} else if (enable == 1) {
|
||||||
|
buzzerFreq = 0;
|
||||||
|
buzzerPattern = 1;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@ -258,9 +249,32 @@ void DMA1_Channel1_IRQHandler() {
|
||||||
// errorl = cmdl - curl;
|
// errorl = cmdl - curl;
|
||||||
// pwml = kp * errorl;
|
// pwml = kp * errorl;
|
||||||
|
|
||||||
|
|
||||||
blockPWM(pwml, posl, &ul, &vl, &wl);
|
blockPWM(pwml, posl, &ul, &vl, &wl);
|
||||||
blockPWM(pwmr, posr, &ur, &vr, &wr);
|
blockPWM(pwmr, posr, &ur, &vr, &wr);
|
||||||
|
|
||||||
|
|
||||||
|
int weakul, weakvl, weakwl;
|
||||||
|
if (pwml > 0) {
|
||||||
|
blockPWM(weakl, (posl+5) % 6, &weakul, &weakvl, &weakwl);
|
||||||
|
} else {
|
||||||
|
blockPWM(-weakl, (posl+1) % 6, &weakul, &weakvl, &weakwl);
|
||||||
|
}
|
||||||
|
ul += weakul;
|
||||||
|
vl += weakvl;
|
||||||
|
wl += weakwl;
|
||||||
|
|
||||||
|
int weakur, weakvr, weakwr;
|
||||||
|
if (pwmr > 0) {
|
||||||
|
blockPWM(weakr, (posr+5) % 6, &weakur, &weakvr, &weakwr);
|
||||||
|
} else {
|
||||||
|
blockPWM(-weakr, (posr+1) % 6, &weakur, &weakvr, &weakwr);
|
||||||
|
}
|
||||||
|
ur += weakur;
|
||||||
|
vr += weakvr;
|
||||||
|
wr += weakwr;
|
||||||
|
// blockPWM(pwmr, posr, &ur, &vr, &wr);
|
||||||
|
|
||||||
LEFT_TIM->LEFT_TIM_U = CLAMP(ul + pwm_res / 2, 10, pwm_res-10);
|
LEFT_TIM->LEFT_TIM_U = CLAMP(ul + pwm_res / 2, 10, pwm_res-10);
|
||||||
LEFT_TIM->LEFT_TIM_V = CLAMP(vl + pwm_res / 2, 10, pwm_res-10);
|
LEFT_TIM->LEFT_TIM_V = CLAMP(vl + pwm_res / 2, 10, pwm_res-10);
|
||||||
LEFT_TIM->LEFT_TIM_W = CLAMP(wl + pwm_res / 2, 10, pwm_res-10);
|
LEFT_TIM->LEFT_TIM_W = CLAMP(wl + pwm_res / 2, 10, pwm_res-10);
|
||||||
|
|
29
Src/comms.c
29
Src/comms.c
|
@ -7,6 +7,15 @@
|
||||||
|
|
||||||
UART_HandleTypeDef huart2;
|
UART_HandleTypeDef huart2;
|
||||||
|
|
||||||
|
#ifdef DEBUG_SERIAL_USART3
|
||||||
|
#define UART_DMA_CHANNEL DMA1_Channel2
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef DEBUG_SERIAL_USART2
|
||||||
|
#define UART_DMA_CHANNEL DMA1_Channel7
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
volatile uint8_t uart_buf[100];
|
volatile uint8_t uart_buf[100];
|
||||||
volatile int16_t ch_buf[8];
|
volatile int16_t ch_buf[8];
|
||||||
//volatile char char_buf[300];
|
//volatile char char_buf[300];
|
||||||
|
@ -28,11 +37,11 @@ void consoleScope() {
|
||||||
uart_buf[8] = CLAMP(ch_buf[7]+127, 0, 255);
|
uart_buf[8] = CLAMP(ch_buf[7]+127, 0, 255);
|
||||||
uart_buf[9] = '\n';
|
uart_buf[9] = '\n';
|
||||||
|
|
||||||
if(DMA1_Channel2->CNDTR == 0) {
|
if(UART_DMA_CHANNEL->CNDTR == 0) {
|
||||||
DMA1_Channel2->CCR &= ~DMA_CCR_EN;
|
UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN;
|
||||||
DMA1_Channel2->CNDTR = 10;
|
UART_DMA_CHANNEL->CNDTR = 10;
|
||||||
DMA1_Channel2->CMAR = (uint32_t)uart_buf;
|
UART_DMA_CHANNEL->CMAR = (uint32_t)uart_buf;
|
||||||
DMA1_Channel2->CCR |= DMA_CCR_EN;
|
UART_DMA_CHANNEL->CCR |= DMA_CCR_EN;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -40,11 +49,11 @@ void consoleScope() {
|
||||||
memset(uart_buf, 0, sizeof(uart_buf));
|
memset(uart_buf, 0, sizeof(uart_buf));
|
||||||
sprintf(uart_buf, "%i;%i;%i;%i\n\r", ch_buf[0], ch_buf[1], ch_buf[2], ch_buf[3]);//, ch_buf[4], ch_buf[5], ch_buf[6], ch_buf[7]);
|
sprintf(uart_buf, "%i;%i;%i;%i\n\r", ch_buf[0], ch_buf[1], ch_buf[2], ch_buf[3]);//, ch_buf[4], ch_buf[5], ch_buf[6], ch_buf[7]);
|
||||||
|
|
||||||
if(DMA1_Channel2->CNDTR == 0) {
|
if(UART_DMA_CHANNEL->CNDTR == 0) {
|
||||||
DMA1_Channel2->CCR &= ~DMA_CCR_EN;
|
UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN;
|
||||||
DMA1_Channel2->CNDTR = strlen(uart_buf);
|
UART_DMA_CHANNEL->CNDTR = strlen(uart_buf);
|
||||||
DMA1_Channel2->CMAR = (uint32_t)uart_buf;
|
UART_DMA_CHANNEL->CMAR = (uint32_t)uart_buf;
|
||||||
DMA1_Channel2->CCR |= DMA_CCR_EN;
|
UART_DMA_CHANNEL->CCR |= DMA_CCR_EN;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
|
@ -8,6 +8,13 @@ TIM_HandleTypeDef TimHandle;
|
||||||
uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1] = {0};
|
uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1] = {0};
|
||||||
uint8_t ppm_count = 0;
|
uint8_t ppm_count = 0;
|
||||||
uint32_t timeout = 100;
|
uint32_t timeout = 100;
|
||||||
|
uint8_t nunchuck_data[6] = {0};
|
||||||
|
|
||||||
|
uint8_t ai2cBuffer[6];
|
||||||
|
|
||||||
|
extern I2C_HandleTypeDef hi2c2;
|
||||||
|
DMA_HandleTypeDef hdma_i2c2_rx;
|
||||||
|
DMA_HandleTypeDef hdma_i2c2_tx;
|
||||||
|
|
||||||
void PPM_ISR_Callback() {
|
void PPM_ISR_Callback() {
|
||||||
// Dummy loop with 16 bit count wrap around
|
// Dummy loop with 16 bit count wrap around
|
||||||
|
@ -47,3 +54,33 @@ void PPM_Init() {
|
||||||
HAL_NVIC_EnableIRQ(EXTI3_IRQn);
|
HAL_NVIC_EnableIRQ(EXTI3_IRQn);
|
||||||
HAL_TIM_Base_Start(&TimHandle);
|
HAL_TIM_Base_Start(&TimHandle);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void Nunchuck_Init() {
|
||||||
|
//-- START -- init WiiNunchuck
|
||||||
|
ai2cBuffer[0] = 0xF0;
|
||||||
|
ai2cBuffer[1] = 0x55;
|
||||||
|
//Originale
|
||||||
|
ai2cBuffer[0] = 0x40;
|
||||||
|
ai2cBuffer[1] = 0x00;
|
||||||
|
//HAL_I2C_Master_Transmit_DMA(&hi2c2, 0xA4, (uint8_t*)ai2cBuffer, 2);
|
||||||
|
//while(wii_JOYdati.I2CTxDone ==0);
|
||||||
|
//wii_JOYdati.I2CTxDone = 0;
|
||||||
|
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)ai2cBuffer, 2, 100);
|
||||||
|
HAL_Delay(10);
|
||||||
|
//wii_JOYdati.done = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Nunchuck_Read() {
|
||||||
|
ai2cBuffer[0] = 0x00;
|
||||||
|
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)ai2cBuffer, 1, 100);
|
||||||
|
HAL_Delay(2);
|
||||||
|
HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)ai2cBuffer, 6, 100);
|
||||||
|
for (int i = 0; i < 6; i++) {
|
||||||
|
nunchuck_data[i] = (ai2cBuffer[i] ^ 0x17) + 0x17;
|
||||||
|
}
|
||||||
|
//setScopeChannel(0, (int)nunchuck_data[0]);
|
||||||
|
//setScopeChannel(1, (int)nunchuck_data[1]);
|
||||||
|
//setScopeChannel(2, (int)nunchuck_data[5] & 1);
|
||||||
|
//setScopeChannel(3, ((int)nunchuck_data[5] >> 1) & 1);
|
||||||
|
}
|
||||||
|
|
91
Src/main.c
91
Src/main.c
|
@ -25,6 +25,9 @@
|
||||||
|
|
||||||
void SystemClock_Config(void);
|
void SystemClock_Config(void);
|
||||||
|
|
||||||
|
extern float adccmd1;
|
||||||
|
extern float adccmd2;
|
||||||
|
|
||||||
extern TIM_HandleTypeDef htim_left;
|
extern TIM_HandleTypeDef htim_left;
|
||||||
extern TIM_HandleTypeDef htim_right;
|
extern TIM_HandleTypeDef htim_right;
|
||||||
extern ADC_HandleTypeDef hadc1;
|
extern ADC_HandleTypeDef hadc1;
|
||||||
|
@ -34,6 +37,10 @@ extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1];
|
||||||
|
|
||||||
extern volatile int pwml;
|
extern volatile int pwml;
|
||||||
extern volatile int pwmr;
|
extern volatile int pwmr;
|
||||||
|
extern volatile int weakl;
|
||||||
|
extern volatile int weakr;
|
||||||
|
|
||||||
|
volatile int pwmrl = 0;
|
||||||
|
|
||||||
extern uint8_t buzzerFreq;
|
extern uint8_t buzzerFreq;
|
||||||
extern uint8_t buzzerPattern;
|
extern uint8_t buzzerPattern;
|
||||||
|
@ -43,6 +50,7 @@ extern uint8_t enable;
|
||||||
extern volatile uint32_t timeout;
|
extern volatile uint32_t timeout;
|
||||||
|
|
||||||
extern float batteryVoltage;
|
extern float batteryVoltage;
|
||||||
|
extern uint8_t nunchuck_data[6];
|
||||||
|
|
||||||
|
|
||||||
int milli_vel_error_sum = 0;
|
int milli_vel_error_sum = 0;
|
||||||
|
@ -91,40 +99,79 @@ int main(void) {
|
||||||
|
|
||||||
int lastSpeedL = 0, lastSpeedR = 0;
|
int lastSpeedL = 0, lastSpeedR = 0;
|
||||||
int speedL = 0, speedR = 0;
|
int speedL = 0, speedR = 0;
|
||||||
|
float direction = 1;
|
||||||
|
|
||||||
#ifdef CONTROL_PPM
|
#ifdef CONTROL_PPM
|
||||||
PPM_Init();
|
PPM_Init();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef CONTROL_NUNCHUCK
|
||||||
|
I2C_Init();
|
||||||
|
Nunchuck_Init();
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
enable = 1;
|
enable = 1;
|
||||||
|
|
||||||
while(1) {
|
while(1) {
|
||||||
HAL_Delay(10);
|
HAL_Delay(2);
|
||||||
// int milli_cur = 3000;
|
|
||||||
// int milli_volt = milli_cur * MILLI_R / 1000;// + vel * MILLI_PSI * 141;
|
#ifdef CONTROL_NUNCHUCK
|
||||||
// // pwm = milli_volt * pwm_res / MILLI_V;
|
Nunchuck_Read();
|
||||||
|
setScopeChannel(0, (int)nunchuck_data[0]);
|
||||||
|
setScopeChannel(1, (int)nunchuck_data[1]);
|
||||||
|
setScopeChannel(2, (int)nunchuck_data[5] & 1);
|
||||||
|
setScopeChannel(3, ((int)nunchuck_data[5] >> 1) & 1);
|
||||||
|
#endif
|
||||||
|
|
||||||
// int milli_vel_cmd = 200;
|
|
||||||
// int milli_vel_error = milli_vel_cmd - vel;
|
|
||||||
// milli_vel_error_sum += milli_vel_error;
|
|
||||||
// milli_vel_error_sum = CLAMP(milli_vel_error_sum, -200000, 200000);
|
|
||||||
// pwm = CLAMP(milli_vel_cmd / 5 + milli_vel_error_sum / 200, -500, 500);
|
|
||||||
// cmdl = 70;
|
|
||||||
|
|
||||||
#ifdef CONTROL_PPM
|
#ifdef CONTROL_PPM
|
||||||
speedL = -(CLAMP((((ppm_captured_value[1]-500)+(ppm_captured_value[0]-500)/2.0)*(ppm_captured_value[2]/500.0)), -800, 800));
|
speedL = -(CLAMP((((ppm_captured_value[1]-500)+(ppm_captured_value[0]-500)/2.0)*(ppm_captured_value[2]/500.0)), -800, 800));
|
||||||
speedR = (CLAMP((((ppm_captured_value[1]-500)-(ppm_captured_value[0]-500)/2.0)*(ppm_captured_value[2]/500.0)), -800, 800));
|
speedR = (CLAMP((((ppm_captured_value[1]-500)-(ppm_captured_value[0]-500)/2.0)*(ppm_captured_value[2]/500.0)), -800, 800));
|
||||||
|
if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < 50) {
|
||||||
|
//pwmr = speedR;
|
||||||
|
//pwml = speedL;
|
||||||
|
}
|
||||||
|
|
||||||
|
lastSpeedL = speedL;
|
||||||
|
lastSpeedR = speedR;
|
||||||
|
//setScopeChannel(0, (int)pwmrl);
|
||||||
|
//setScopeChannel(1, (int)speedL);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < 50) {
|
#ifdef CONTROL_ADC
|
||||||
pwmr = speedR;
|
//adccmd1 = adccmd1 * 0.9 + (float)adc_buffer.l_rx2 * 0.1; // throttle
|
||||||
pwml = speedL;
|
adccmd2 = adccmd2 * 0.9 + (float)adc_buffer.l_tx2 * 0.1; // button
|
||||||
}
|
|
||||||
|
|
||||||
lastSpeedL = speedL;
|
pwmrl = pwmrl * 0.9 + (CLAMP(adc_buffer.l_rx2 - 700, 0, 2350) / 2.35) * 0.1 * direction;
|
||||||
lastSpeedR = speedR;
|
// pwmrl has to be 0-1000 (or negative when driving backwards)
|
||||||
setScopeChannel(0, speedR);
|
|
||||||
setScopeChannel(1, speedL);
|
setScopeChannel(0, (int)adccmd1);
|
||||||
|
setScopeChannel(1, (int)adccmd2);
|
||||||
|
|
||||||
|
// adccmd2 = button, ranges 0 in idle and 4096 when pressed
|
||||||
|
if (adccmd2 > 2000 && pwmrl < 300) { // driving backwards at low speeds
|
||||||
|
direction = -0.2;
|
||||||
|
} else {
|
||||||
|
direction = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (adccmd2 > 2000 && pwmrl > 700) { // field weakening at high speeds
|
||||||
|
weakl = pwmrl - 600; // weak should never exceed 400 or 450 MAX!!
|
||||||
|
weakr = pwmrl - 600;
|
||||||
|
} else {
|
||||||
|
weakl = 0;
|
||||||
|
weakr = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (pwml < 1000) {
|
||||||
|
pwml +=1;
|
||||||
|
}
|
||||||
|
|
||||||
|
pwml = pwmrl;
|
||||||
|
pwmr = -pwmrl;
|
||||||
|
#endif
|
||||||
|
|
||||||
consoleScope();
|
consoleScope();
|
||||||
|
|
||||||
|
@ -162,14 +209,6 @@ int main(void) {
|
||||||
buzzerFreq = 0;
|
buzzerFreq = 0;
|
||||||
buzzerPattern = 0;
|
buzzerPattern = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// if(vel > milli_vel_cmd){
|
|
||||||
// HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
|
|
||||||
// }
|
|
||||||
// else{
|
|
||||||
// HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0);
|
|
||||||
// }
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
124
Src/setup.c
124
Src/setup.c
|
@ -41,8 +41,10 @@ TIM_HandleTypeDef htim_right;
|
||||||
TIM_HandleTypeDef htim_left;
|
TIM_HandleTypeDef htim_left;
|
||||||
ADC_HandleTypeDef hadc1;
|
ADC_HandleTypeDef hadc1;
|
||||||
ADC_HandleTypeDef hadc2;
|
ADC_HandleTypeDef hadc2;
|
||||||
|
I2C_HandleTypeDef hi2c2;
|
||||||
volatile adc_buf_t adc_buffer;
|
volatile adc_buf_t adc_buffer;
|
||||||
|
|
||||||
|
#ifdef DEBUG_SERIAL_USART3
|
||||||
void UART_Init() {
|
void UART_Init() {
|
||||||
__HAL_RCC_USART3_CLK_ENABLE();
|
__HAL_RCC_USART3_CLK_ENABLE();
|
||||||
__HAL_RCC_DMA1_CLK_ENABLE();
|
__HAL_RCC_DMA1_CLK_ENABLE();
|
||||||
|
@ -73,6 +75,40 @@ void UART_Init() {
|
||||||
DMA1_Channel2->CCR = DMA_CCR_MINC | DMA_CCR_DIR;
|
DMA1_Channel2->CCR = DMA_CCR_MINC | DMA_CCR_DIR;
|
||||||
DMA1->IFCR = DMA_IFCR_CTCIF2 | DMA_IFCR_CHTIF2 | DMA_IFCR_CGIF2;
|
DMA1->IFCR = DMA_IFCR_CTCIF2 | DMA_IFCR_CHTIF2 | DMA_IFCR_CGIF2;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef DEBUG_SERIAL_USART2
|
||||||
|
void UART_Init() {
|
||||||
|
__HAL_RCC_USART2_CLK_ENABLE();
|
||||||
|
__HAL_RCC_DMA1_CLK_ENABLE();
|
||||||
|
|
||||||
|
UART_HandleTypeDef huart2;
|
||||||
|
huart2.Instance = USART2;
|
||||||
|
huart2.Init.BaudRate = DEBUG_BAUD;
|
||||||
|
huart2.Init.WordLength = UART_WORDLENGTH_8B;
|
||||||
|
huart2.Init.StopBits = UART_STOPBITS_1;
|
||||||
|
huart2.Init.Parity = UART_PARITY_NONE;
|
||||||
|
huart2.Init.Mode = UART_MODE_TX;
|
||||||
|
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
|
||||||
|
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
|
||||||
|
HAL_UART_Init(&huart2);
|
||||||
|
|
||||||
|
USART2->CR3 |= USART_CR3_DMAT; // | USART_CR3_DMAR | USART_CR3_OVRDIS;
|
||||||
|
|
||||||
|
GPIO_InitTypeDef GPIO_InitStruct;
|
||||||
|
GPIO_InitStruct.Pin = GPIO_PIN_2;
|
||||||
|
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||||
|
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||||
|
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
|
||||||
|
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||||||
|
|
||||||
|
DMA1_Channel7->CCR = 0;
|
||||||
|
DMA1_Channel7->CPAR = (uint32_t) & (USART2->DR);
|
||||||
|
DMA1_Channel7->CNDTR = 0;
|
||||||
|
DMA1_Channel7->CCR = DMA_CCR_MINC | DMA_CCR_DIR;
|
||||||
|
DMA1->IFCR = DMA_IFCR_CTCIF7 | DMA_IFCR_CHTIF7 | DMA_IFCR_CGIF7;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
/*
|
/*
|
||||||
void UART_Init() {
|
void UART_Init() {
|
||||||
|
@ -107,6 +143,94 @@ void UART_Init() {
|
||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
DMA_HandleTypeDef hdma_i2c2_rx;
|
||||||
|
|
||||||
|
DMA_HandleTypeDef hdma_i2c2_tx;
|
||||||
|
|
||||||
|
|
||||||
|
void I2C_Init()
|
||||||
|
{
|
||||||
|
|
||||||
|
__HAL_RCC_I2C2_CLK_ENABLE();
|
||||||
|
__HAL_RCC_DMA1_CLK_ENABLE();
|
||||||
|
|
||||||
|
/* DMA1_Channel4_IRQn interrupt configuration */
|
||||||
|
HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 1, 4);
|
||||||
|
HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn);
|
||||||
|
/* DMA1_Channel5_IRQn interrupt configuration */
|
||||||
|
HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 1, 3);
|
||||||
|
HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);
|
||||||
|
|
||||||
|
hi2c2.Instance = I2C2;
|
||||||
|
hi2c2.Init.ClockSpeed = 100000;
|
||||||
|
hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2;
|
||||||
|
hi2c2.Init.OwnAddress1 = 0;
|
||||||
|
hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
|
||||||
|
hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
|
||||||
|
hi2c2.Init.OwnAddress2 = 0;
|
||||||
|
hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
|
||||||
|
hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
|
||||||
|
HAL_I2C_Init(&hi2c2);
|
||||||
|
|
||||||
|
GPIO_InitTypeDef GPIO_InitStruct;
|
||||||
|
|
||||||
|
__HAL_RCC_DMA1_CLK_ENABLE();
|
||||||
|
__HAL_RCC_GPIOB_CLK_ENABLE();
|
||||||
|
/* USER CODE BEGIN I2C2_MspInit 0 */
|
||||||
|
|
||||||
|
/* USER CODE END I2C2_MspInit 0 */
|
||||||
|
|
||||||
|
/**I2C2 GPIO Configuration
|
||||||
|
PB10 ------> I2C2_SCL
|
||||||
|
PB11 ------> I2C2_SDA
|
||||||
|
*/
|
||||||
|
GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11;
|
||||||
|
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
|
||||||
|
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||||
|
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
|
||||||
|
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
|
||||||
|
|
||||||
|
/* Peripheral clock enable */
|
||||||
|
__HAL_RCC_I2C2_CLK_ENABLE();
|
||||||
|
|
||||||
|
/* Peripheral DMA init*/
|
||||||
|
|
||||||
|
hdma_i2c2_rx.Instance = DMA1_Channel5;
|
||||||
|
hdma_i2c2_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
|
||||||
|
hdma_i2c2_rx.Init.PeriphInc = DMA_PINC_DISABLE;
|
||||||
|
hdma_i2c2_rx.Init.MemInc = DMA_MINC_ENABLE;
|
||||||
|
hdma_i2c2_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
|
||||||
|
hdma_i2c2_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
|
||||||
|
hdma_i2c2_rx.Init.Mode = DMA_NORMAL;
|
||||||
|
hdma_i2c2_rx.Init.Priority = DMA_PRIORITY_MEDIUM;
|
||||||
|
HAL_DMA_Init(&hdma_i2c2_rx);
|
||||||
|
|
||||||
|
__HAL_LINKDMA(&hi2c2,hdmarx,hdma_i2c2_rx);
|
||||||
|
|
||||||
|
hdma_i2c2_tx.Instance = DMA1_Channel4;
|
||||||
|
hdma_i2c2_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
|
||||||
|
hdma_i2c2_tx.Init.PeriphInc = DMA_PINC_DISABLE;
|
||||||
|
hdma_i2c2_tx.Init.MemInc = DMA_MINC_ENABLE;
|
||||||
|
hdma_i2c2_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
|
||||||
|
hdma_i2c2_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
|
||||||
|
hdma_i2c2_tx.Init.Mode = DMA_NORMAL;
|
||||||
|
hdma_i2c2_tx.Init.Priority = DMA_PRIORITY_MEDIUM;
|
||||||
|
HAL_DMA_Init(&hdma_i2c2_tx);
|
||||||
|
|
||||||
|
__HAL_LINKDMA(&hi2c2,hdmatx,hdma_i2c2_tx);
|
||||||
|
|
||||||
|
/* Peripheral interrupt init */
|
||||||
|
HAL_NVIC_SetPriority(I2C2_EV_IRQn, 0, 0);
|
||||||
|
HAL_NVIC_EnableIRQ(I2C2_EV_IRQn);
|
||||||
|
HAL_NVIC_SetPriority(I2C2_ER_IRQn, 0, 0);
|
||||||
|
HAL_NVIC_EnableIRQ(I2C2_ER_IRQn);
|
||||||
|
/* USER CODE BEGIN I2C2_MspInit 1 */
|
||||||
|
|
||||||
|
/* USER CODE END I2C2_MspInit 1 */
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
void MX_GPIO_Init(void) {
|
void MX_GPIO_Init(void) {
|
||||||
GPIO_InitTypeDef GPIO_InitStruct;
|
GPIO_InitTypeDef GPIO_InitStruct;
|
||||||
|
|
||||||
|
|
|
@ -35,6 +35,10 @@
|
||||||
#include "stm32f1xx.h"
|
#include "stm32f1xx.h"
|
||||||
#include "stm32f1xx_it.h"
|
#include "stm32f1xx_it.h"
|
||||||
|
|
||||||
|
extern DMA_HandleTypeDef hdma_i2c2_rx;
|
||||||
|
extern DMA_HandleTypeDef hdma_i2c2_tx;
|
||||||
|
extern I2C_HandleTypeDef hi2c2;
|
||||||
|
|
||||||
/* USER CODE BEGIN 0 */
|
/* USER CODE BEGIN 0 */
|
||||||
|
|
||||||
/* USER CODE END 0 */
|
/* USER CODE END 0 */
|
||||||
|
@ -165,6 +169,47 @@ void SysTick_Handler(void) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
extern I2C_HandleTypeDef hi2c2;
|
||||||
|
void I2C1_EV_IRQHandler(void)
|
||||||
|
{
|
||||||
|
HAL_I2C_EV_IRQHandler(&hi2c2);
|
||||||
|
}
|
||||||
|
|
||||||
|
void I2C1_ER_IRQHandler(void)
|
||||||
|
{
|
||||||
|
HAL_I2C_ER_IRQHandler(&hi2c2);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This function handles DMA1 channel4 global interrupt.
|
||||||
|
*/
|
||||||
|
void DMA1_Channel4_IRQHandler(void)
|
||||||
|
{
|
||||||
|
/* USER CODE BEGIN DMA1_Channel4_IRQn 0 */
|
||||||
|
|
||||||
|
/* USER CODE END DMA1_Channel4_IRQn 0 */
|
||||||
|
HAL_DMA_IRQHandler(&hdma_i2c2_tx);
|
||||||
|
/* USER CODE BEGIN DMA1_Channel4_IRQn 1 */
|
||||||
|
|
||||||
|
/* USER CODE END DMA1_Channel4_IRQn 1 */
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This function handles DMA1 channel5 global interrupt.
|
||||||
|
*/
|
||||||
|
void DMA1_Channel5_IRQHandler(void)
|
||||||
|
{
|
||||||
|
/* USER CODE BEGIN DMA1_Channel5_IRQn 0 */
|
||||||
|
|
||||||
|
/* USER CODE END DMA1_Channel5_IRQn 0 */
|
||||||
|
HAL_DMA_IRQHandler(&hdma_i2c2_rx);
|
||||||
|
/* USER CODE BEGIN DMA1_Channel5_IRQn 1 */
|
||||||
|
|
||||||
|
/* USER CODE END DMA1_Channel5_IRQn 1 */
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void EXTI3_IRQHandler(void)
|
void EXTI3_IRQHandler(void)
|
||||||
{
|
{
|
||||||
PPM_ISR_Callback();
|
PPM_ISR_Callback();
|
||||||
|
|
2442
build/hover.hex
2442
build/hover.hex
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue