feedc0de/bobbycar-foc-model
1
0
Fork 0
forked from EFeru/hoverboard-firmware-hack-FOC
Code Pull Requests Activity

Implemented TranspOtter modifications

Browse Source
This commit is contained in:
Julian Metzler
2019-11-26 23:58:20 +01:00
parent 9a9eed7d10
commit f98f531f3e
11 changed files with 2155 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

328
Src/main.c
View File

@@ -25,7 +25,14 @@
#include "setup.h"
#include "config.h"
#include "comms.h"
//#include "hd44780.h"
#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD)
#include "hd44780.h"
#endif
#ifdef TRANSPOTTER
#include "eeprom.h"
#endif
// Matlab includes and defines - from auto-code generation
// ###############################################################################
@@ -59,10 +66,34 @@ extern TIM_HandleTypeDef htim_right;
extern ADC_HandleTypeDef hadc1;
extern ADC_HandleTypeDef hadc2;
extern volatile adc_buf_t adc_buffer;
//LCD_PCF8574_HandleTypeDef lcd;
#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD)
LCD_PCF8574_HandleTypeDef lcd;
#endif
extern I2C_HandleTypeDef hi2c2;
extern UART_HandleTypeDef huart2;
#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD)
extern uint8_t LCDerrorFlag;
#endif
#ifdef TRANSPOTTER
uint8_t nunchuck_connected = 0;
float steering;
int feedforward;
void saveConfig(void);
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;
uint16_t counter = 0;
#endif
typedef struct{
int16_t steer;
int16_t speed;
@@ -199,6 +230,24 @@ int main(void) {
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
#ifdef TRANSPOTTER
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 CONTROL_PPM
PPM_Init();
@@ -214,7 +263,7 @@ int main(void) {
HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, 4);
#endif
#ifdef DEBUG_I2C_LCD
#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD)
I2C_Init();
HAL_Delay(50);
lcd.pcf8574.PCF_I2C_ADDRESS = 0x27;
@@ -231,11 +280,34 @@ int main(void) {
LCD_ClearDisplay(&lcd);
HAL_Delay(5);
LCD_SetLocation(&lcd, 0, 0);
LCD_WriteString(&lcd, "Hover V2.0");
#ifdef TRANSPOTTER
LCD_WriteString(&lcd, "TranspOtter V2.1");
#else
LCD_WriteString(&lcd, "Hover V2.0");
#endif
LCD_SetLocation(&lcd, 0, 1);
LCD_WriteString(&lcd, "Initializing...");
#endif
#if defined(TRANSPOTTER) && defined(SUPPORT_LCD)
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
int16_t lastSpeedL = 0, lastSpeedR = 0;
int16_t speedL = 0, speedR = 0;
@@ -248,13 +320,95 @@ int main(void) {
while(1) {
HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms
#ifdef CONTROL_NUNCHUCK
#ifdef TRANSPOTTER
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);
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;
}
#endif
#if defined(CONTROL_NUNCHUCK) || defined(SUPPORT_NUNCHUCK)
#ifdef TRANSPOTTER
if (nunchuck_connected != 0) {
#endif
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;
#ifdef TRANSPOTTER
}
#endif
#endif
#ifdef CONTROL_PPM
@@ -294,51 +448,127 @@ int main(void) {
timeout = 0;
#endif
#ifndef TRANSPOTTER
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
buzzerPattern = 0;
buzzerFreq = 6; HAL_Delay(100); // make 2 beeps indicating the motor enable
buzzerFreq = 4; HAL_Delay(200);
buzzerFreq = 0;
enable = 1; // enable motors
}
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
buzzerPattern = 0;
buzzerFreq = 6; HAL_Delay(100); // make 2 beeps indicating the motor enable
buzzerFreq = 4; HAL_Delay(200);
buzzerFreq = 0;
enable = 1; // enable motors
}
// ####### LOW-PASS FILTER #######
rateLimiter16(cmd1, RATE, &steerRateFixdt);
rateLimiter16(cmd2, RATE, &speedRateFixdt);
filtLowPass16(steerRateFixdt >> 4, FILTER, &steerFixdt);
filtLowPass16(speedRateFixdt >> 4, FILTER, &speedFixdt);
steer = steerFixdt >> 4; // convert fixed-point to integer
speed = speedFixdt >> 4; // convert fixed-point to integer
// ####### LOW-PASS FILTER #######
rateLimiter16(cmd1, RATE, &steerRateFixdt);
rateLimiter16(cmd2, RATE, &speedRateFixdt);
filtLowPass16(steerRateFixdt >> 4, FILTER, &steerFixdt);
filtLowPass16(speedRateFixdt >> 4, FILTER, &speedFixdt);
steer = steerFixdt >> 4; // convert fixed-point to integer
speed = speedFixdt >> 4; // convert fixed-point to integer
// ####### MIXER #######
// speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000);
// speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000);
mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above
// ####### MIXER #######
// speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000);
// speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000);
mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above
#ifdef ADDITIONAL_CODE
ADDITIONAL_CODE;
#endif
#ifdef ADDITIONAL_CODE
ADDITIONAL_CODE;
// ####### SET OUTPUTS (if the target change is less than +/- 50) #######
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;
#endif
}
#endif
#ifdef TRANSPOTTER
if (timeout > TIMEOUT) {
pwml = 0;
pwmr = 0;
enable = 0;
#ifdef SUPPORT_LCD
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
// ####### SET OUTPUTS (if the target change is less than +/- 50) #######
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;
#endif
}
HAL_Delay(1000);
nunchuck_connected = 0;
}
#endif
lastSpeedL = speedL;
lastSpeedR = speedR;
#ifdef TRANSPOTTER
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 too 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,batVoltage, 1);
LCD_SetLocation(&lcd, 11, 1);
//LCD_WriteFloat(&lcd,MAX(ABS(currentR), ABS(currentL)),2);
}
}
#endif
counter++;
#endif
if (inactivity_timeout_counter % 25 == 0) {
// ####### CALC BOARD TEMPERATURE #######
@@ -409,6 +639,26 @@ int main(void) {
}
}
#ifdef TRANSPOTTER
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();
}
#endif
/** System Clock Configuration
*/
void SystemClock_Config(void) {