mirror of
https://github.com/EFeru/hoverboard-firmware-hack-FOC.git
synced 2025-08-03 09:34:26 +02:00
Improvements and clean-up
- clean-up printfs - removed consoleLog function with respective files - removed Delay when using printf - renamed speedL, speedR to cmdL, cmdR - corrected Arduino baud rate - updated FLASH write pointer cast int16_t to uint16_t
This commit is contained in:
EmanuelFeru
106
Src/comms.c
106
Src/comms.c
@@ -1,106 +0,0 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include "stm32f1xx_hal.h"
|
||||
#include "defines.h"
|
||||
#include "setup.h"
|
||||
#include "config.h"
|
||||
#include "comms.h"
|
||||
|
||||
extern UART_HandleTypeDef huart2;
|
||||
extern UART_HandleTypeDef huart3;
|
||||
|
||||
static volatile uint8_t uart_buf[100];
|
||||
static volatile int16_t ch_buf[8];
|
||||
//volatile char char_buf[300];
|
||||
|
||||
/* retarget the C library printf function to the USART */
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
#ifdef __GNUC__
|
||||
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
|
||||
#else
|
||||
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
|
||||
#endif
|
||||
PUTCHAR_PROTOTYPE {
|
||||
#if defined(DEBUG_SERIAL_USART2)
|
||||
HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, 1000);
|
||||
#elif defined(DEBUG_SERIAL_USART3)
|
||||
HAL_UART_Transmit(&huart3, (uint8_t *)&ch, 1, 1000);
|
||||
#endif
|
||||
return ch;
|
||||
}
|
||||
|
||||
#ifdef __GNUC__
|
||||
int _write(int file, char *data, int len) {
|
||||
int i;
|
||||
for (i = 0; i < len; i++) { __io_putchar( *data++ );}
|
||||
return len;
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
void setScopeChannel(uint8_t ch, int16_t val) {
|
||||
ch_buf[ch] = val;
|
||||
}
|
||||
|
||||
void consoleScope(void) {
|
||||
#if defined DEBUG_SERIAL_SERVOTERM && (defined DEBUG_SERIAL_USART2 || defined DEBUG_SERIAL_USART3)
|
||||
uart_buf[0] = 0xff;
|
||||
uart_buf[1] = CLAMP(ch_buf[0]+127, 0, 255);
|
||||
uart_buf[2] = CLAMP(ch_buf[1]+127, 0, 255);
|
||||
uart_buf[3] = CLAMP(ch_buf[2]+127, 0, 255);
|
||||
uart_buf[4] = CLAMP(ch_buf[3]+127, 0, 255);
|
||||
uart_buf[5] = CLAMP(ch_buf[4]+127, 0, 255);
|
||||
uart_buf[6] = CLAMP(ch_buf[5]+127, 0, 255);
|
||||
uart_buf[7] = CLAMP(ch_buf[6]+127, 0, 255);
|
||||
uart_buf[8] = CLAMP(ch_buf[7]+127, 0, 255);
|
||||
uart_buf[9] = '\n';
|
||||
|
||||
#ifdef DEBUG_SERIAL_USART2
|
||||
if(__HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) {
|
||||
HAL_UART_Transmit_DMA(&huart2, (uint8_t *)uart_buf, strLength);
|
||||
}
|
||||
#endif
|
||||
#ifdef DEBUG_SERIAL_USART3
|
||||
if(__HAL_DMA_GET_COUNTER(huart3.hdmatx) == 0) {
|
||||
HAL_UART_Transmit_DMA(&huart3, (uint8_t *)uart_buf, strLength);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined DEBUG_SERIAL_ASCII && (defined DEBUG_SERIAL_USART2 || defined DEBUG_SERIAL_USART3)
|
||||
// memset((void *)(uintptr_t)uart_buf, 0, sizeof(uart_buf));
|
||||
int strLength;
|
||||
strLength = sprintf((char *)(uintptr_t)uart_buf,
|
||||
"1:%i 2:%i 3:%i 4:%i 5:%i 6:%i 7:%i 8:%i\r\n",
|
||||
ch_buf[0], ch_buf[1], ch_buf[2], ch_buf[3], ch_buf[4], ch_buf[5], ch_buf[6], ch_buf[7]);
|
||||
|
||||
#ifdef DEBUG_SERIAL_USART2
|
||||
if(__HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) {
|
||||
HAL_UART_Transmit_DMA(&huart2, (uint8_t *)uart_buf, strLength);
|
||||
}
|
||||
#endif
|
||||
#ifdef DEBUG_SERIAL_USART3
|
||||
if(__HAL_DMA_GET_COUNTER(huart3.hdmatx) == 0) {
|
||||
HAL_UART_Transmit_DMA(&huart3, (uint8_t *)uart_buf, strLength);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
|
||||
void consoleLog(char *message)
|
||||
{
|
||||
#if defined DEBUG_SERIAL_ASCII && (defined DEBUG_SERIAL_USART2 || defined DEBUG_SERIAL_USART3)
|
||||
#ifdef DEBUG_SERIAL_USART2
|
||||
if(__HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) {
|
||||
HAL_UART_Transmit_DMA(&huart2, (uint8_t *)message, strlen((char *)(uintptr_t)message));
|
||||
}
|
||||
#endif
|
||||
#ifdef DEBUG_SERIAL_USART3
|
||||
if(__HAL_DMA_GET_COUNTER(huart3.hdmatx) == 0) {
|
||||
HAL_UART_Transmit_DMA(&huart3, (uint8_t *)message, strlen((char *)(uintptr_t)message));
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
66
Src/main.c
66
Src/main.c
@@ -27,7 +27,6 @@
|
||||
#include "setup.h"
|
||||
#include "config.h"
|
||||
#include "util.h"
|
||||
#include "comms.h"
|
||||
#include "BLDC_controller.h" /* BLDC's header file */
|
||||
#include "rtwtypes.h"
|
||||
|
||||
@@ -191,8 +190,8 @@ int main(void) {
|
||||
poweronMelody();
|
||||
HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET);
|
||||
|
||||
int16_t speedL = 0, speedR = 0;
|
||||
int16_t lastSpeedL = 0, lastSpeedR = 0;
|
||||
int16_t cmdL = 0, cmdR = 0;
|
||||
int16_t lastCmdL = 0, lastCmdR = 0;
|
||||
|
||||
int32_t board_temp_adcFixdt = adc_buffer.temp << 16; // Fixed-point filter output initialized with current ADC converted to fixed-point
|
||||
int16_t board_temp_adcFilt = adc_buffer.temp;
|
||||
@@ -212,7 +211,9 @@ int main(void) {
|
||||
beepShort(4); HAL_Delay(100);
|
||||
steerFixdt = speedFixdt = 0; // reset filters
|
||||
enable = 1; // enable motors
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("-- Motors enabled --\r\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
// ####### VARIANT_HOVERCAR #######
|
||||
@@ -276,21 +277,21 @@ int main(void) {
|
||||
#endif
|
||||
|
||||
// ####### MIXER #######
|
||||
// speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA);
|
||||
// speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA);
|
||||
mixerFcn(speed << 4, steer << 4, &speedR, &speedL); // This function implements the equations above
|
||||
// cmdR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA);
|
||||
// cmdL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA);
|
||||
mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL); // This function implements the equations above
|
||||
|
||||
// ####### SET OUTPUTS (if the target change is less than +/- 100) #######
|
||||
if ((speedL > lastSpeedL-100 && speedL < lastSpeedL+100) && (speedR > lastSpeedR-100 && speedR < lastSpeedR+100)) {
|
||||
if ((cmdL > lastCmdL-100 && cmdL < lastCmdL+100) && (cmdR > lastCmdR-100 && cmdR < lastCmdR+100)) {
|
||||
#ifdef INVERT_R_DIRECTION
|
||||
pwmr = speedR;
|
||||
pwmr = cmdR;
|
||||
#else
|
||||
pwmr = -speedR;
|
||||
pwmr = -cmdR;
|
||||
#endif
|
||||
#ifdef INVERT_L_DIRECTION
|
||||
pwml = -speedL;
|
||||
pwml = -cmdL;
|
||||
#else
|
||||
pwml = speedL;
|
||||
pwml = cmdL;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
@@ -301,22 +302,22 @@ int main(void) {
|
||||
distanceErr = distance - (int)(setDistance * 1345);
|
||||
|
||||
if (nunchuk_connected == 0) {
|
||||
speedL = speedL * 0.8f + (CLAMP(distanceErr + (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
|
||||
speedR = speedR * 0.8f + (CLAMP(distanceErr - (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
|
||||
if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50)) {
|
||||
cmdL = cmdL * 0.8f + (CLAMP(distanceErr + (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
|
||||
cmdR = cmdR * 0.8f + (CLAMP(distanceErr - (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
|
||||
if ((cmdL < lastCmdL + 50 && cmdL > lastCmdL - 50) && (cmdR < lastCmdR + 50 && cmdR > lastCmdR - 50)) {
|
||||
if (distanceErr > 0) {
|
||||
enable = 1;
|
||||
}
|
||||
if (distanceErr > -300) {
|
||||
#ifdef INVERT_R_DIRECTION
|
||||
pwmr = speedR;
|
||||
pwmr = cmdR;
|
||||
#else
|
||||
pwmr = -speedR;
|
||||
pwmr = -cmdR;
|
||||
#endif
|
||||
#ifdef INVERT_L_DIRECTION
|
||||
pwml = -speedL;
|
||||
pwml = -cmdL;
|
||||
#else
|
||||
pwml = speedL;
|
||||
pwml = cmdL;
|
||||
#endif
|
||||
|
||||
if (checkRemote) {
|
||||
@@ -410,15 +411,15 @@ int main(void) {
|
||||
// ####### DEBUG SERIAL OUT #######
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms
|
||||
printf("in1:%i in2:%i spdL:%i spdR:%i adcBat:%i BatV:%i adcTemp:%i Temp:%i\r\n",
|
||||
input1, // 1: INPUT1
|
||||
input2, // 2: INPUT2
|
||||
speedL, // 3: output command: [-1000, 1000]
|
||||
speedR, // 4: output command: [-1000, 1000]
|
||||
adc_buffer.batt1, // 5: for battery voltage calibration
|
||||
printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i\r\n",
|
||||
input1, // 1: INPUT1
|
||||
input2, // 2: INPUT2
|
||||
cmdL, // 3: output command: [-1000, 1000]
|
||||
cmdR, // 4: output command: [-1000, 1000]
|
||||
adc_buffer.batt1, // 5: for battery voltage calibration
|
||||
batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC, // 6: for verifying battery voltage calibration
|
||||
board_temp_adcFilt, // 7: for board temperature calibration
|
||||
board_temp_deg_c); // 8: for verifying board temperature calibration
|
||||
board_temp_adcFilt, // 7: for board temperature calibration
|
||||
board_temp_deg_c); // 8: for verifying board temperature calibration
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -463,25 +464,18 @@ int main(void) {
|
||||
} else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // 1 beep (low pitch): Motor error, disable motors
|
||||
enable = 0;
|
||||
beepCount(1, 24, 1);
|
||||
printf("#ErrL: %i ErrR: %i\r\n", rtY_Left.z_errCode, rtY_Right.z_errCode);
|
||||
} else if (timeoutFlagADC) { // 2 beeps (low pitch): ADC timeout
|
||||
beepCount(2, 24, 1);
|
||||
printf("#ADC timeout\r\n");
|
||||
} else if (timeoutFlagSerial) { // 3 beeps (low pitch): Serial timeout
|
||||
beepCount(3, 24, 1);
|
||||
printf("#Serial timeout\r\n");
|
||||
} else if (timeoutCnt > TIMEOUT) { // 4 beeps (low pitch): General timeout (PPM, PWM, Nunchuck)
|
||||
beepCount(4, 24, 1);
|
||||
printf("#General timeout\r\n");
|
||||
} else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // 5 beeps (low pitch): Mainboard temperature warning
|
||||
beepCount(5, 24, 1);
|
||||
printf("#STM32 hot: %i\r\n", board_temp_deg_c);
|
||||
} else if (BAT_LVL1_ENABLE && batVoltage < BAT_LVL1) { // 1 beep fast (medium pitch): Low bat 1
|
||||
beepCount(0, 10, 6);
|
||||
printf("#Battery empty: %i\r\n", batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC);
|
||||
} else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) { // 1 beep slow (medium pitch): Low bat 2
|
||||
beepCount(0, 10, 30);
|
||||
printf("#Battery empty: %i\r\n", batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC);
|
||||
} else if (BEEPS_BACKWARD && ((speed < -50 && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // 1 beep fast (high pitch): Backward spinning motors
|
||||
beepCount(0, 5, 1);
|
||||
backwardDrive = 1;
|
||||
@@ -492,7 +486,7 @@ int main(void) {
|
||||
|
||||
|
||||
// ####### INACTIVITY TIMEOUT #######
|
||||
if (abs(speedL) > 50 || abs(speedR) > 50) {
|
||||
if (abs(cmdL) > 50 || abs(cmdR) > 50) {
|
||||
inactivity_timeout_counter = 0;
|
||||
} else {
|
||||
inactivity_timeout_counter++;
|
||||
@@ -503,8 +497,8 @@ int main(void) {
|
||||
|
||||
// HAL_GPIO_TogglePin(LED_PORT, LED_PIN); // This is to measure the main() loop duration with an oscilloscope connected to LED_PIN
|
||||
// Update main loop states
|
||||
lastSpeedL = speedL;
|
||||
lastSpeedR = speedR;
|
||||
lastCmdL = cmdL;
|
||||
lastCmdR = cmdR;
|
||||
main_loop_counter++;
|
||||
timeoutCnt++;
|
||||
}
|
||||
|
||||
105
Src/util.c
105
Src/util.c
@@ -133,13 +133,13 @@ static int16_t INPUT_MIN; // [-] Input target minimum limitation
|
||||
static uint8_t cur_spd_valid = 0;
|
||||
static uint8_t inp_cal_valid = 0;
|
||||
static uint16_t INPUT1_TYP_CAL = INPUT1_TYPE;
|
||||
static uint16_t INPUT1_MIN_CAL = INPUT1_MIN;
|
||||
static uint16_t INPUT1_MID_CAL = INPUT1_MID;
|
||||
static uint16_t INPUT1_MAX_CAL = INPUT1_MAX;
|
||||
static int16_t INPUT1_MIN_CAL = INPUT1_MIN;
|
||||
static int16_t INPUT1_MID_CAL = INPUT1_MID;
|
||||
static int16_t INPUT1_MAX_CAL = INPUT1_MAX;
|
||||
static uint16_t INPUT2_TYP_CAL = INPUT2_TYPE;
|
||||
static uint16_t INPUT2_MIN_CAL = INPUT2_MIN;
|
||||
static uint16_t INPUT2_MID_CAL = INPUT2_MID;
|
||||
static uint16_t INPUT2_MAX_CAL = INPUT2_MAX;
|
||||
static int16_t INPUT2_MIN_CAL = INPUT2_MIN;
|
||||
static int16_t INPUT2_MID_CAL = INPUT2_MID;
|
||||
static int16_t INPUT2_MAX_CAL = INPUT2_MAX;
|
||||
#endif
|
||||
|
||||
#if defined(CONTROL_ADC)
|
||||
@@ -198,6 +198,34 @@ static uint8_t cruiseCtrlAcv = 0;
|
||||
static uint8_t standstillAcv = 0;
|
||||
#endif
|
||||
|
||||
/* =========================== Retargeting printf =========================== */
|
||||
/* retarget the C library printf function to the USART */
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
#ifdef __GNUC__
|
||||
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
|
||||
#else
|
||||
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
|
||||
#endif
|
||||
PUTCHAR_PROTOTYPE {
|
||||
#if defined(DEBUG_SERIAL_USART2)
|
||||
HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, 1000);
|
||||
#elif defined(DEBUG_SERIAL_USART3)
|
||||
HAL_UART_Transmit(&huart3, (uint8_t *)&ch, 1, 1000);
|
||||
#endif
|
||||
return ch;
|
||||
}
|
||||
|
||||
#ifdef __GNUC__
|
||||
int _write(int file, char *data, int len) {
|
||||
int i;
|
||||
for (i = 0; i < len; i++) { __io_putchar( *data++ );}
|
||||
return len;
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/* =========================== Initialization Functions =========================== */
|
||||
|
||||
void BLDC_Init(void) {
|
||||
@@ -280,13 +308,13 @@ void Input_Init(void) {
|
||||
EE_ReadVariable(VirtAddVarTab[0], &writeCheck);
|
||||
if (writeCheck == FLASH_WRITE_KEY) {
|
||||
EE_ReadVariable(VirtAddVarTab[1] , &INPUT1_TYP_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[2] , &INPUT1_MIN_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[3] , &INPUT1_MID_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[4] , &INPUT1_MAX_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[2] , (uint16_t *)(intptr_t)INPUT1_MIN_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[3] , (uint16_t *)(intptr_t)INPUT1_MID_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[4] , (uint16_t *)(intptr_t)INPUT1_MAX_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[5] , &INPUT2_TYP_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[6] , &INPUT2_MIN_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[7] , &INPUT2_MID_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[8] , &INPUT2_MAX_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[6] , (uint16_t *)(intptr_t)INPUT2_MIN_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[7] , (uint16_t *)(intptr_t)INPUT2_MID_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[8] , (uint16_t *)(intptr_t)INPUT2_MAX_CAL);
|
||||
EE_ReadVariable(VirtAddVarTab[9] , &i_max);
|
||||
EE_ReadVariable(VirtAddVarTab[10], &n_max);
|
||||
rtP_Left.i_max = i_max;
|
||||
@@ -465,31 +493,34 @@ int checkInputType(int16_t min, int16_t mid, int16_t max){
|
||||
int16_t threshold = 200;
|
||||
#endif
|
||||
|
||||
HAL_Delay(10);
|
||||
if ((min / threshold) == (max / threshold) || (mid / threshold) == (max / threshold) || min > max || mid > max) {
|
||||
type = 0;
|
||||
printf("Input is ignored"); // (MIN and MAX) OR (MID and MAX) are close, disable input
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("ignored"); // (MIN and MAX) OR (MID and MAX) are close, disable input
|
||||
#endif
|
||||
} else {
|
||||
if ((min / threshold) == (mid / threshold)){
|
||||
type = 1;
|
||||
printf("Input is a normal pot"); // MIN and MID are close, it's a normal pot
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("a normal pot"); // MIN and MID are close, it's a normal pot
|
||||
#endif
|
||||
} else {
|
||||
type = 2;
|
||||
printf("Input is a mid-resting pot"); // it's a mid resting pot
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("a mid-resting pot"); // it's a mid resting pot
|
||||
#endif
|
||||
}
|
||||
HAL_Delay(10);
|
||||
|
||||
#ifdef CONTROL_ADC
|
||||
if ((min + INPUT_MARGIN - ADC_PROTECT_THRESH) > 0 && (max - INPUT_MARGIN + ADC_PROTECT_THRESH) < 4095) {
|
||||
printf(" and protected");
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf(" AND protected");
|
||||
#endif
|
||||
beepLong(2); // Indicate protection by a beep
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
HAL_Delay(10);
|
||||
printf("\r\n");
|
||||
HAL_Delay(10);
|
||||
|
||||
return type;
|
||||
}
|
||||
|
||||
@@ -509,7 +540,10 @@ void adcCalibLim(void) {
|
||||
}
|
||||
|
||||
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
|
||||
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("Input calibration started...\r\n");
|
||||
#endif
|
||||
|
||||
readInput();
|
||||
// Inititalization: MIN = a high value, MAX = a low value
|
||||
@@ -538,32 +572,44 @@ void adcCalibLim(void) {
|
||||
HAL_Delay(5);
|
||||
}
|
||||
|
||||
printf("Input1 is ");
|
||||
INPUT1_TYP_CAL = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp);
|
||||
if (INPUT1_TYP_CAL == INPUT1_TYPE || INPUT1_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
|
||||
INPUT1_MIN_CAL = INPUT1_MIN_temp + INPUT_MARGIN;
|
||||
INPUT1_MID_CAL = INPUT1_MID_temp;
|
||||
INPUT1_MAX_CAL = INPUT1_MAX_temp - INPUT_MARGIN;
|
||||
printf("Input1 OK\r\n"); HAL_Delay(10);
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("..OK\r\n");
|
||||
#endif
|
||||
} else {
|
||||
INPUT1_TYP_CAL = 0; // Disable input
|
||||
printf("Input1 Fail\r\n"); HAL_Delay(10);
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("..NOK\r\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
printf("Input2 is ");
|
||||
INPUT2_TYP_CAL = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp);
|
||||
if (INPUT2_TYP_CAL == INPUT2_TYPE || INPUT2_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
|
||||
INPUT2_MIN_CAL = INPUT2_MIN_temp + INPUT_MARGIN;
|
||||
INPUT2_MID_CAL = INPUT2_MID_temp;
|
||||
INPUT2_MAX_CAL = INPUT2_MAX_temp - INPUT_MARGIN;
|
||||
printf("Input2 OK\r\n"); HAL_Delay(10);
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("..OK\r\n");
|
||||
#endif
|
||||
} else {
|
||||
INPUT2_TYP_CAL = 0; // Disable input
|
||||
printf("Input2 Fail\r\n"); HAL_Delay(10);
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("..NOK\r\n");
|
||||
#endif
|
||||
}
|
||||
inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("Limits Input1: TYP:%i MIN:%i MID:%i MAX:%i\r\nLimits Input2: TYP:%i MIN:%i MID:%i MAX:%i\r\n",
|
||||
INPUT1_TYP_CAL, INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL,
|
||||
INPUT2_TYP_CAL, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
/*
|
||||
* Update Maximum Motor Current Limit (via ADC1) and Maximum Speed Limit (via ADC2)
|
||||
@@ -578,7 +624,10 @@ void updateCurSpdLim(void) {
|
||||
}
|
||||
|
||||
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
|
||||
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("Torque and Speed limits update started...\r\n");
|
||||
#endif
|
||||
|
||||
int32_t input1_fixdt = input1 << 16;
|
||||
int32_t input2_fixdt = input2 << 16;
|
||||
@@ -610,10 +659,12 @@ void updateCurSpdLim(void) {
|
||||
cur_spd_valid += 2; // Mark update to be saved in Flash at shutdown
|
||||
}
|
||||
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
// cur_spd_valid: 0 = No limit changed, 1 = Current limit changed, 2 = Speed limit changed, 3 = Both limits changed
|
||||
printf("Limits (%i)\r\nCurrent: fixdt:%li factor%i i_max:%i \r\nSpeed: fixdt:%li factor:%i n_max:%i\r\n",
|
||||
cur_spd_valid, input1_fixdt, cur_factor, rtP_Left.i_max, input2_fixdt, spd_factor, rtP_Left.n_max);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
@@ -745,7 +796,9 @@ void cruiseControl(uint8_t button) {
|
||||
|
||||
void poweroff(void) {
|
||||
enable = 0;
|
||||
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
|
||||
printf("-- Motors disabled --\r\n");
|
||||
#endif
|
||||
buzzerCount = 0; // prevent interraction with beep counter
|
||||
buzzerPattern = 0;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
|
||||
Reference in New Issue
Block a user