ADD: buzzer, undervoltage lockout, safe poweroff, foo
This commit is contained in:
parent
10da0983c6
commit
6842f7b7fe
|
@ -13,10 +13,10 @@
|
|||
#define PWM_FREQ 16000 // PWM frequency in Hz
|
||||
#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_ASCII
|
||||
//#define DEBUG_SERIAL_SERVOTERM
|
||||
#define DEBUG_SERIAL_ASCII
|
||||
#define DEBUG_BAUD 115200 // UART baud rate
|
||||
//#define DEBUG_I2C_LCD
|
||||
|
||||
|
|
|
@ -114,6 +114,9 @@
|
|||
#define OFF_PIN GPIO_PIN_5
|
||||
#define OFF_PORT GPIOA
|
||||
|
||||
#define BUTTON_PIN GPIO_PIN_1
|
||||
#define BUTTON_PORT GPIOA
|
||||
|
||||
#define CHARGER_PIN GPIO_PIN_12
|
||||
#define CHARGER_PORT GPIOA
|
||||
|
||||
|
|
2
Makefile
2
Makefile
|
@ -97,7 +97,7 @@ C_INCLUDES = \
|
|||
# compile gcc flags
|
||||
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)
|
||||
CFLAGS += -g -gdwarf-2
|
||||
|
|
28
Src/bldc.c
28
Src/bldc.c
|
@ -12,6 +12,13 @@ volatile int pwmr = 0;
|
|||
|
||||
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 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 timer = 0;
|
||||
uint16_t buzzerTimer = 0;
|
||||
int max_time = PWM_FREQ / 10;
|
||||
volatile int vel = 0;
|
||||
|
||||
|
@ -123,6 +131,8 @@ int offsetrr2 = 2000;
|
|||
int offsetdcl = 2000;
|
||||
int offsetdcr = 2000;
|
||||
|
||||
float batteryVoltage;
|
||||
|
||||
int curl = 0;
|
||||
// int errorl = 0;
|
||||
// int kp = 5;
|
||||
|
@ -143,7 +153,9 @@ void DMA1_Channel1_IRQHandler() {
|
|||
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;
|
||||
//HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
|
||||
} else {
|
||||
|
@ -151,7 +163,7 @@ void DMA1_Channel1_IRQHandler() {
|
|||
//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;
|
||||
} else {
|
||||
RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
|
||||
|
@ -182,9 +194,19 @@ void DMA1_Channel1_IRQHandler() {
|
|||
|
||||
setScopeChannel(2, (adc_buffer.rl1 - offsetrl1) / 8);
|
||||
setScopeChannel(3, (adc_buffer.rl2 - offsetrl2) / 8);
|
||||
consoleScope();
|
||||
|
||||
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){
|
||||
// timer = max_time;
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
UART_HandleTypeDef huart2;
|
||||
|
||||
volatile uint8_t uart_buf[300];
|
||||
volatile uint8_t uart_buf[100];
|
||||
volatile int16_t ch_buf[8];
|
||||
//volatile char char_buf[300];
|
||||
|
||||
|
@ -37,8 +37,8 @@ void consoleScope() {
|
|||
#endif
|
||||
|
||||
#ifdef DEBUG_SERIAL_ASCII
|
||||
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]);
|
||||
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]);
|
||||
|
||||
if(DMA1_Channel2->CNDTR == 0) {
|
||||
DMA1_Channel2->CCR &= ~DMA_CCR_EN;
|
||||
|
|
|
@ -7,16 +7,13 @@
|
|||
TIM_HandleTypeDef TimHandle;
|
||||
uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1] = {0};
|
||||
uint8_t ppm_count = 0;
|
||||
uint32_t timeout = 0;
|
||||
uint32_t timeout = 100;
|
||||
|
||||
void PPM_ISR_Callback() {
|
||||
// Dummy loop with 16 bit count wrap around
|
||||
uint16_t rc_delay = TIM2->CNT;
|
||||
TIM2->CNT = 0;
|
||||
|
||||
HAL_TIM_Base_Stop(&TimHandle);
|
||||
__HAL_RCC_TIM2_CLK_DISABLE();
|
||||
|
||||
if (rc_delay > 3000) {
|
||||
ppm_count = 0;
|
||||
}
|
||||
|
@ -25,8 +22,7 @@ void PPM_ISR_Callback() {
|
|||
ppm_captured_value[ppm_count] = CLAMP(rc_delay, 1000, 2000) - 1000;
|
||||
ppm_count++;
|
||||
}
|
||||
__HAL_RCC_TIM2_CLK_ENABLE();
|
||||
HAL_TIM_Base_Start(&TimHandle);
|
||||
|
||||
}
|
||||
|
||||
void PPM_Init() {
|
||||
|
@ -49,4 +45,5 @@ void PPM_Init() {
|
|||
/* EXTI interrupt init*/
|
||||
HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0);
|
||||
HAL_NVIC_EnableIRQ(EXTI3_IRQn);
|
||||
HAL_TIM_Base_Start(&TimHandle);
|
||||
}
|
||||
|
|
69
Src/main.c
69
Src/main.c
|
@ -35,6 +35,15 @@ extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1];
|
|||
extern volatile int pwml;
|
||||
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;
|
||||
|
||||
|
@ -67,20 +76,30 @@ int main(void) {
|
|||
MX_ADC2_Init();
|
||||
UART_Init();
|
||||
|
||||
#ifdef CONTROL_PPM
|
||||
PPM_Init();
|
||||
#endif
|
||||
|
||||
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, 1);
|
||||
|
||||
HAL_ADC_Start(&hadc1);
|
||||
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 speedL = 0, speedR = 0;
|
||||
|
||||
#ifdef CONTROL_PPM
|
||||
PPM_Init();
|
||||
#endif
|
||||
|
||||
enable = 1;
|
||||
|
||||
while(1) {
|
||||
HAL_Delay(0);
|
||||
HAL_Delay(10);
|
||||
// int milli_cur = 3000;
|
||||
// int milli_volt = milli_cur * MILLI_R / 1000;// + vel * MILLI_PSI * 141;
|
||||
// // pwm = milli_volt * pwm_res / MILLI_V;
|
||||
|
@ -93,19 +112,55 @@ int main(void) {
|
|||
// cmdl = 70;
|
||||
|
||||
#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));
|
||||
speedR = (CLAMP((((ppm_captured_value[1]-500)-(ppm_captured_value[0]-500)/2.0)*(ppm_captured_value[2]/500.0)), -800, 800));
|
||||
#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;
|
||||
pwml = speedL;
|
||||
}
|
||||
|
||||
lastSpeedL = speedL;
|
||||
lastSpeedR = speedR;
|
||||
setScopeChannel(0, speedR);
|
||||
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){
|
||||
// HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
|
||||
// }
|
||||
|
|
|
@ -140,6 +140,9 @@ void MX_GPIO_Init(void) {
|
|||
GPIO_InitStruct.Pin = CHARGER_PIN;
|
||||
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;
|
||||
|
||||
|
|
1626
build/hover.hex
1626
build/hover.hex
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue