UPD: more universal codebase for different vehicles

This commit is contained in:
Niklas Fauth 2018-04-09 21:43:59 +02:00
parent 69a14c52a9
commit de8957536c
6 changed files with 1211 additions and 1455 deletions

View File

@ -10,22 +10,49 @@
#define MILLI_PSI (PSI * 1000) #define MILLI_PSI (PSI * 1000)
#define MILLI_V (V * 1000) #define MILLI_V (V * 1000)
// ################################################################################
#define PWM_FREQ 16000 // PWM frequency in Hz #define PWM_FREQ 16000 // PWM frequency in Hz
#define DEAD_TIME 32 // PWM deadtime #define DEAD_TIME 32 // PWM deadtime
//#define DC_CUR_LIMIT 34 // Motor DC current limit in amps
#define DC_CUR_LIMIT 35 // Motor DC current limit in amps #define DC_CUR_LIMIT 35 // Motor DC current limit in amps
// ################################################################################
#define DEBUG_SERIAL_USART2
//#define DEBUG_SERIAL_USART3
#define DEBUG_BAUD 115200 // UART baud rate
//#define DEBUG_SERIAL_SERVOTERM //#define DEBUG_SERIAL_SERVOTERM
#define DEBUG_SERIAL_ASCII #define DEBUG_SERIAL_ASCII
#define DEBUG_SERIAL_USART2
#define DEBUG_BAUD 115200 // UART baud rate
//#define DEBUG_I2C_LCD //#define DEBUG_I2C_LCD
//#define CONTROL_PPM // use PPM CONTROL_PPM // ################################################################################
#define PPM_NUM_CHANNELS 6 // number of PPM channels to receive
// ###### CONTROL VIA RC REMOTE ######
//#define CONTROL_PPM // use PPM CONTROL_PPM
//#define PPM_NUM_CHANNELS 6 // number of PPM channels to receive
// ###### CONTROL VIA TWO POTENTIOMETERS ######
// #define CONTROL_ADC // #define CONTROL_ADC
// ###### CONTROL VIA NINTENDO NUNCHUCK ######
#define CONTROL_NUNCHUCK #define CONTROL_NUNCHUCK
// ################################################################################
// ###### DRIVING BEHAVIOR ######
#define FILTER 0.1
#define SPEED_COEFFICIENT 0.5
#define STEER_COEFFICIENT 0.5
// ###### BOBBYCAR ######
// #define FILTER 0.1
// #define SPEED_COEFFICIENT 1
// #define STEER_COEFFICIENT 0
// ###### ARMCHAIR ######
// #define FILTER 0.05
// #define SPEED_COEFFICIENT 0.5
// #define STEER_COEFFICIENT 0.2
// #define BEEPS_BACKWARD // #define BEEPS_BACKWARD

View File

@ -12,7 +12,7 @@ volatile int pwmr = 0;
volatile int weakl = 0; volatile int weakl = 0;
volatile int weakr = 0; volatile int weakr = 0;
extern volatile int pwmrl; extern volatile int speed;
extern volatile adc_buf_t adc_buffer; extern volatile adc_buf_t adc_buffer;
@ -132,8 +132,6 @@ int offsetdcl = 2000;
int offsetdcr = 2000; int offsetdcr = 2000;
float batteryVoltage = 40.0; float batteryVoltage = 40.0;
float adccmd1;
float adccmd2;
int curl = 0; int curl = 0;
// int errorl = 0; // int errorl = 0;
@ -166,7 +164,7 @@ void DMA1_Channel1_IRQHandler() {
#ifdef BEEPS_BACKWARD #ifdef BEEPS_BACKWARD
if (pwmrl < -50 && enable == 1) { if (speed < -50 && enable == 1) {
buzzerFreq = 5; buzzerFreq = 5;
buzzerPattern = 1; buzzerPattern = 1;
} else if (enable == 1) { } else if (enable == 1) {
@ -214,8 +212,8 @@ void DMA1_Channel1_IRQHandler() {
blockPhaseCurrent(posl, adc_buffer.rl1 - offsetrl1, adc_buffer.rl2 - offsetrl2, &curl); blockPhaseCurrent(posl, adc_buffer.rl1 - offsetrl1, adc_buffer.rl2 - offsetrl2, &curl);
setScopeChannel(2, (adc_buffer.rl1 - offsetrl1) / 8); //setScopeChannel(2, (adc_buffer.rl1 - offsetrl1) / 8);
setScopeChannel(3, (adc_buffer.rl2 - offsetrl2) / 8); //setScopeChannel(3, (adc_buffer.rl2 - offsetrl2) / 8);
buzzerTimer++; buzzerTimer++;

View File

@ -5,7 +5,6 @@
#include "config.h" #include "config.h"
TIM_HandleTypeDef TimHandle; TIM_HandleTypeDef TimHandle;
uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1] = {0};
uint8_t ppm_count = 0; uint8_t ppm_count = 0;
uint32_t timeout = 100; uint32_t timeout = 100;
uint8_t nunchuck_data[6] = {0}; uint8_t nunchuck_data[6] = {0};
@ -16,6 +15,9 @@ extern I2C_HandleTypeDef hi2c2;
DMA_HandleTypeDef hdma_i2c2_rx; DMA_HandleTypeDef hdma_i2c2_rx;
DMA_HandleTypeDef hdma_i2c2_tx; DMA_HandleTypeDef hdma_i2c2_tx;
#ifdef CONTROL_PPM
uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1] = {0};
void PPM_ISR_Callback() { void PPM_ISR_Callback() {
// Dummy loop with 16 bit count wrap around // Dummy loop with 16 bit count wrap around
uint16_t rc_delay = TIM2->CNT; uint16_t rc_delay = TIM2->CNT;
@ -54,7 +56,7 @@ void PPM_Init() {
HAL_NVIC_EnableIRQ(EXTI3_IRQn); HAL_NVIC_EnableIRQ(EXTI3_IRQn);
HAL_TIM_Base_Start(&TimHandle); HAL_TIM_Base_Start(&TimHandle);
} }
#endif
void Nunchuck_Init() { void Nunchuck_Init() {
//-- START -- init WiiNunchuck //-- START -- init WiiNunchuck

View File

@ -25,15 +25,21 @@
void SystemClock_Config(void); void SystemClock_Config(void);
extern float adccmd1; int cmd1;
extern float adccmd2; int cmd2;
int cmd3;
int steer;
int speed;
extern TIM_HandleTypeDef htim_left; extern TIM_HandleTypeDef htim_left;
extern TIM_HandleTypeDef htim_right; extern TIM_HandleTypeDef htim_right;
extern ADC_HandleTypeDef hadc1; extern ADC_HandleTypeDef hadc1;
extern ADC_HandleTypeDef hadc2; extern ADC_HandleTypeDef hadc2;
extern volatile adc_buf_t adc_buffer; extern volatile adc_buf_t adc_buffer;
#ifdef CONTROL_PPM
extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1]; extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1];
#endif
extern volatile int pwml; extern volatile int pwml;
extern volatile int pwmr; extern volatile int pwmr;
@ -110,8 +116,6 @@ int main(void) {
Nunchuck_Init(); Nunchuck_Init();
#endif #endif
enable = 1; enable = 1;
while(1) { while(1) {
@ -119,62 +123,51 @@ int main(void) {
#ifdef CONTROL_NUNCHUCK #ifdef CONTROL_NUNCHUCK
Nunchuck_Read(); Nunchuck_Read();
adccmd1 = adccmd1 * 0.9 + (float)nunchuck_data[0] * 0.1; cmd1 = CLAMP((nunchuck_data[0] - 127) * 10, -1000, 1000);
adccmd2 = adccmd2 * 0.9 + (float)nunchuck_data[1] * 0.1; cmd2 = CLAMP((nunchuck_data[1] - 127) * 10, -1000, 1000);
setScopeChannel(0, adccmd1);
setScopeChannel(1, adccmd2); //uint8_t button1 = (uint8_t)nunchuck_data[5] & 1;
setScopeChannel(2, (int)nunchuck_data[5] & 1); //uint8_t button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1;
setScopeChannel(3, ((int)nunchuck_data[5] >> 1) & 1);
timeout = 0;
#endif #endif
#ifdef CONTROL_PPM #ifdef CONTROL_PPM
speedL = -(CLAMP((((ppm_captured_value[1]-500)+(ppm_captured_value[0]-500)/2.0)*(ppm_captured_value[2]/500.0)), -800, 800)); cmd1 = CLAMP((ppm_captured_value[0] - 500) * 2, -1000, 1000);
speedR = (CLAMP((((ppm_captured_value[1]-500)-(ppm_captured_value[0]-500)/2.0)*(ppm_captured_value[2]/500.0)), -800, 800)); cmd2 = CLAMP((ppm_captured_value[1] - 500) * 2, -1000, 1000);
if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < 50) {
//pwmr = speedR;
//pwml = speedL;
}
lastSpeedL = speedL;
lastSpeedR = speedR;
//setScopeChannel(0, (int)pwmrl);
//setScopeChannel(1, (int)speedL);
#endif #endif
#ifdef CONTROL_ADC #ifdef CONTROL_ADC
//adccmd1 = adccmd1 * 0.9 + (float)adc_buffer.l_rx2 * 0.1; // throttle cmd1 = CLAMP(adc_buffer.l_rx2 - 700, 0, 2350) / 2.35;
adccmd2 = adccmd2 * 0.9 + (float)adc_buffer.l_tx2 * 0.1; // button //uint8_t button1 = (uint8_t)(adc_buffer.l_tx2 > 2000);
pwmrl = pwmrl * 0.9 + (CLAMP(adc_buffer.l_rx2 - 700, 0, 2350) / 2.35) * 0.1 * direction; timeout = 0;
// pwmrl has to be 0-1000 (or negative when driving backwards)
setScopeChannel(0, (int)adccmd1);
setScopeChannel(1, (int)adccmd2);
// adccmd2 = button, ranges 0 in idle and 4096 when pressed
if (adccmd2 > 2000 && pwmrl < 300) { // driving backwards at low speeds
direction = -0.2;
} else {
direction = 1;
}
if (adccmd2 > 2000 && pwmrl > 700) { // field weakening at high speeds
weakl = pwmrl - 600; // weak should never exceed 400 or 450 MAX!!
weakr = pwmrl - 600;
} else {
weakl = 0;
weakr = 0;
}
if (pwml < 1000) {
pwml +=1;
}
pwml = pwmrl;
pwmr = -pwmrl;
#endif #endif
// ####### LOW-PASS FILTER #######
speed = speed * (1.0 - FILTER) + cmd1 * FILTER;
steer = steer * (1.0 - FILTER) + cmd2 * FILTER;
setScopeChannel(0, (int)speed);
setScopeChannel(1, (int)steer);
// ####### MIXER #######
speedR = CLAMP(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT, -1000, 1000);
speedL = CLAMP(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT, -1000, 1000);
setScopeChannel(2, (int)speedR);
setScopeChannel(3, (int)speedL);
// ####### SET OUTPUTS #######
if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < 50) {
pwmr = speedR;
pwml = speedL;
}
lastSpeedL = speedL;
lastSpeedR = speedR;
// ####### LOG TO CONSOLE #######
consoleScope(); consoleScope();
timeout=0; timeout=0;

View File

@ -168,7 +168,7 @@ void SysTick_Handler(void) {
/* USER CODE END SysTick_IRQn 1 */ /* USER CODE END SysTick_IRQn 1 */
} }
#ifdef CONTROL_NUNCHUCK
extern I2C_HandleTypeDef hi2c2; extern I2C_HandleTypeDef hi2c2;
void I2C1_EV_IRQHandler(void) void I2C1_EV_IRQHandler(void)
{ {
@ -207,14 +207,15 @@ void DMA1_Channel5_IRQHandler(void)
/* USER CODE END DMA1_Channel5_IRQn 1 */ /* USER CODE END DMA1_Channel5_IRQn 1 */
} }
#endif
#ifdef CONTROL_PPM
void EXTI3_IRQHandler(void) void EXTI3_IRQHandler(void)
{ {
PPM_ISR_Callback(); PPM_ISR_Callback();
__HAL_GPIO_EXTI_CLEAR_IT(GPIO_PIN_3); __HAL_GPIO_EXTI_CLEAR_IT(GPIO_PIN_3);
} }
#endif
/******************************************************************************/ /******************************************************************************/
/* STM32F1xx Peripheral Interrupt Handlers */ /* STM32F1xx Peripheral Interrupt Handlers */

File diff suppressed because it is too large Load Diff