changes for independant left and right speed serial control

This commit is contained in:
interfisch 2019-11-30 21:42:32 +01:00
parent 1841257078
commit 7e670e06d2
5 changed files with 95 additions and 71 deletions

View File

@ -61,11 +61,11 @@
* Enable warning and/or poweroff and make and flash firmware.
*/
#define TEMP_FILT_COEF 655 // temperature filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16
#define TEMP_CAL_LOW_ADC 1655 // temperature 1: ADC value
#define TEMP_CAL_LOW_DEG_C 358 // temperature 1: measured temperature [°C * 10]. Here 35.8 °C
#define TEMP_CAL_HIGH_ADC 1588 // temperature 2: ADC value
#define TEMP_CAL_HIGH_DEG_C 489 // temperature 2: measured temperature [°C * 10]. Here 48.9 °C
#define TEMP_WARNING_ENABLE 0 // to beep or not to beep, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION!
#define TEMP_CAL_LOW_ADC 1716 // temperature 1: ADC value
#define TEMP_CAL_LOW_DEG_C 210 // temperature 1: measured temperature [°C * 10]. Here 35.8 °C
#define TEMP_CAL_HIGH_ADC 1600 // temperature 2: ADC value
#define TEMP_CAL_HIGH_DEG_C 420 // temperature 2: measured temperature [°C * 10]. Here 48.9 °C
#define TEMP_WARNING_ENABLE 1 // to beep or not to beep, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION!
#define TEMP_WARNING 600 // annoying fast beeps [°C * 10]. Here 60.0 °C
#define TEMP_POWEROFF_ENABLE 0 // to poweroff or not to poweroff, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION!
#define TEMP_POWEROFF 650 // overheat poweroff. (while not driving) [°C * 10]. Here 65.0 °C
@ -91,15 +91,15 @@
#define USART2_BAUD 38400 // UART2 baud rate (long wired cable)
#define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
// #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino
// #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
#define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino
#define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
// #define DEBUG_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
#define USART3_BAUD 38400 // UART3 baud rate (short wired cable)
#define USART3_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
// #define CONTROL_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used! For Arduino control check the hoverSerial.ino
// #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
//#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
#if defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)
#define UART_DMA_CHANNEL DMA1_Channel7
@ -122,7 +122,7 @@
* For middle resting potis: Let the potis in the middle resting position, write value 1 to ADC1_MID and value 2 to ADC2_MID
* Make, flash and test it.
*/
#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
//#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#define ADC1_MID_POT // ADC1 middle resting poti: comment-out if NOT a middle resting poti
#define ADC2_MID_POT // ADC2 middle resting poti: comment-out if NOT a middle resting poti
#define ADC1_MIN 0 // min ADC1-value while poti at minimum-position (0 - 4095)

View File

@ -52,7 +52,7 @@ startup_stm32f103xe.s
#######################################
# binaries
#######################################
PREFIX = arm-none-eabi-
PREFIX = ~/gcc-arm-none-eabi-7-2018-q2-update/bin/arm-none-eabi-
CC = $(PREFIX)gcc
AS = $(PREFIX)gcc -x assembler-with-cpp
CP = $(PREFIX)objcopy

View File

@ -69,8 +69,10 @@ static UART_HandleTypeDef huart;
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
typedef struct{
uint16_t start;
int16_t steer;
int16_t speed;
//int16_t steer;
int16_t speedLeft;
//int16_t speed;
int16_t speedRight;
uint16_t checksum;
} Serialcommand;
static volatile Serialcommand command;
@ -103,12 +105,18 @@ uint8_t ctrlModReqRaw = CTRL_MOD_REQ;
uint8_t ctrlModReq; // Final control mode request
static int cmd1; // normalized input value. -1000 to 1000
static int cmd2; // normalized input value. -1000 to 1000
static int16_t steer; // local variable for steering. -1000 to 1000
static int16_t speed; // local variable for speed. -1000 to 1000
static int16_t steerFixdt; // local fixed-point variable for steering low-pass filter
static int16_t speedFixdt; // local fixed-point variable for speed low-pass filter
static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter
static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter
//static int16_t steer; // local variable for steering. -1000 to 1000
//static int16_t speed; // local variable for speed. -1000 to 1000
//static int16_t steerFixdt; // local fixed-point variable for steering low-pass filter
//static int16_t speedFixdt; // local fixed-point variable for speed low-pass filter
static int16_t speedLeftFixdt; // local fixed-point variable for speedLeft low-pass filter
static int16_t speedRightFixdt; // local fixed-point variable for speedRight low-pass filter
//static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter
//static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter
static int16_t speedLeftRateFixdt; // local fixed-point variable for steering rate limiter
static int16_t speedRightRateFixdt; // local fixed-point variable for speed rate limiter
static int16_t speed; // local variable for speed. -1000 to 1000. only used for security checks. will be calculated by speedL and speedR
extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
@ -322,16 +330,30 @@ int main(void) {
#if defined CONTROL_SERIAL_USART2 || defined CONTROL_SERIAL_USART3
// Handle received data validity, timeout and fix out-of-sync if necessary
if (command.start == START_FRAME && command.checksum == (command.start ^ command.steer ^ command.speed)) {
//if (command.start == START_FRAME && command.checksum == (command.start ^ command.steer ^ command.speed)) {
if (command.start == START_FRAME && command.checksum == (command.start ^ command.speedLeft ^ command.speedRight)) {
if (timeoutFlag) { // Check for previous timeout flag
if (timeoutCnt-- <= 0) // Timeout de-qualification
timeoutFlag = 0; // Timeout flag cleared
} else {
cmd1 = CLAMP((int16_t)command.steer, -1000, 1000);
cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
//cmd1 = CLAMP((int16_t)command.steer, -1000, 1000);
cmd1 = CLAMP((int16_t)command.speedLeft, -1000, 1000);
//cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
cmd2 = CLAMP((int16_t)command.speedRight, -1000, 1000);
command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle
timeoutCnt = 0; // Reset the timeout counter
}
// ####### 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
consoleLog("-- Motors enabled --\r\n");
}
} else {
if (timeoutCnt++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlag = 1; // Timeout detected
@ -357,28 +379,30 @@ int main(void) {
#endif
// ####### 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
consoleLog("-- Motors enabled --\r\n");
}
// ####### 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
*/
rateLimiter16(cmd1, RATE, &speedLeftRateFixdt);
rateLimiter16(cmd2, RATE, &speedRightRateFixdt);
filtLowPass16(speedLeftRateFixdt >> 4, FILTER, &speedLeftFixdt);
filtLowPass16(speedRightRateFixdt >> 4, FILTER, &speedRightFixdt);
speedL = speedLeftFixdt >> 4; // convert fixed-point to integer
speedR = speedRightFixdt >> 4; // convert fixed-point to integer
speed = (speedL+speedR)/2;
// ####### 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
//mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above
// ####### 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) {

Binary file not shown.

Binary file not shown.