2018-01-06 17:54:51 +00:00
/*
2018-05-08 11:02:20 +00:00
* This file is part of the hoverboard - firmware - hack project .
2018-01-06 17:54:51 +00:00
*
2018-05-08 11:02:20 +00:00
* Copyright ( C ) 2017 - 2018 Rene Hopf < renehopf @ mac . com >
* Copyright ( C ) 2017 - 2018 Nico Stute < crinq @ crinq . de >
* Copyright ( C ) 2017 - 2018 Niklas Fauth < niklas . fauth @ kit . fail >
2018-01-06 17:54:51 +00:00
*
* 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/>.
*/
2017-12-30 01:55:59 +00:00
# include "stm32f1xx_hal.h"
# include "defines.h"
2018-01-06 22:11:22 +00:00
# include "setup.h"
2018-02-09 07:53:25 +00:00
# include "config.h"
2018-05-11 20:04:12 +00:00
//#include "hd44780.h"
2017-12-30 01:55:59 +00:00
void SystemClock_Config ( void ) ;
extern TIM_HandleTypeDef htim_left ;
extern TIM_HandleTypeDef htim_right ;
extern ADC_HandleTypeDef hadc1 ;
extern ADC_HandleTypeDef hadc2 ;
2018-01-06 21:59:15 +00:00
extern volatile adc_buf_t adc_buffer ;
2018-05-11 20:04:12 +00:00
//LCD_PCF8574_HandleTypeDef lcd;
extern I2C_HandleTypeDef hi2c2 ;
2018-06-11 12:29:39 +00:00
extern UART_HandleTypeDef huart2 ;
2018-01-06 17:54:51 +00:00
2018-05-27 20:16:52 +00:00
int cmd1 ; // normalized input values. -1000 to 1000
2018-05-08 10:51:01 +00:00
int cmd2 ;
int cmd3 ;
2018-04-07 22:03:35 +00:00
2018-06-11 12:29:39 +00:00
typedef struct {
2018-06-11 12:48:58 +00:00
int16_t steer ;
int16_t speed ;
2018-06-11 12:29:39 +00:00
//uint32_t crc;
} Serialcommand ;
volatile Serialcommand command ;
2018-05-11 20:04:12 +00:00
uint8_t button1 , button2 ;
2018-05-08 10:51:01 +00:00
int steer ; // global variable for steering. -1000 to 1000
int speed ; // global variable for speed. -1000 to 1000
2017-12-30 01:55:59 +00:00
2018-05-08 10:51:01 +00:00
extern volatile int pwml ; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr ; // global variable for pwm right. -1000 to 1000
extern volatile int weakl ; // global variable for field weakening left. -1000 to 1000
extern volatile int weakr ; // global variable for field weakening right. -1000 to 1000
2018-02-10 00:37:36 +00:00
2018-05-08 10:51:01 +00:00
extern uint8_t buzzerFreq ; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerPattern ; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7...
2018-02-10 00:37:36 +00:00
2018-05-08 10:51:01 +00:00
extern uint8_t enable ; // global variable for motor enable
2018-02-10 00:37:36 +00:00
2018-05-08 10:51:01 +00:00
extern volatile uint32_t timeout ; // global variable for timeout
extern float batteryVoltage ; // global variable for battery voltage
2018-02-10 00:37:36 +00:00
2018-07-25 10:54:13 +00:00
uint32_t inactivity_timeout_counter ;
2018-05-08 10:51:01 +00:00
extern uint8_t nunchuck_data [ 6 ] ;
# ifdef CONTROL_PPM
extern volatile uint16_t ppm_captured_value [ PPM_NUM_CHANNELS + 1 ] ;
# endif
2017-12-30 01:55:59 +00:00
2017-12-30 03:07:09 +00:00
int milli_vel_error_sum = 0 ;
2018-07-29 15:30:49 +00:00
void poweroff ( ) {
if ( abs ( speed ) < 20 ) {
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 ) { }
}
}
2018-01-06 22:33:34 +00:00
int main ( void ) {
2017-12-30 01:55:59 +00:00
HAL_Init ( ) ;
2018-01-06 22:11:22 +00:00
__HAL_RCC_AFIO_CLK_ENABLE ( ) ;
HAL_NVIC_SetPriorityGrouping ( NVIC_PRIORITYGROUP_4 ) ;
/* System interrupt init*/
/* MemoryManagement_IRQn interrupt configuration */
HAL_NVIC_SetPriority ( MemoryManagement_IRQn , 0 , 0 ) ;
/* BusFault_IRQn interrupt configuration */
HAL_NVIC_SetPriority ( BusFault_IRQn , 0 , 0 ) ;
/* UsageFault_IRQn interrupt configuration */
HAL_NVIC_SetPriority ( UsageFault_IRQn , 0 , 0 ) ;
/* SVCall_IRQn interrupt configuration */
HAL_NVIC_SetPriority ( SVCall_IRQn , 0 , 0 ) ;
/* DebugMonitor_IRQn interrupt configuration */
HAL_NVIC_SetPriority ( DebugMonitor_IRQn , 0 , 0 ) ;
/* PendSV_IRQn interrupt configuration */
HAL_NVIC_SetPriority ( PendSV_IRQn , 0 , 0 ) ;
/* SysTick_IRQn interrupt configuration */
HAL_NVIC_SetPriority ( SysTick_IRQn , 0 , 0 ) ;
2017-12-30 01:55:59 +00:00
SystemClock_Config ( ) ;
__HAL_RCC_DMA1_CLK_DISABLE ( ) ;
MX_GPIO_Init ( ) ;
MX_TIM_Init ( ) ;
MX_ADC1_Init ( ) ;
MX_ADC2_Init ( ) ;
2018-05-11 20:04:12 +00:00
# if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
UART_Init ( ) ;
# endif
2017-12-30 01:55:59 +00:00
HAL_GPIO_WritePin ( OFF_PORT , OFF_PIN , 1 ) ;
2018-01-06 22:33:34 +00:00
2017-12-30 01:55:59 +00:00
HAL_ADC_Start ( & hadc1 ) ;
HAL_ADC_Start ( & hadc2 ) ;
2018-02-10 00:37:36 +00:00
for ( int i = 8 ; i > = 0 ; i - - ) {
buzzerFreq = i ;
HAL_Delay ( 100 ) ;
}
buzzerFreq = 0 ;
HAL_GPIO_WritePin ( LED_PORT , LED_PIN , 1 ) ;
2018-02-09 08:39:47 +00:00
int lastSpeedL = 0 , lastSpeedR = 0 ;
int speedL = 0 , speedR = 0 ;
2018-04-07 22:03:35 +00:00
float direction = 1 ;
2018-02-09 08:39:47 +00:00
2018-02-10 00:37:36 +00:00
# ifdef CONTROL_PPM
PPM_Init ( ) ;
# endif
2018-04-07 22:03:35 +00:00
# ifdef CONTROL_NUNCHUCK
I2C_Init ( ) ;
Nunchuck_Init ( ) ;
# endif
2018-06-11 12:29:39 +00:00
# ifdef CONTROL_SERIAL_USART2
UART_Control_Init ( ) ;
HAL_UART_Receive_DMA ( & huart2 , ( uint8_t * ) & command , 4 ) ;
# endif
2018-05-11 20:04:12 +00:00
# ifdef DEBUG_I2C_LCD
I2C_Init ( ) ;
HAL_Delay ( 50 ) ;
lcd . pcf8574 . PCF_I2C_ADDRESS = 0x27 ;
2018-05-27 20:16:52 +00:00
lcd . pcf8574 . PCF_I2C_TIMEOUT = 5 ;
lcd . pcf8574 . i2c = hi2c2 ;
lcd . NUMBER_OF_LINES = NUMBER_OF_LINES_2 ;
lcd . type = TYPE0 ;
if ( LCD_Init ( & lcd ) ! = LCD_OK ) {
// error occured
//TODO while(1);
}
2018-05-11 20:04:12 +00:00
2018-06-11 12:48:58 +00:00
LCD_ClearDisplay ( & lcd ) ;
2018-05-11 20:04:12 +00:00
HAL_Delay ( 5 ) ;
LCD_SetLocation ( & lcd , 0 , 0 ) ;
2018-06-11 12:48:58 +00:00
LCD_WriteString ( & lcd , " Hover V2.0 " ) ;
2018-05-11 20:04:12 +00:00
LCD_SetLocation ( & lcd , 0 , 1 ) ;
LCD_WriteString ( & lcd , " Initializing... " ) ;
# endif
2018-07-29 15:30:49 +00:00
float board_temp_adc_filtered = ( float ) adc_buffer . temp ;
float board_temp_deg_c ;
2018-05-27 20:16:52 +00:00
enable = 1 ; // enable motors
2018-02-10 00:37:36 +00:00
2018-01-06 22:33:34 +00:00
while ( 1 ) {
2018-07-25 10:54:13 +00:00
HAL_Delay ( DELAY_IN_MAIN_LOOP ) ; //delay in ms
2018-06-11 12:29:39 +00:00
2018-04-07 22:03:35 +00:00
# ifdef CONTROL_NUNCHUCK
Nunchuck_Read ( ) ;
2018-05-06 18:11:53 +00:00
cmd1 = CLAMP ( ( nunchuck_data [ 0 ] - 127 ) * 8 , - 1000 , 1000 ) ; // x - axis. Nunchuck joystick readings range 30 - 230
cmd2 = CLAMP ( ( nunchuck_data [ 1 ] - 128 ) * 8 , - 1000 , 1000 ) ; // y - axis
2018-04-07 22:03:35 +00:00
2018-05-11 20:04:12 +00:00
button1 = ( uint8_t ) nunchuck_data [ 5 ] & 1 ;
button2 = ( uint8_t ) ( nunchuck_data [ 5 ] > > 1 ) & 1 ;
2018-04-09 19:43:59 +00:00
# endif
2018-04-07 22:03:35 +00:00
2018-04-09 19:43:59 +00:00
# ifdef CONTROL_PPM
cmd1 = CLAMP ( ( ppm_captured_value [ 0 ] - 500 ) * 2 , - 1000 , 1000 ) ;
cmd2 = CLAMP ( ( ppm_captured_value [ 1 ] - 500 ) * 2 , - 1000 , 1000 ) ;
2018-05-11 20:04:12 +00:00
button1 = ppm_captured_value [ 5 ] > 500 ;
float scale = ppm_captured_value [ 2 ] / 1000.0f ;
2018-02-09 08:39:47 +00:00
# endif
2018-04-07 22:03:35 +00:00
# ifdef CONTROL_ADC
2018-05-27 20:16:52 +00:00
// ADC values range: 0-4095, see ADC-calibration in config.h
cmd1 = CLAMP ( adc_buffer . l_tx2 - ADC1_MIN , 0 , ADC1_MAX ) / ( ADC1_MAX / 1000.0f ) ; // ADC1
cmd2 = CLAMP ( adc_buffer . l_rx2 - ADC2_MIN , 0 , ADC2_MAX ) / ( ADC2_MAX / 1000.0f ) ; // ADC2
2018-06-11 12:29:39 +00:00
2018-05-27 20:16:52 +00:00
// use ADCs as button inputs:
button1 = ( uint8_t ) ( adc_buffer . l_tx2 > 2000 ) ; // ADC1
button2 = ( uint8_t ) ( adc_buffer . l_rx2 > 2000 ) ; // ADC2
2018-02-10 00:37:36 +00:00
2018-04-09 19:43:59 +00:00
timeout = 0 ;
# endif
2018-04-07 22:03:35 +00:00
2018-06-11 12:29:39 +00:00
# ifdef CONTROL_SERIAL_USART2
2018-06-11 12:48:58 +00:00
cmd1 = CLAMP ( ( int16_t ) command . steer , - 1000 , 1000 ) ;
cmd2 = CLAMP ( ( int16_t ) command . speed , - 1000 , 1000 ) ;
timeout = 0 ;
2018-06-11 12:29:39 +00:00
# endif
2018-05-08 11:06:10 +00:00
2018-04-09 19:43:59 +00:00
// ####### LOW-PASS FILTER #######
2018-05-06 18:11:53 +00:00
steer = steer * ( 1.0 - FILTER ) + cmd1 * FILTER ;
speed = speed * ( 1.0 - FILTER ) + cmd2 * FILTER ;
2018-04-07 22:03:35 +00:00
2018-04-09 19:43:59 +00:00
// ####### MIXER #######
speedR = CLAMP ( speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT , - 1000 , 1000 ) ;
speedL = CLAMP ( speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT , - 1000 , 1000 ) ;
2018-04-07 22:03:35 +00:00
2018-05-27 20:16:52 +00:00
2018-05-11 20:04:12 +00:00
# ifdef ADDITIONAL_CODE
2018-05-27 20:16:52 +00:00
ADDITIONAL_CODE ;
2018-05-11 20:04:12 +00:00
# endif
2018-07-21 15:37:02 +00:00
2018-04-09 19:43:59 +00:00
// ####### SET OUTPUTS #######
2018-05-06 18:11:53 +00:00
if ( ( speedL < lastSpeedL + 50 & & speedL > lastSpeedL - 50 ) & & ( speedR < lastSpeedR + 50 & & speedR > lastSpeedR - 50 ) & & timeout < TIMEOUT ) {
2018-05-27 20:16:52 +00:00
# ifdef INVERT_R_DIRECTION
2018-04-09 19:43:59 +00:00
pwmr = speedR ;
2018-05-27 20:16:52 +00:00
# else
pwmr = - speedR ;
# endif
# ifdef INVERT_L_DIRECTION
2018-04-25 17:27:15 +00:00
pwml = - speedL ;
2018-05-27 20:16:52 +00:00
# else
pwml = speedL ;
# endif
2018-04-09 19:43:59 +00:00
}
lastSpeedL = speedL ;
lastSpeedR = speedR ;
2018-01-06 17:54:51 +00:00
2018-02-10 00:37:36 +00:00
2018-07-29 15:30:49 +00:00
if ( inactivity_timeout_counter % 25 = = 0 ) {
// ####### CALC BOARD TEMPERATURE #######
board_temp_adc_filtered = board_temp_adc_filtered * 0.99 + ( float ) adc_buffer . temp * 0.01 ;
board_temp_deg_c = ( ( float ) TEMP_CAL_HIGH_DEG_C - ( float ) TEMP_CAL_LOW_DEG_C ) / ( ( float ) TEMP_CAL_HIGH_ADC - ( float ) TEMP_CAL_LOW_ADC ) * ( board_temp_adc_filtered - ( float ) TEMP_CAL_LOW_ADC ) + ( float ) TEMP_CAL_LOW_DEG_C ;
// ####### DEBUG SERIAL OUT #######
# ifdef CONTROL_ADC
setScopeChannel ( 0 , ( int ) adc_buffer . l_tx2 ) ; // 1: ADC1
setScopeChannel ( 1 , ( int ) adc_buffer . l_rx2 ) ; // 2: ADC2
# endif
setScopeChannel ( 2 , ( int ) speedR ) ; // 3: output speed: 0-1000
setScopeChannel ( 3 , ( int ) speedL ) ; // 4: output speed: 0-1000
setScopeChannel ( 4 , ( int ) adc_buffer . batt1 ) ; // 5: for battery voltage calibration
setScopeChannel ( 5 , ( int ) ( batteryVoltage * 100.0f ) ) ; // 6: for verifying battery voltage calibration
setScopeChannel ( 6 , ( int ) board_temp_adc_filtered ) ; // 7: for board temperature calibration
setScopeChannel ( 7 , ( int ) board_temp_deg_c ) ; // 8: for verifying board temperature calibration
consoleScope ( ) ;
}
2018-07-21 15:37:02 +00:00
// ####### POWEROFF BY POWER-BUTTON #######
2018-07-29 15:30:49 +00:00
if ( HAL_GPIO_ReadPin ( BUTTON_PORT , BUTTON_PIN ) & & weakr = = 0 & & weakl = = 0 ) {
2018-02-10 00:37:36 +00:00
enable = 0 ;
2018-02-18 13:02:05 +00:00
while ( HAL_GPIO_ReadPin ( BUTTON_PORT , BUTTON_PIN ) ) { }
2018-07-29 15:30:49 +00:00
poweroff ( ) ;
2018-02-10 00:37:36 +00:00
}
2018-07-21 15:37:02 +00:00
2018-07-29 15:30:49 +00:00
// ####### BEEP AND EMERGENCY POWEROFF #######
if ( ( TEMP_POWEROFF_ENABLE & & board_temp_deg_c > = TEMP_POWEROFF & & abs ( speed ) < 20 ) | | ( batteryVoltage < ( ( float ) BAT_LOW_DEAD * ( float ) BAT_NUMBER_OF_CELLS ) & & abs ( speed ) < 20 ) ) { // poweroff before mainboard burns OR low bat 3
poweroff ( ) ;
} else if ( TEMP_WARNING_ENABLE & & board_temp_deg_c > = TEMP_WARNING ) { // beep if mainboard gets hot
buzzerFreq = 4 ;
2018-07-25 10:54:13 +00:00
buzzerPattern = 1 ;
} else if ( batteryVoltage < ( ( float ) BAT_LOW_LVL1 * ( float ) BAT_NUMBER_OF_CELLS ) & & batteryVoltage > ( ( float ) BAT_LOW_LVL2 * ( float ) BAT_NUMBER_OF_CELLS ) & & BAT_LOW_LVL1_ENABLE ) { // low bat 1: slow beep
2018-02-10 00:37:36 +00:00
buzzerFreq = 5 ;
2018-07-21 15:37:02 +00:00
buzzerPattern = 42 ;
2018-07-25 10:54:13 +00:00
} else if ( batteryVoltage < ( ( float ) BAT_LOW_LVL2 * ( float ) BAT_NUMBER_OF_CELLS ) & & batteryVoltage > ( ( float ) BAT_LOW_DEAD * ( float ) BAT_NUMBER_OF_CELLS ) & & BAT_LOW_LVL2_ENABLE ) { // low bat 2: fast beep
2018-02-10 00:37:36 +00:00
buzzerFreq = 5 ;
2018-07-21 15:37:02 +00:00
buzzerPattern = 6 ;
2018-07-29 15:30:49 +00:00
} else if ( BEEPS_BACKWARD & & speed < - 50 ) { // backward beep
buzzerFreq = 5 ;
buzzerPattern = 1 ;
2018-07-25 10:54:13 +00:00
} else { // do not beep
2018-02-10 00:37:36 +00:00
buzzerFreq = 0 ;
buzzerPattern = 0 ;
}
2018-07-25 10:54:13 +00:00
// ####### INACTIVITY TIMEOUT #######
if ( abs ( speedL ) > 50 | | abs ( speedR ) > 50 ) {
inactivity_timeout_counter = 0 ;
} else {
inactivity_timeout_counter + + ;
}
if ( inactivity_timeout_counter > ( INACTIVITY_TIMEOUT * 60 * 1000 ) / ( DELAY_IN_MAIN_LOOP + 1 ) ) { // rest of main loop needs maybe 1ms
2018-07-29 15:30:49 +00:00
poweroff ( ) ;
2018-07-25 10:54:13 +00:00
}
2017-12-30 01:55:59 +00:00
}
}
/** System Clock Configuration
*/
2018-01-06 22:33:34 +00:00
void SystemClock_Config ( void ) {
2017-12-30 01:55:59 +00:00
RCC_OscInitTypeDef RCC_OscInitStruct ;
RCC_ClkInitTypeDef RCC_ClkInitStruct ;
RCC_PeriphCLKInitTypeDef PeriphClkInit ;
2018-02-09 07:53:25 +00:00
/**Initializes the CPU, AHB and APB busses clocks
2017-12-30 01:55:59 +00:00
*/
2018-01-06 22:33:34 +00:00
RCC_OscInitStruct . OscillatorType = RCC_OSCILLATORTYPE_HSI ;
RCC_OscInitStruct . HSIState = RCC_HSI_ON ;
2017-12-30 01:55:59 +00:00
RCC_OscInitStruct . HSICalibrationValue = 16 ;
2018-01-06 22:33:34 +00:00
RCC_OscInitStruct . PLL . PLLState = RCC_PLL_ON ;
RCC_OscInitStruct . PLL . PLLSource = RCC_PLLSOURCE_HSI_DIV2 ;
RCC_OscInitStruct . PLL . PLLMUL = RCC_PLL_MUL16 ;
2018-01-06 22:11:22 +00:00
HAL_RCC_OscConfig ( & RCC_OscInitStruct ) ;
2017-12-30 01:55:59 +00:00
2018-02-09 07:53:25 +00:00
/**Initializes the CPU, AHB and APB busses clocks
2017-12-30 01:55:59 +00:00
*/
2018-01-06 22:33:34 +00:00
RCC_ClkInitStruct . ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2 ;
RCC_ClkInitStruct . SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK ;
RCC_ClkInitStruct . AHBCLKDivider = RCC_SYSCLK_DIV1 ;
2017-12-30 01:55:59 +00:00
RCC_ClkInitStruct . APB1CLKDivider = RCC_HCLK_DIV2 ;
RCC_ClkInitStruct . APB2CLKDivider = RCC_HCLK_DIV1 ;
2018-01-06 22:11:22 +00:00
HAL_RCC_ClockConfig ( & RCC_ClkInitStruct , FLASH_LATENCY_2 ) ;
2017-12-30 01:55:59 +00:00
PeriphClkInit . PeriphClockSelection = RCC_PERIPHCLK_ADC ;
2018-07-29 15:30:49 +00:00
PeriphClkInit . AdcClockSelection = RCC_ADCPCLK2_DIV8 ; // 8 MHz
2018-01-06 22:11:22 +00:00
HAL_RCCEx_PeriphCLKConfig ( & PeriphClkInit ) ;
2017-12-30 01:55:59 +00:00
2018-02-09 07:53:25 +00:00
/**Configure the Systick interrupt time
2017-12-30 01:55:59 +00:00
*/
2018-01-06 22:33:34 +00:00
HAL_SYSTICK_Config ( HAL_RCC_GetHCLKFreq ( ) / 1000 ) ;
2017-12-30 01:55:59 +00:00
2018-02-09 07:53:25 +00:00
/**Configure the Systick
2017-12-30 01:55:59 +00:00
*/
HAL_SYSTICK_CLKSourceConfig ( SYSTICK_CLKSOURCE_HCLK ) ;
/* SysTick_IRQn interrupt configuration */
HAL_NVIC_SetPriority ( SysTick_IRQn , 0 , 0 ) ;
}