hoverboard-firmware-hack-se.../Src/main.c

253 lines
7.1 KiB
C
Raw Normal View History

2018-01-06 17:54:51 +00:00
/*
* This file is part of the stmbl project.
*
* Copyright (C) 2013-2018 Rene Hopf <renehopf@mac.com>
* Copyright (C) 2013-2018 Nico Stute <crinq@crinq.de>
*
* 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"
#include "config.h"
2017-12-30 01:55:59 +00:00
void SystemClock_Config(void);
int cmd1;
int cmd2;
int cmd3;
int steer;
int speed;
2018-04-07 22:03:35 +00:00
2017-12-30 01:55:59 +00:00
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;
#ifdef CONTROL_PPM
2018-02-09 08:39:47 +00:00
extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1];
#endif
2018-01-06 17:54:51 +00:00
extern volatile int pwml;
extern volatile int pwmr;
2018-04-07 22:03:35 +00:00
extern volatile int weakl;
extern volatile int weakr;
volatile int pwmrl = 0;
2017-12-30 01:55:59 +00:00
extern uint8_t buzzerFreq;
extern uint8_t buzzerPattern;
extern uint8_t enable;
extern volatile uint32_t timeout;
extern float batteryVoltage;
2018-04-07 22:03:35 +00:00
extern uint8_t nunchuck_data[6];
2017-12-30 01:55:59 +00:00
2017-12-30 03:07:09 +00:00
int milli_vel_error_sum = 0;
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-01-06 21:59:15 +00:00
UART_Init();
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);
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
#ifdef CONTROL_PPM
PPM_Init();
#endif
2018-04-07 22:03:35 +00:00
#ifdef CONTROL_NUNCHUCK
I2C_Init();
Nunchuck_Init();
#endif
enable = 1;
2018-01-06 22:33:34 +00:00
while(1) {
2018-04-07 22:03:35 +00:00
HAL_Delay(2);
#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
//uint8_t button1 = (uint8_t)nunchuck_data[5] & 1;
//uint8_t button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1;
#endif
2018-04-07 22:03:35 +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-02-09 08:39:47 +00:00
#endif
2018-04-07 22:03:35 +00:00
#ifdef CONTROL_ADC
2018-04-09 20:03:19 +00:00
cmd1 = CLAMP(adc_buffer.l_rx2 - 700, 0, 2350) / 2.35; // ADC values range 0-4095, however full range of our poti only covers 650 - 3050
uint8_t button1 = (uint8_t)(adc_buffer.l_tx2 > 2000);
timeout = 0;
#endif
2018-04-07 22:03:35 +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
setScopeChannel(0, (int)speed);
setScopeChannel(1, (int)steer);
2018-04-07 22:03:35 +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
setScopeChannel(2, (int)speedR);
2018-05-06 18:13:11 +00:00
setScopeChannel(3, (int)speedL);
2018-04-07 22:03:35 +00:00
// ####### ADDITIONAL CODE #######
#ifdef ADDITIONAL_CODE
ADDITIONAL_CODE;
#endif
// ####### 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) {
pwmr = speedR;
pwml = -speedL;
}
lastSpeedL = speedL;
lastSpeedR = speedR;
2018-01-06 17:54:51 +00:00
// ####### LOG TO CONSOLE #######
consoleScope();
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {}
2018-02-26 21:00:09 +00:00
buzzerFreq = 0;
buzzerPattern = 0;
for (int i = 0; i < 8; i++) {
buzzerFreq = i;
HAL_Delay(100);
}
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, 0);
while(1) {}
}
2018-04-09 20:03:19 +00:00
if (batteryVoltage < BAT_LOW_LVL1 && batteryVoltage > BAT_LOW_LVL2) {
buzzerFreq = 5;
buzzerPattern = 8;
2018-04-09 20:03:19 +00:00
} else if (batteryVoltage < BAT_LOW_LVL2 && batteryVoltage > BAT_LOW_DEAD) {
buzzerFreq = 5;
buzzerPattern = 1;
2018-04-09 20:03:19 +00:00
} else if (batteryVoltage < BAT_LOW_DEAD) {
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;
}
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;
/**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
/**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-01-06 22:33:34 +00:00
PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV8;
2018-01-06 22:11:22 +00:00
HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);
2017-12-30 01:55:59 +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
/**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);
}