lucordes/touchscreengames
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

direct control

Browse Source
This commit is contained in:
lucordes 2025-09-06 19:42:33 +02:00
parent 0700f7c4f5
commit 91a7a8529a
1 changed files with 44 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

166
touchscreengames/Core/Src/main.c
View File

@ -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 */
}