changes for independant left and right speed serial control
This commit is contained in:
parent
1841257078
commit
7e670e06d2
18
Inc/config.h
18
Inc/config.h
|
@ -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)
|
||||
|
|
2
Makefile
2
Makefile
|
@ -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
|
||||
|
|
66
Src/main.c
66
Src/main.c
|
@ -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.
Loading…
Reference in New Issue