jerome
/
hoverboard-firmware-hack
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

TranspOtterNG firmware ready

Browse Source
jblb
Niklas Fauth 7 years ago
parent a6659bbbea
commit c6e5cf5baf
7 changed files with 1452 additions and 1427 deletions
Show Stats Download Patch File Download Diff File

282
Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_i2c.c
View File

File diff suppressed because it is too large Load Diff

19
Inc/config.h
Unescape View File

@ -18,12 +18,12 @@
#define BAT_CALIB_REAL_VOLTAGE 43.0 // input voltage measured by multimeter
#define BAT_CALIB_ADC 1704 // adc-value measured by mainboard (value nr 5 on UART debug output)
#define BAT_NUMBER_OF_CELLS 10 // normal Hoverboard battery: 10s
#define BAT_NUMBER_OF_CELLS 9 // normal Hoverboard battery: 10s
#define BAT_LOW_LVL1_ENABLE 0 // to beep or not to beep, 1 or 0
#define BAT_LOW_LVL1 3.6 // gently beeps at this voltage level. [V/cell]
#define BAT_LOW_LVL1 3.2 // gently beeps at this voltage level. [V/cell]
#define BAT_LOW_LVL2_ENABLE 1 // to beep or not to beep, 1 or 0
#define BAT_LOW_LVL2 3.5 // your battery is almost empty. Charge now! [V/cell]
#define BAT_LOW_DEAD 3.37 // undervoltage poweroff. (while not driving) [V/cell]
#define BAT_LOW_LVL2 3.0 // your battery is almost empty. Charge now! [V/cell]
#define BAT_LOW_DEAD 2.6 // undervoltage poweroff. (while not driving) [V/cell]
#define DC_CUR_LIMIT 15 // DC current limit in amps per motor. so 15 means it will draw 30A out of your battery. it does not disable motors, it is a soft current limit.
@ -45,10 +45,10 @@
// ############################### SERIAL DEBUG ###############################
#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
#define DEBUG_BAUD 115200 // UART baud rate
//#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
//#define DEBUG_BAUD 115200 // UART baud rate
//#define DEBUG_SERIAL_SERVOTERM
#define DEBUG_SERIAL_ASCII // "1:345 2:1337 3:0 4:0 5:0 6:0 7:0 8:0\r\n"
//#define DEBUG_SERIAL_ASCII // "1:345 2:1337 3:0 4:0 5:0 6:0 7:0 8:0\r\n"
// ############################### INPUT ###############################
@ -72,13 +72,12 @@
// ###### TRANSPOTTER FIRMWARE #######
#define CONTROL_GAMETRAK
//#define INVERT_GAMETRAK
#define SUPPORT_LCD
#define SUPPORT_NUNCHUCK
//#define SUPPORT_REMOTE
#define VEL_P 0.9
#define ROT_P 1.2
#define ROT_P -1.2
// ###### CONTROL VIA NINTENDO NUNCHUCK ######
@ -96,7 +95,7 @@
// - weakr and weakl: field weakening for extra boost at high speed (speedR > 700 and speedL > 700). 0 to ~400
#define FILTER 0.1 // lower value == softer filter. do not use values <0.01, you will get float precision issues.
#define SPEED_COEFFICIENT 0.5 // higher value == stronger. 0.0 to ~2.0?
#define SPEED_COEFFICIENT 0.9 // higher value == stronger. 0.0 to ~2.0?
#define STEER_COEFFICIENT 0.5 // higher value == stronger. if you do not want any steering, set it to 0.0; 0.0 to 1.0
#define INVERT_R_DIRECTION
#define INVERT_L_DIRECTION

17
Src/control.c
Unescape View File

@ -84,6 +84,13 @@ void PPM_Init() {
}
#endif
uint8_t Nunchuck_Ping() {
if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 1, 10) == HAL_OK) {
return 1;
}
return 0;
}
void Nunchuck_Init() {
//-- START -- init WiiNunchuck
i2cBuffer[0] = 0xF0;
@ -101,18 +108,18 @@ void Nunchuck_Init() {
void Nunchuck_Read() {
i2cBuffer[0] = 0x00;
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 100);
HAL_Delay(5);
if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 6, 100) == HAL_OK) {
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 10);
HAL_Delay(3);
if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 6, 10) == HAL_OK) {
timeout = 0;
} else {
timeout++;
}
if (timeout > 3) {
/*if (timeout > 3) {
HAL_Delay(50);
Nunchuck_Init();
}
}*/
//setScopeChannel(0, (int)nunchuck_data[0]);
//setScopeChannel(1, (int)nunchuck_data[1]);

21
Src/hd44780.c
Unescape View File

@ -13,10 +13,11 @@ uint8_t LCDerrorFlag = 0;
void LCD_WaitForBusyFlag(LCD_PCF8574_HandleTypeDef* handle) {
uint8_t flag;
LCD_GetBusyFlag(handle, &flag);
uint32_t startTick=HAL_GetTick();
while (flag == 1 && HAL_GetTick()-startTick<handle->pcf8574.PCF_I2C_TIMEOUT) {
LCD_GetBusyFlag(handle, &flag);
}
//uint32_t startTick=HAL_GetTick();
//while (flag == 1 && HAL_GetTick()-startTick<handle->pcf8574.PCF_I2C_TIMEOUT) {
// LCD_GetBusyFlag(handle, &flag);
//}
return;
}
LCD_RESULT LCD_I2C_WriteOut(LCD_PCF8574_HandleTypeDef* handle) {
@ -61,7 +62,7 @@ LCD_RESULT LCD_Init(LCD_PCF8574_HandleTypeDef* handle) {
return LCD_ERROR;
}
HAL_Delay(50);
//HAL_Delay(50);
LCD_StateWriteBit(handle, 0, LCD_PIN_RS);
LCD_StateWriteBit(handle, 0, LCD_PIN_RW);
LCD_StateWriteBit(handle, 0, LCD_PIN_E);
@ -155,17 +156,17 @@ LCD_RESULT LCD_GetBusyFlag(LCD_PCF8574_HandleTypeDef* handle, uint8_t* flag) {
LCD_StateWriteBit(handle, 1, LCD_PIN_E);
PCF8574_Read(&handle->pcf8574, flag);
//PCF8574_Read(&handle->pcf8574, flag);
*flag &= 1 << handle->pins[LCD_PIN_D7];
*flag >>= handle->pins[LCD_PIN_D7];
//*flag &= 1 << handle->pins[LCD_PIN_D7];
//*flag >>= handle->pins[LCD_PIN_D7];
LCD_StateWriteBit(handle, 0, LCD_PIN_E);
LCD_StateWriteBit(handle, 1, LCD_PIN_E);
uint8_t flag2;
PCF8574_Read(&handle->pcf8574, &flag2);
//uint8_t flag2;
//PCF8574_Read(&handle->pcf8574, &flag2);
LCD_StateWriteBit(handle, 0, LCD_PIN_E);
LCD_StateWriteBit(handle, 0, LCD_PIN_RW);

261
Src/main.c
Unescape View File

@ -30,6 +30,14 @@
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);
@ -272,15 +280,15 @@ int main(void) {
I2C_Init();
HAL_Delay(50);
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);
}
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);
@ -298,41 +306,6 @@ int main(void) {
while(1) {
HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms
#ifdef CONTROL_NUNCHUCK
Nunchuck_Read();
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
button1 = (uint8_t)nunchuck_data[5] & 1;
button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1;
#endif
#ifdef CONTROL_PPM
cmd1 = CLAMP((ppm_captured_value[0] - 500) * 2, -1000, 1000);
cmd2 = CLAMP((ppm_captured_value[1] - 500) * 2, -1000, 1000);
button1 = ppm_captured_value[5] > 500;
float scale = ppm_captured_value[2] / 1000.0f;
#endif
#ifdef CONTROL_ADC
// 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
// use ADCs as button inputs:
button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1
button2 = (uint8_t)(adc_buffer.l_rx2 > 2000); // ADC2
timeout = 0;
#endif
#ifdef CONTROL_SERIAL_USART2
cmd1 = CLAMP((int16_t)command.steer, -1000, 1000);
cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
timeout = 0;
#endif
#ifdef CONTROL_GAMETRAK
if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
@ -360,32 +333,100 @@ int main(void) {
}
uint16_t distance = CLAMP((adc_buffer.l_rx2) - 180, 0, 4095);
float steering = (adc_buffer.l_tx2 - 2048) / 2048.0;
int feedforward = ((distance - (int)(setDistance * 1345)));
speedL = speedL * 0.8f + (CLAMP(feedforward + ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -750, 750) * -0.2f);
speedR = speedR * 0.8f + (CLAMP(feedforward - ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -750, 750) * -0.2f);
if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50)) {
if (distance - (int)(setDistance * 1345) > -300) {
pwml = speedL;
pwmr = -speedR;
if (checkRemote) {
if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) {
enable = 1;
} else {
enable = 0;
steering = (adc_buffer.l_tx2 - 2048) / 2048.0;
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 = 1;
enable = 0;
}
} else {
}
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();
@ -400,81 +441,47 @@ int main(void) {
poweroff();
}
#ifdef SUPPORT_LCD
if (counter % 100 == 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);
#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
#endif
#ifndef CONTROL_GAMETRAK
// ####### 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, -1000, 1000);
speedL = CLAMP(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT, -1000, 1000);
#ifdef SUPPORT_LCD
if (counter % 100 == 0) {
if (LCDerrorFlag == 1 && enable == 0) {
#ifdef ADDITIONAL_CODE
ADDITIONAL_CODE;
#endif
} 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);
}
}
// ####### SET OUTPUTS #######
if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < TIMEOUT) {
#ifdef INVERT_R_DIRECTION
pwmr = speedR;
#else
pwmr = -speedR;
#endif
#ifdef INVERT_L_DIRECTION
pwml = -speedL;
#else
pwml = speedL;
counter++;
#endif
}
lastSpeedL = speedL;
lastSpeedR = speedR;
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();
}
// ####### POWEROFF BY POWER-BUTTON #######
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && weakr == 0 && weakl == 0) {
enable = 0;
while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {}
poweroff();
}
#endif
// ####### BEEP AND EMERGENCY POWEROFF #######

2
Src/setup.c
Unescape View File

@ -238,7 +238,7 @@ void I2C_Init()
HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);
hi2c2.Instance = I2C2;
hi2c2.Init.ClockSpeed = 100000;
hi2c2.Init.ClockSpeed = 200000;
hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c2.Init.OwnAddress1 = 0;
hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;

2277
build/hover.hex
View File

File diff suppressed because it is too large Load Diff
Write Preview
Loading…
Cancel
Save