jerome
/
hoverboard-firmware-hack
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'afffb6209c'
${ noResults }
hoverboard-firmware-hack/Src/main.c

587 lines
16 KiB
Raw Blame History

/*
* This file is part of the hoverboard-firmware-hack project.
*
* 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>
*
* 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/>.
*/
#include "stm32f1xx_hal.h"
#include "defines.h"
#include "setup.h"
#include "config.h"
#ifdef CONTROL_GAMETRAK
#include "hd44780.h"
#include "eeprom.h"
LCD_PCF8574_HandleTypeDef lcd;
extern I2C_HandleTypeDef hi2c2;
extern uint8_t LCDerrorFlag;
uint8_t nunchuck_connected = 0;
float steering;
int feedforward;
void longBeep(void);
void shortBeep(void);
/* Virtual address defined by the user: 0xFFFF value is prohibited */
uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1337};
uint16_t VarDataTab[NB_OF_VAR] = {0};
uint16_t VarValue = 0;
uint16_t saveValue = 0;
extern volatile float currentR;
extern volatile float currentL;
uint16_t counter = 0;
#endif
void SystemClock_Config(void);
extern TIM_HandleTypeDef htim_left;
extern TIM_HandleTypeDef htim_right;
extern ADC_HandleTypeDef hadc1;
extern ADC_HandleTypeDef hadc2;
extern volatile adc_buf_t adc_buffer;
//LCD_PCF8574_HandleTypeDef lcd;
extern I2C_HandleTypeDef hi2c2;
extern UART_HandleTypeDef huart2;
int cmd1; // normalized input values. -1000 to 1000
int cmd2;
int cmd3;
typedef struct{
int16_t steer;
int16_t speed;
//uint32_t crc;
} Serialcommand;
volatile Serialcommand command;
uint8_t button1, button2;
int steer; // global variable for steering. -1000 to 1000
int speed; // global variable for speed. -1000 to 1000
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
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...
extern uint8_t enable; // global variable for motor enable
extern volatile uint32_t timeout; // global variable for timeout
extern float batteryVoltage; // global variable for battery voltage
uint32_t inactivity_timeout_counter;
extern uint8_t nunchuck_data[6];
#ifdef CONTROL_PPM
extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1];
#endif
int milli_vel_error_sum = 0;
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) {}
}
}
int main(void) {
HAL_Init();
__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);
SystemClock_Config();
__HAL_RCC_DMA1_CLK_DISABLE();
MX_GPIO_Init();
MX_TIM_Init();
MX_ADC1_Init();
MX_ADC2_Init();
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
UART_Init();
#endif
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, 1);
HAL_ADC_Start(&hadc1);
HAL_ADC_Start(&hadc2);
#ifndef CONTROL_GAMETRAK
for (int i = 8; i >= 0; i--) {
buzzerFreq = i;
HAL_Delay(100);
}
buzzerFreq = 0;
#endif
#ifdef CONTROL_GAMETRAK
for (int i = 4; i >= 0; i--) {
buzzerFreq = i*2;
HAL_Delay(100);
}
for (int i = 4; i >= 0; i--) {
buzzerFreq = i*2-1;
HAL_Delay(100);
}
buzzerFreq = 0;
int lastDistance = 0;
enable = 1;
uint8_t checkRemote = 0;
HAL_FLASH_Unlock();
/* EEPROM Init */
EE_Init();
EE_ReadVariable(VirtAddVarTab[0], &saveValue);
HAL_FLASH_Lock();
float setDistance = saveValue / 1000.0;
if (setDistance < 0.2) {
setDistance = 1.0;
}
#endif
#ifdef SUPPORT_LCD
I2C_Init();
//Led_init();
lcd.pcf8574.PCF_I2C_ADDRESS = 0x27;
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);
}
HAL_Delay(50);
LCD_ClearDisplay(&lcd);
HAL_Delay(50);
LCD_SetLocation(&lcd, 0, 0);
LCD_WriteString(&lcd, "TranspOtter V2.1");
LCD_SetLocation(&lcd, 0, 1);
LCD_WriteString(&lcd, "Initializing...");
int buttonTimeout = 0;
#ifdef SUPPORT_REMOTE
checkRemote = 1;
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
buttonTimeout++;
HAL_Delay(100);
if(buttonTimeout > 20) {
LCD_ClearDisplay(&lcd);
HAL_Delay(5);
LCD_SetLocation(&lcd, 0, 0);
LCD_WriteString(&lcd, "Starting without");
LCD_SetLocation(&lcd, 0, 1);
LCD_WriteString(&lcd, "remote E-off!");
checkRemote = 0;
HAL_Delay(2000);
}
}
#endif
LCD_ClearDisplay(&lcd);
HAL_Delay(5);
LCD_SetLocation(&lcd, 0, 1);
LCD_WriteString(&lcd, "Bat:");
LCD_SetLocation(&lcd, 8, 1);
LCD_WriteString(&lcd, "V");
LCD_SetLocation(&lcd, 15, 1);
LCD_WriteString(&lcd, "A");
LCD_SetLocation(&lcd, 0, 0);
LCD_WriteString(&lcd, "Len:");
LCD_SetLocation(&lcd, 8, 0);
LCD_WriteString(&lcd, "m(");
LCD_SetLocation(&lcd, 14, 0);
LCD_WriteString(&lcd, "m)");
#endif
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
int lastSpeedL = 0, lastSpeedR = 0;
int speedL = 0, speedR = 0;
float direction = 1;
#ifdef CONTROL_PPM
PPM_Init();
#endif
#ifdef CONTROL_NUNCHUCK
I2C_Init();
Nunchuck_Init();
#endif
#ifdef CONTROL_SERIAL_USART2
UART_Control_Init();
HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, 4);
#endif
#ifdef DEBUG_I2C_LCD
I2C_Init();
HAL_Delay(50);
lcd.pcf8574.PCF_I2C_ADDRESS = 0x27;
lcd.pcf8574.PCF_I2C_TIMEOUT = 1;
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);
}
LCD_ClearDisplay(&lcd);
HAL_Delay(5);
LCD_SetLocation(&lcd, 0, 0);
LCD_WriteString(&lcd, "Hover V2.0");
LCD_SetLocation(&lcd, 0, 1);
LCD_WriteString(&lcd, "Initializing...");
#endif
float board_temp_adc_filtered = (float)adc_buffer.temp;
float board_temp_deg_c;
enable = 1; // enable motors
while(1) {
HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms
#ifdef CONTROL_GAMETRAK
if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
HAL_Delay(10);
}
shortBeep();
HAL_Delay(300);
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
HAL_Delay(10);
}
longBeep();
HAL_Delay(350);
//Power_Set(0);
poweroff();
} else {
setDistance += 0.25;
if (setDistance > 2.6) {
setDistance = 0.5;
}
saveValue = setDistance * 1000;
saveConfig();
}
}
#ifdef GAMETRAK_CONNECTION_NORMAL
uint16_t distance = CLAMP((adc_buffer.l_rx2) - 180, 0, 4095);
steering = (adc_buffer.l_tx2 - 2048) / 2048.0;
#endif
#ifdef GAMETRAK_CONNECTION_ALTERNATE
uint16_t distance = CLAMP((adc_buffer.l_tx2) - 180, 0, 4095);
steering = (adc_buffer.l_rx2 - 2048) / 2048.0;
#endif
feedforward = ((distance - (int)(setDistance * 1345)));
if (nunchuck_connected == 0) {
speedL = speedL * 0.8f + (CLAMP(feedforward + ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -850, 850) * -0.2f);
speedR = speedR * 0.8f + (CLAMP(feedforward - ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -850, 850) * -0.2f);
if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50)) {
if (distance - (int)(setDistance * 1345) > 0) {
enable = 1;
}
if (distance - (int)(setDistance * 1345) > -300) {
#ifdef INVERT_R_DIRECTION
pwmr = -speedR;
#endif
#ifndef INVERT_R_DIRECTION
pwmr = speedR;
#endif
#ifdef INVERT_L_DIRECTION
pwml = -speedL;
#endif
#ifndef INVERT_L_DIRECTION
pwml = speedL;
#endif
if (checkRemote) {
if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) {
//enable = 1;
} else {
enable = 0;
}
}
} else {
enable = 0;
}
}
lastSpeedL = speedL;
lastSpeedR = speedR;
timeout = 0;
} else {
Nunchuck_Read();
enable = 1;
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
// ####### LOW-PASS FILTER #######
steer = steer * (1.0 - FILTER) + cmd1 * FILTER;
speed = speed * (1.0 - FILTER) + cmd2 * FILTER;
// ####### MIXER #######
speedR = CLAMP(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT, -900, 900);
speedL = CLAMP(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT, -900, 900);
// ####### SET OUTPUTS #######
if ((speedL < lastSpeedL + 100 && speedL > lastSpeedL - 100) && (speedR < lastSpeedR + 100 && speedR > lastSpeedR - 100) && timeout < TIMEOUT) {
#ifdef INVERT_R_DIRECTION
pwmr = speedR;
#else
pwmr = -speedR;
#endif
#ifdef INVERT_L_DIRECTION
pwml = speedL;
#else
pwml = -speedL;
#endif
}
else if (timeout > TIMEOUT) {
pwml = 0;
pwmr = 0;
enable = 0;
LCD_SetLocation(&lcd, 0, 0);
LCD_WriteString(&lcd, "Len:");
LCD_SetLocation(&lcd, 8, 0);
LCD_WriteString(&lcd, "m(");
LCD_SetLocation(&lcd, 14, 0);
LCD_WriteString(&lcd, "m)");
HAL_Delay(1000);
nunchuck_connected = 0;
}
lastSpeedL = speedL;
lastSpeedR = speedR;
}
if ((distance / 1345.0) - setDistance > 0.5 && (lastDistance / 1345.0) - setDistance > 0.5) { // Error, robot too far away!
enable = 0;
longBeep();
#ifdef SUPPORT_LCD
LCD_ClearDisplay(&lcd);
HAL_Delay(5);
LCD_SetLocation(&lcd, 0, 0);
LCD_WriteString(&lcd, "Emergency Off!");
LCD_SetLocation(&lcd, 0, 1);
LCD_WriteString(&lcd, "Keeper to fast.");
#endif
poweroff();
}
#ifdef SUPPORT_NUNCHUCK
if (counter % 500 == 0) {
if (nunchuck_connected == 0 && enable == 0) {
if (Nunchuck_Ping()) {
HAL_Delay(500);
Nunchuck_Init();
#ifdef SUPPORT_LCD
LCD_SetLocation(&lcd, 0, 0);
LCD_WriteString(&lcd, "Nunchuck Control");
#endif
timeout = 0;
HAL_Delay(1000);
nunchuck_connected = 1;
}
}
}
#endif
#ifdef SUPPORT_LCD
if (counter % 100 == 0) {
if (LCDerrorFlag == 1 && enable == 0) {
} else {
if (nunchuck_connected == 0) {
LCD_SetLocation(&lcd, 4, 0);
LCD_WriteFloat(&lcd,distance/1345.0,2);
LCD_SetLocation(&lcd, 10, 0);
LCD_WriteFloat(&lcd,setDistance,2);
}
LCD_SetLocation(&lcd, 4, 1);
LCD_WriteFloat(&lcd,batteryVoltage, 1);
LCD_SetLocation(&lcd, 11, 1);
LCD_WriteFloat(&lcd,MAX(ABS(currentR), ABS(currentL)),2);
}
}
#endif
counter++;
#endif
// ####### 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;
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
buzzerFreq = 5;
buzzerPattern = 42;
} 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
buzzerFreq = 5;
buzzerPattern = 6;
} else if (BEEPS_BACKWARD && speed < -50) { // backward beep
buzzerFreq = 5;
buzzerPattern = 1;
} else { // do not beep
buzzerFreq = 0;
buzzerPattern = 0;
}
// ####### 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
poweroff();
}
}
}
void longBeep(){
buzzerFreq = 5;
HAL_Delay(500);
buzzerFreq = 0;
}
void shortBeep(){
buzzerFreq = 5;
HAL_Delay(100);
buzzerFreq = 0;
}
void saveConfig() {
HAL_FLASH_Unlock();
EE_WriteVariable(VirtAddVarTab[0], saveValue);
HAL_FLASH_Lock();
}
/** System Clock Configuration
*/
void SystemClock_Config(void) {
RCC_OscInitTypeDef RCC_OscInitStruct;
RCC_ClkInitTypeDef RCC_ClkInitStruct;
RCC_PeriphCLKInitTypeDef PeriphClkInit;
/**Initializes the CPU, AHB and APB busses clocks
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = 16;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;
HAL_RCC_OscConfig(&RCC_OscInitStruct);
/**Initializes the CPU, AHB and APB busses clocks
*/
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;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC;
PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV8; // 8 MHz
HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);
/**Configure the Systick interrupt time
*/
HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq() / 1000);
/**Configure the Systick
*/
HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
/* SysTick_IRQn interrupt configuration */
HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
}
Reference in new issue View Git Blame Copy Permalink