From 91a7a8529a46661ead0076ad4806fa0ae1456680 Mon Sep 17 00:00:00 2001 From: lucordes Date: Sat, 6 Sep 2025 19:42:33 +0200 Subject: [PATCH] direct control --- touchscreengames/Core/Src/main.c | 166 ++++++++----------------------- 1 file changed, 44 insertions(+), 122 deletions(-) diff --git a/touchscreengames/Core/Src/main.c b/touchscreengames/Core/Src/main.c index 9537cbf..626e216 100644 --- a/touchscreengames/Core/Src/main.c +++ b/touchscreengames/Core/Src/main.c @@ -869,7 +869,6 @@ void start_control(void *argument) { motor.ur = 0.0 * M_PI; for (;;) { - vTaskDelay(100000); vTaskDelay(TSmillis); vTaskGetRunTimeStats(statsBuffer); @@ -917,37 +916,37 @@ void start_control(void *argument) { } // Calculation for Motor 1 control signal - float error1 = (-1) * motor.ur - motor.wr; - if (error1 > 10) { - int t = 0; - } - float anti_windup_1 = Ki * error1 - sat_diff_1 * Kw; - integral1 += anti_windup_1 * TS; - float control1 = integral1 + Kp * error1; - float unsat_control_1 = control1; - control1 = - fabsf(control1) > 0.7f ? - (control1 / fabsf(control1)) * 0.7f : control1; - sat_diff_1 = unsat_control_1 - control1; +// float error1 = (-1) * motor.ur - motor.wr; +// if (error1 > 10) { +// int t = 0; +// } +// float anti_windup_1 = Ki * error1 - sat_diff_1 * Kw; +// integral1 += anti_windup_1 * TS; +// float control1 = integral1 + Kp * error1; +// float unsat_control_1 = control1; +// control1 = +// fabsf(control1) > 0.7f ? +// (control1 / fabsf(control1)) * 0.7f : control1; +// sat_diff_1 = unsat_control_1 - control1; +// +// //set_pwm(control1, 'r', 'c'); +// //set_pwm(0.15, 'r', 'c'); +// +// // Calculation for Motor 2 control signal +// float error2 = motor.ul - motor.wl; +// if (error2 > 10) { +// int t = 0; +// } +// float anti_windup_2 = Ki * error2 - sat_diff_2 * Kw; +// integral2 += anti_windup_2 * TS; +// float control2 = integral2 + Kp * error2; +// float unsat_control_2 = control2; +// control2 = +// fabsf(control2) > 0.7f ? +// (control2 / fabsf(control2)) * 0.7f : control2; +// sat_diff_2 = unsat_control_2 - control2; - set_pwm(control1, 'r', 'c'); - //set_pwm(0.15, 'r', 'c'); - - // Calculation for Motor 2 control signal - float error2 = motor.ul - motor.wl; - if (error2 > 10) { - int t = 0; - } - float anti_windup_2 = Ki * error2 - sat_diff_2 * Kw; - integral2 += anti_windup_2 * TS; - float control2 = integral2 + Kp * error2; - float unsat_control_2 = control2; - control2 = - fabsf(control2) > 0.7f ? - (control2 / fabsf(control2)) * 0.7f : control2; - sat_diff_2 = unsat_control_2 - control2; - - set_pwm(control2, 'l', 'c'); + //set_pwm(control2, 'l', 'c'); //set_pwm(0.15, 'l', 'c'); } @@ -1219,8 +1218,8 @@ void start_balance(void *argument) { HAL_Delay(500); float alpha = 0.9; - float Kp = 40; //80 good - float Ki = 2; + float Kp = 5; //80 good + float Ki = 5*0.15; float Kd = 100 * 0; float integral; @@ -1229,15 +1228,8 @@ void start_balance(void *argument) { float last_pitch; float Kw = 0.01; - float tempspeed; - float Ki_m = 4; - float Kp_m = 0.005 * 10; - float Kw_m = 5; - float integral1 = 0; - float integral2 = 0; - float sat_diff_1 = 0; - float sat_diff_2 = 0; - + float position; + float position_kp=-10; for (;;) { vTaskDelay(TSmillis); if (MPU6050_Read(&sensor) == HAL_OK) { @@ -1260,94 +1252,24 @@ void start_balance(void *argument) { // sat_diff_1 = unsat_control_1 - control1; integral += error; float control = Kp * error + integral * Ki - + (last_pitch - pitch) * Kd; + + (last_pitch - pitch) * Kd+position*position_kp; - motor.ur = control; - motor.ul = motor.ur; + + set_pwm(control, 'l', 'c'); + set_pwm(-control, 'r', 'c'); + //motor.ul = motor.ur; last_pitch = pitch; - } else { - motor.ur = 0; - motor.ul = motor.ur; - } - - - tempspeed = -encoder_TIM4_speed_rad(); - if (fabsf(tempspeed - motor.wr) < 700.0f) { - motor.wr = tempspeed; - // char uart_buf[20]; - // int whole = (int) motor.wr; - // int frac = (int) ((motor.wr - whole) * 1000); // 3 decimal digits - // - // if (frac < 0) - // frac = -frac; // handle negative numbers - //// - // int len = 0; - // if (motor.wr < 0 && whole == 0) - // len = sprintf(uart_buf, "-0.%03d\r\n", frac); - // else - // len = sprintf(uart_buf, "%d.%03d\r\n", whole, frac); - // - // HAL_UART_Transmit(&huart6, (uint8_t*) uart_buf, len, HAL_MAX_DELAY); - } - tempspeed = encoder_TIM1_speed_rad(); - if (fabsf(tempspeed - motor.wl) > 70.0f) { + position+=motor.wl*0.03*2*M_PI*TS; //3cm wheel radius } else { - motor.wl = tempspeed; - - // char uart_buf[20]; - // int whole = (int) motor.wl; - // int frac = (int) ((motor.wl - whole) * 1000); // 3 decimal digits - // - // if (frac < 0) - // frac = -frac; // handle negative numbers - //// - // int len = 0; - // if (motor.wl < 0 && whole == 0) - // len = sprintf(uart_buf, "-0.%03d\r\n", frac); - // else - // len = sprintf(uart_buf, "%d.%03d\r\n", whole, frac); - // - // HAL_UART_Transmit(&huart6, (uint8_t*) uart_buf, len, HAL_MAX_DELAY); - + //motor.ur = 0; + //motor.ul = motor.ur; + set_pwm(0, 'l', 'c'); + set_pwm(0, 'r', 'c'); } - // Calculation for Motor 1 control signal - float error1 = (-1) * motor.ur - motor.wr; - if (error1 > 10) { - int t = 0; - } - float anti_windup_1 = Ki_m * error1 - sat_diff_1 * Kw; - integral1 += anti_windup_1 * TS; - float control1 = integral1 + Kp_m * error1; - float unsat_control_1 = control1; - control1 = - fabsf(control1) > 0.7f ? - (control1 / fabsf(control1)) * 0.7f : control1; - sat_diff_1 = unsat_control_1 - control1; - - set_pwm(control1, 'r', 'c'); - //set_pwm(0.15, 'r', 'c'); - - // Calculation for Motor 2 control signal - float error2 = motor.ul - motor.wl; - if (error2 > 10) { - int t = 0; - } - float anti_windup_2 = Ki_m * error2 - sat_diff_2 * Kw; - integral2 += anti_windup_2 * TS; - float control2 = integral2 + Kp_m * error2; - float unsat_control_2 = control2; - control2 = - fabsf(control2) > 0.7f ? - (control2 / fabsf(control2)) * 0.7f : control2; - sat_diff_2 = unsat_control_2 - control2; - - set_pwm(control2, 'l', 'c'); - //set_pwm(0.15, 'l', 'c'); - } /* USER CODE END start_balance */ }