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

Merge pull request #260 from sdegeorgio/master

Browse Source 
Reworked the Wii nunchuck driver to make it much more robust.
This commit is contained in:
Emanuel Feru
2022-03-04 08:31:47 +01:00
committed by GitHub
parent 4770b13b2c 50704b3395
commit d97ec61c03
5 changed files with 183 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
Inc/defines.h
View File

@ -229,11 +229,16 @@ typedef struct {
uint16_t l_rx2;
} adc_buf_t;
typedef enum {
NUNCHUK_CONNECTING,
NUNCHUK_DISCONNECTED,
NUNCHUK_RECONNECTING,
NUNCHUK_CONNECTED
} nunchuk_state;
// Define I2C, Nunchuk, PPM, PWM functions
void I2C_Init(void);
void Nunchuk_Init(void);
void Nunchuk_Read(void);
uint8_t Nunchuk_Ping(void);
nunchuk_state Nunchuk_Read(void);
void PPM_Init(void);
void PPM_ISR_Callback(void);
void PWM_Init(void);
@ -256,6 +261,5 @@ void PWM_ISR_CH2_Callback(void);
#define SWC_SET (0x1800) // 0001 1000 0000 0000
#define SWD_SET (0x2000) // 0010 0000 0000 0000
#endif // DEFINES_H

128
Src/control.c
View File

@ -6,6 +6,8 @@
#include "setup.h"
#include "config.h"
#define NUNCHUK_I2C_ADDRESS 0xA4
TIM_HandleTypeDef TimHandle;
TIM_HandleTypeDef TimHandle2;
uint8_t ppm_count = 0;
@ -15,6 +17,7 @@ uint8_t timeoutFlgGen = 0;
uint8_t nunchuk_data[6] = {0};
uint8_t i2cBuffer[2];
nunchuk_state nunchukState = NUNCHUK_CONNECTING;
extern I2C_HandleTypeDef hi2c2;
extern DMA_HandleTypeDef hdma_i2c2_rx;
@ -214,44 +217,127 @@ void PWM_Init(void) {
}
#endif
uint8_t Nunchuk_Ping(void) {
if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuk_data, 1, 10) == HAL_OK) {
return 1;
uint8_t Nunchuk_tx(uint8_t i2cBuffer[], uint8_t i2cBufferLength) {
if(HAL_I2C_Master_Transmit(&hi2c2,NUNCHUK_I2C_ADDRESS,(uint8_t*)i2cBuffer, i2cBufferLength, 100) == HAL_OK) {
return true;
}
return 0;
return false;
}
void Nunchuk_Init(void) {
//-- START -- init WiiNunchuk
uint8_t Nunchuk_rx(uint8_t i2cBuffer[], uint8_t i2cBufferLength) {
if(HAL_I2C_Master_Receive(&hi2c2,NUNCHUK_I2C_ADDRESS,(uint8_t*)i2cBuffer, i2cBufferLength, 100) == HAL_OK) {
return true;
}
return false;
}
uint8_t Nunchuk_Init(void) {
//-- START -- init WiiNunchuk
i2cBuffer[0] = 0xF0;
i2cBuffer[1] = 0x55;
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 2, 100);
if(Nunchuk_tx(i2cBuffer, 2) == false) {
return false;
}
HAL_Delay(10);
i2cBuffer[0] = 0xFB;
i2cBuffer[1] = 0x00;
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 2, 100);
if(Nunchuk_tx(i2cBuffer, 2) == false) {
return false;
}
HAL_Delay(10);
return true;
}
void Nunchuk_Read(void) {
i2cBuffer[0] = 0x00;
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 10);
HAL_Delay(3);
if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuk_data, 6, 10) == HAL_OK) {
uint8_t Nunchuk_Connect() {
/* Initialise / re-initialise I2C peripheral */
I2C_Init();
/* Initialise / re-initialise nunchuk */
if(Nunchuk_Init() == true) {
nunchukState = NUNCHUK_CONNECTED;
return true;
} else {
return false;
}
}
nunchuk_state Nunchuk_Read(void) {
static uint8_t delay_counter = 0;
uint16_t checksum = 0;
uint8_t success = true;
uint8_t i = 0;
switch(nunchukState) {
case NUNCHUK_DISCONNECTED:
success = false;
/* Delay a bit before reconnecting */
if(delay_counter++ > 100) {
success = Nunchuk_Connect();
delay_counter = 0;
}
break;
case NUNCHUK_CONNECTING:
case NUNCHUK_RECONNECTING:
/* Try to reconnect once, if fails again fall back to disconnected state */
success = Nunchuk_Connect();
if(!success) {
nunchukState = NUNCHUK_DISCONNECTED;
}
break;
case NUNCHUK_CONNECTED:
/* Send read address of 0x00 to the Nunchuk */
i2cBuffer[0] = 0x00;
if(!Nunchuk_tx(i2cBuffer, 1)) {
success = false;
}
HAL_Delay(3);
/* Clear the receive data buffer */
for(i = 0; i<6; i++) {
nunchuk_data[i] = 0;
}
/* Read back 6 bytes from the Nunchuk */
if(!Nunchuk_rx(nunchuk_data, 6)) {
success = false;
}
HAL_Delay(3);
/* Checksum the receive buffer to ensure it is not in an error condition, i.e. all 0x00 or 0xFF */
for(i = 0; i<6; i++) {
checksum += nunchuk_data[i];
}
if(checksum == 0 || checksum == 0x5FA) {
success = false;
}
/* Comms failure or timeout counter reached timeout limit */
if(success == false || timeoutCntGen > 3) {
/* Clear the receive data buffer */
for(i = 0; i<6; i++) {
nunchuk_data[i] = 0;
}
/* Brings motors to safe stop */
/* Expected values from nunchuk for stopped (mid) position */
nunchuk_data[0] = 127;
nunchuk_data[1] = 128;
timeoutFlgGen = 1;
nunchukState = NUNCHUK_RECONNECTING;
}
break;
}
/* Reset the timeout flag and counter if successful communication */
if(success == true) {
timeoutCntGen = 0;
timeoutFlgGen = 0;
}
#ifndef TRANSPOTTER
if (timeoutCntGen > 3) {
HAL_Delay(50);
Nunchuk_Init();
}
#endif
return nunchukState;
//setScopeChannel(0, (int)nunchuk_data[0]);
//setScopeChannel(1, (int)nunchuk_data[1]);
//setScopeChannel(2, (int)nunchuk_data[5] & 1);

22
Src/main.c
View File

@ -138,7 +138,7 @@ static uint8_t sideboard_leds_R;
#endif
#ifdef VARIANT_TRANSPOTTER
extern uint8_t nunchuk_connected;
uint8_t nunchuk_connected;
extern float setDistance;
static uint8_t checkRemote = 0;
@ -376,17 +376,15 @@ int main(void) {
#ifdef SUPPORT_NUNCHUK
if (transpotter_counter % 500 == 0) {
if (nunchuk_connected == 0 && enable == 0) {
if (Nunchuk_Ping()) {
HAL_Delay(500);
Nunchuk_Init();
#ifdef SUPPORT_LCD
LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Nunchuk Control");
#endif
timeoutCntGen = 0;
timeoutFlgGen = 0;
HAL_Delay(1000);
nunchuk_connected = 1;
}
if(Nunchuk_Read() == NUNCHUK_CONNECTED) {
#ifdef SUPPORT_LCD
LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Nunchuk Control");
#endif
nunchuk_connected = 1;
}
} else {
nunchuk_connected = 0;
}
}
}
#endif

116
Src/setup.c
View File

@ -283,17 +283,21 @@ DMA_HandleTypeDef hdma_i2c2_tx;
void I2C_Init(void)
{
/* Initialise I2C2 GPIO pins
* I2C2 GPIO Configuration
* PB10 ------> I2C2_SCL
* PB11 ------> I2C2_SDA
*/
GPIO_InitTypeDef GPIO_InitStruct;
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* Initialise I2C peripheral */
__HAL_RCC_I2C2_CLK_ENABLE();
__HAL_RCC_DMA1_CLK_ENABLE();
/* DMA1_Channel4_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 1, 4);
HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn);
/* DMA1_Channel5_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 1, 3);
HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);
hi2c2.Instance = I2C2;
hi2c2.Init.ClockSpeed = 200000;
hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2;
@ -303,65 +307,53 @@ void I2C_Init(void)
hi2c2.Init.OwnAddress2 = 0;
hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
__HAL_RCC_I2C2_FORCE_RESET();
__HAL_RCC_I2C2_RELEASE_RESET();
HAL_I2C_Init(&hi2c2);
GPIO_InitTypeDef GPIO_InitStruct;
/* Peripheral DMA init*/
/* __HAL_RCC_DMA1_CLK_ENABLE();
*/
/* DMA1_Channel4_IRQn interrupt configuration */
/* HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 1, 4);
HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn);
*/
/* DMA1_Channel5_IRQn interrupt configuration */
/* HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 1, 3);
HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);
*/
__HAL_RCC_DMA1_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/* USER CODE BEGIN I2C2_MspInit 0 */
/* hdma_i2c2_rx.Instance = DMA1_Channel5;
hdma_i2c2_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_i2c2_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_i2c2_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_i2c2_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_i2c2_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_i2c2_rx.Init.Mode = DMA_NORMAL;
hdma_i2c2_rx.Init.Priority = DMA_PRIORITY_MEDIUM;
HAL_DMA_Init(&hdma_i2c2_rx);
/* USER CODE END I2C2_MspInit 0 */
/**I2C2 GPIO Configuration
PB10 ------> I2C2_SCL
PB11 ------> I2C2_SDA
*/
GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* Peripheral clock enable */
__HAL_RCC_I2C2_CLK_ENABLE();
/* Peripheral DMA init*/
hdma_i2c2_rx.Instance = DMA1_Channel5;
hdma_i2c2_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_i2c2_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_i2c2_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_i2c2_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_i2c2_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_i2c2_rx.Init.Mode = DMA_NORMAL;
hdma_i2c2_rx.Init.Priority = DMA_PRIORITY_MEDIUM;
HAL_DMA_Init(&hdma_i2c2_rx);
__HAL_LINKDMA(&hi2c2,hdmarx,hdma_i2c2_rx);
hdma_i2c2_tx.Instance = DMA1_Channel4;
hdma_i2c2_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_i2c2_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_i2c2_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_i2c2_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_i2c2_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_i2c2_tx.Init.Mode = DMA_NORMAL;
hdma_i2c2_tx.Init.Priority = DMA_PRIORITY_MEDIUM;
HAL_DMA_Init(&hdma_i2c2_tx);
__HAL_LINKDMA(&hi2c2,hdmatx,hdma_i2c2_tx);
/* Peripheral interrupt init */
HAL_NVIC_SetPriority(I2C2_EV_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(I2C2_EV_IRQn);
HAL_NVIC_SetPriority(I2C2_ER_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(I2C2_ER_IRQn);
/* USER CODE BEGIN I2C2_MspInit 1 */
/* USER CODE END I2C2_MspInit 1 */
__HAL_LINKDMA(&hi2c2,hdmarx,hdma_i2c2_rx);
*/
/* hdma_i2c2_tx.Instance = DMA1_Channel4;
hdma_i2c2_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_i2c2_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_i2c2_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_i2c2_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_i2c2_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_i2c2_tx.Init.Mode = DMA_NORMAL;
hdma_i2c2_tx.Init.Priority = DMA_PRIORITY_MEDIUM;
HAL_DMA_Init(&hdma_i2c2_tx);
__HAL_LINKDMA(&hi2c2,hdmatx,hdma_i2c2_tx);
*/
/* Peripheral interrupt init */
/* HAL_NVIC_SetPriority(I2C2_EV_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(I2C2_EV_IRQn);
HAL_NVIC_SetPriority(I2C2_ER_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(I2C2_ER_IRQn);
*/
}
void MX_GPIO_Init(void) {

20
Src/util.c
View File

@ -111,12 +111,6 @@ uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request
LCD_PCF8574_HandleTypeDef lcd;
#endif
#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK)
uint8_t nunchuk_connected = 1;
#else
uint8_t nunchuk_connected = 0;
#endif
#ifdef VARIANT_TRANSPOTTER
float setDistance;
uint16_t VirtAddVarTab[NB_OF_VAR] = {1337}; // Virtual address defined by the user: 0xFFFF value is prohibited
@ -288,11 +282,6 @@ void Input_Init(void) {
PWM_Init();
#endif
#ifdef CONTROL_NUNCHUK
I2C_Init();
Nunchuk_Init();
#endif
#if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2)
UART2_Init();
#endif
@ -643,8 +632,8 @@ void updateCurSpdLim(void) {
void standstillHold(void) {
#if defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE)
if (!rtP_Left.b_cruiseCtrlEna) { // If Stanstill in NOT Active -> try Activation
if ((input1[inIdx].cmd > 50 && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small
|| (abs(input2[inIdx].cmd) < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small
if (((input1[inIdx].cmd > 50 || input2[inIdx].cmd < -50) && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small
|| (input2[inIdx].cmd < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small
rtP_Left.n_cruiseMotTgt = 0;
rtP_Right.n_cruiseMotTgt = 0;
rtP_Left.b_cruiseCtrlEna = 1;
@ -653,7 +642,7 @@ void standstillHold(void) {
}
}
else { // If Stanstill is Active -> try Deactivation
if (input1[inIdx].cmd < 20 && abs(input2[inIdx].cmd) > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control
if (input1[inIdx].cmd < 20 && input2[inIdx].cmd > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control
rtP_Left.b_cruiseCtrlEna = 0;
rtP_Right.b_cruiseCtrlEna = 0;
standstillAcv = 0;
@ -813,8 +802,7 @@ void readInputRaw(void) {
#endif
#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK)
if (nunchuk_connected) {
Nunchuk_Read();
if (Nunchuk_Read() == NUNCHUK_CONNECTED) {
if (inIdx == CONTROL_NUNCHUK) {
input1[inIdx].raw = (nunchuk_data[0] - 127) * 8; // X axis 0-255
input2[inIdx].raw = (nunchuk_data[1] - 128) * 8; // Y axis 0-255