ADD: buzzer, undervoltage lockout, safe poweroff, foo

This commit is contained in:
Niklas Fauth 2018-02-10 01:37:36 +01:00
parent 10da0983c6
commit 6842f7b7fe
9 changed files with 1022 additions and 730 deletions

View File

@ -13,10 +13,10 @@
#define PWM_FREQ 16000 // PWM frequency in Hz #define PWM_FREQ 16000 // PWM frequency in Hz
#define DEAD_TIME 32 // PWM deadtime #define DEAD_TIME 32 // PWM deadtime
#define DC_CUR_LIMIT 5 // Motor DC current limit in amps #define DC_CUR_LIMIT 32 // Motor DC current limit in amps
#define DEBUG_SERIAL_SERVOTERM //#define DEBUG_SERIAL_SERVOTERM
//#define DEBUG_SERIAL_ASCII #define DEBUG_SERIAL_ASCII
#define DEBUG_BAUD 115200 // UART baud rate #define DEBUG_BAUD 115200 // UART baud rate
//#define DEBUG_I2C_LCD //#define DEBUG_I2C_LCD

View File

@ -114,6 +114,9 @@
#define OFF_PIN GPIO_PIN_5 #define OFF_PIN GPIO_PIN_5
#define OFF_PORT GPIOA #define OFF_PORT GPIOA
#define BUTTON_PIN GPIO_PIN_1
#define BUTTON_PORT GPIOA
#define CHARGER_PIN GPIO_PIN_12 #define CHARGER_PIN GPIO_PIN_12
#define CHARGER_PORT GPIOA #define CHARGER_PORT GPIOA

View File

@ -97,7 +97,7 @@ C_INCLUDES = \
# compile gcc flags # compile gcc flags
ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections
CFLAGS = $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections CFLAGS = $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections -std=gnu11
ifeq ($(DEBUG), 1) ifeq ($(DEBUG), 1)
CFLAGS += -g -gdwarf-2 CFLAGS += -g -gdwarf-2

View File

@ -12,6 +12,13 @@ volatile int pwmr = 0;
extern volatile adc_buf_t adc_buffer; extern volatile adc_buf_t adc_buffer;
extern volatile uint32_t timeout;
uint8_t buzzerFreq = 0;
uint8_t buzzerPattern = 0;
uint8_t enable = 0;
const int pwm_res = 64000000 / 2 / PWM_FREQ; // = 2000 const int pwm_res = 64000000 / 2 / PWM_FREQ; // = 2000
const uint8_t hall_to_pos[8] = { const uint8_t hall_to_pos[8] = {
@ -112,6 +119,7 @@ inline void blockPhaseCurrent(int pos, int u, int v, int *q) {
int last_pos = 0; int last_pos = 0;
int timer = 0; int timer = 0;
uint16_t buzzerTimer = 0;
int max_time = PWM_FREQ / 10; int max_time = PWM_FREQ / 10;
volatile int vel = 0; volatile int vel = 0;
@ -123,6 +131,8 @@ int offsetrr2 = 2000;
int offsetdcl = 2000; int offsetdcl = 2000;
int offsetdcr = 2000; int offsetdcr = 2000;
float batteryVoltage;
int curl = 0; int curl = 0;
// int errorl = 0; // int errorl = 0;
// int kp = 5; // int kp = 5;
@ -143,7 +153,9 @@ void DMA1_Channel1_IRQHandler() {
return; return;
} }
if((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP > DC_CUR_LIMIT) { batteryVoltage = batteryVoltage * 0.99 + ((float)adc_buffer.batt1 * ADC_BATTERY_VOLT) * 0.01;
if((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP > DC_CUR_LIMIT || timeout > 50 || enable == 0) {
LEFT_TIM->BDTR &= ~TIM_BDTR_MOE; LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
//HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); //HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
} else { } else {
@ -151,7 +163,7 @@ void DMA1_Channel1_IRQHandler() {
//HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); //HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0);
} }
if((adc_buffer.dcr - offsetdcr) * MOTOR_AMP_CONV_DC_AMP > DC_CUR_LIMIT) { if((adc_buffer.dcr - offsetdcr) * MOTOR_AMP_CONV_DC_AMP > DC_CUR_LIMIT || timeout > 50 || enable == 0) {
RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
} else { } else {
RIGHT_TIM->BDTR |= TIM_BDTR_MOE; RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
@ -182,9 +194,19 @@ void DMA1_Channel1_IRQHandler() {
setScopeChannel(2, (adc_buffer.rl1 - offsetrl1) / 8); setScopeChannel(2, (adc_buffer.rl1 - offsetrl1) / 8);
setScopeChannel(3, (adc_buffer.rl2 - offsetrl2) / 8); setScopeChannel(3, (adc_buffer.rl2 - offsetrl2) / 8);
consoleScope();
timer++; timer++;
buzzerTimer++;
if (buzzerFreq != 0 && (buzzerTimer / 1500) % (buzzerPattern + 1) == 0) {
if (buzzerTimer % buzzerFreq == 0) {
HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN);
}
} else {
HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, 0);
}
// if(timer > max_time){ // if(timer > max_time){
// timer = max_time; // timer = max_time;

View File

@ -7,7 +7,7 @@
UART_HandleTypeDef huart2; UART_HandleTypeDef huart2;
volatile uint8_t uart_buf[300]; 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];
@ -37,8 +37,8 @@ void consoleScope() {
#endif #endif
#ifdef DEBUG_SERIAL_ASCII #ifdef DEBUG_SERIAL_ASCII
memset(&uart_buf, 0, sizeof(uart_buf)); memset(uart_buf, 0, sizeof(uart_buf));
sprintf(uart_buf, "%i;%i;%i;%i;%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(DMA1_Channel2->CNDTR == 0) {
DMA1_Channel2->CCR &= ~DMA_CCR_EN; DMA1_Channel2->CCR &= ~DMA_CCR_EN;

View File

@ -7,16 +7,13 @@
TIM_HandleTypeDef TimHandle; 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 = 0; uint32_t timeout = 100;
void PPM_ISR_Callback() { void PPM_ISR_Callback() {
// Dummy loop with 16 bit count wrap around // Dummy loop with 16 bit count wrap around
uint16_t rc_delay = TIM2->CNT; uint16_t rc_delay = TIM2->CNT;
TIM2->CNT = 0; TIM2->CNT = 0;
HAL_TIM_Base_Stop(&TimHandle);
__HAL_RCC_TIM2_CLK_DISABLE();
if (rc_delay > 3000) { if (rc_delay > 3000) {
ppm_count = 0; ppm_count = 0;
} }
@ -25,8 +22,7 @@ void PPM_ISR_Callback() {
ppm_captured_value[ppm_count] = CLAMP(rc_delay, 1000, 2000) - 1000; ppm_captured_value[ppm_count] = CLAMP(rc_delay, 1000, 2000) - 1000;
ppm_count++; ppm_count++;
} }
__HAL_RCC_TIM2_CLK_ENABLE();
HAL_TIM_Base_Start(&TimHandle);
} }
void PPM_Init() { void PPM_Init() {
@ -49,4 +45,5 @@ void PPM_Init() {
/* EXTI interrupt init*/ /* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0); HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI3_IRQn); HAL_NVIC_EnableIRQ(EXTI3_IRQn);
HAL_TIM_Base_Start(&TimHandle);
} }

View File

@ -35,6 +35,15 @@ 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 uint8_t buzzerFreq;
extern uint8_t buzzerPattern;
extern uint8_t enable;
extern volatile uint32_t timeout;
extern float batteryVoltage;
int milli_vel_error_sum = 0; int milli_vel_error_sum = 0;
@ -67,20 +76,30 @@ int main(void) {
MX_ADC2_Init(); MX_ADC2_Init();
UART_Init(); UART_Init();
#ifdef CONTROL_PPM
PPM_Init();
#endif
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, 1); HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, 1);
HAL_ADC_Start(&hadc1); HAL_ADC_Start(&hadc1);
HAL_ADC_Start(&hadc2); HAL_ADC_Start(&hadc2);
for (int i = 8; i >= 0; i--) {
buzzerFreq = i;
HAL_Delay(100);
}
buzzerFreq = 0;
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
int lastSpeedL = 0, lastSpeedR = 0; int lastSpeedL = 0, lastSpeedR = 0;
int speedL = 0, speedR = 0; int speedL = 0, speedR = 0;
#ifdef CONTROL_PPM
PPM_Init();
#endif
enable = 1;
while(1) { while(1) {
HAL_Delay(0); HAL_Delay(10);
// int milli_cur = 3000; // int milli_cur = 3000;
// int milli_volt = milli_cur * MILLI_R / 1000;// + vel * MILLI_PSI * 141; // int milli_volt = milli_cur * MILLI_R / 1000;// + vel * MILLI_PSI * 141;
// // pwm = milli_volt * pwm_res / MILLI_V; // // pwm = milli_volt * pwm_res / MILLI_V;
@ -93,19 +112,55 @@ int main(void) {
// cmdl = 70; // cmdl = 70;
#ifdef CONTROL_PPM #ifdef CONTROL_PPM
speedR = -(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)); 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));
#endif #endif
if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50)) { if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < 50) {
pwmr = speedR; pwmr = speedR;
pwml = speedL; pwml = speedL;
} }
lastSpeedL = speedL; lastSpeedL = speedL;
lastSpeedR = speedR; lastSpeedR = speedR;
setScopeChannel(0, speedR); setScopeChannel(0, speedR);
setScopeChannel(1, speedL); setScopeChannel(1, speedL);
consoleScope();
timeout++;
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
for (int i = 0; i < 8; i++) {
buzzerFreq = i;
HAL_Delay(100);
}
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, 0);
while(1) {}
}
if (batteryVoltage < 36.0 && batteryVoltage > 33.0) {
buzzerFreq = 5;
buzzerPattern = 8;
} else if (batteryVoltage < 33.0 && batteryVoltage > 30.0) {
buzzerFreq = 5;
buzzerPattern = 1;
} else if (batteryVoltage < 30.0) {
buzzerPattern = 0;
enable = 0;
for (int i = 0; i < 8; i++) {
buzzerFreq = i;
HAL_Delay(100);
}
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, 0);
while(1) {}
} else {
buzzerFreq = 0;
buzzerPattern = 0;
}
// if(vel > milli_vel_cmd){ // if(vel > milli_vel_cmd){
// HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
// } // }

View File

@ -140,6 +140,9 @@ void MX_GPIO_Init(void) {
GPIO_InitStruct.Pin = CHARGER_PIN; GPIO_InitStruct.Pin = CHARGER_PIN;
HAL_GPIO_Init(CHARGER_PORT, &GPIO_InitStruct); HAL_GPIO_Init(CHARGER_PORT, &GPIO_InitStruct);
GPIO_InitStruct.Pin = BUTTON_PIN;
HAL_GPIO_Init(BUTTON_PORT, &GPIO_InitStruct);
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;

File diff suppressed because it is too large Load Diff