cyy/origincar_controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
4dadbeeb5cb59459a6ad213e1af5a17edeffe8c0
origincar_controller/BALANCE/balance.c
cyy_mac 93ca37e36c 完善阿克曼控制与高速串口遥测 
- 校正舵机中位、转向符号和阿克曼后轮差速模型\n- 增加航向角速度辅助及遥控通道调试开关\n- 将速度环提升至 200Hz，并按实际 dt 计算 PI 积分\n- 将 IMU 启动校准缩短为 2 秒\n- 为 USART3 增加 DMA 发送和 MCU 采样时间戳
2026-08-12 18:51:48 +08:00

935 lines
42 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#include "balance.h"
int Time_count=0; //Time variable //<2F><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>
// Robot mode is wrong to detect flag bits
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λ
int robot_mode_check_flag=0;
short test_num;
Encoder OriginalEncoder; //Encoder raw data //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
u8 command_lost_count=0; //<2F><><EFBFBD>ڡ<EFBFBD>CAN<41><4E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧʱ<CAA7><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ1<CAA7><31><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
/* Calibrated Ackermann steering model (manual push-test, motors disabled).
Maps signed path curvature kappa = 1/R [1/m, R at the rear-axle center] to
servo PWM. Sign convention: kappa > 0 -> right turn, kappa < 0 -> left turn.
Quadratic fit of measured (servo, 1/R) points, max residual ~12 PWM:
servo = AKM_C0 + AKM_C1*kappa + AKM_C2*kappa*kappa
NOTE: kappa is in 1/m. The raw calibration table listed 1/R in 1/cm; these
coefficients were fit after converting R from cm to m, so they must be fed
SI curvature (kappa = wz/Vx, both SI). Straight-ahead lands near 1656 PWM. */
#define AKM_SERVO_C0 1656.373f
#define AKM_SERVO_C1 140.548f
#define AKM_SERVO_C2 (-7.654f)
/* Steering range from the calibration table: PWM 1100 (left) .. 2000 (right). */
#define AKM_SERVO_MIN 1100
#define AKM_SERVO_MAX 2000
/* Largest curvature the car can actually track: R_min ~= 0.30 m -> 3.33 /m. */
#define AKM_KAPPA_MAX 3.331f
/* Remote CH1 neutral pulse width [us]. The servo straight-ahead neutral is
SERVO_INIT (motor.h). The two differ, so the CH1 passthrough is shifted by
(SERVO_INIT - AKM_REMOTER_CH1_MID) to keep stick-center = wheels-straight. */
#define AKM_REMOTER_CH1_MID 1500
/* Linear steering map that honors the servo's real straight-ahead neutral
(SERVO_INIT) instead of the arithmetic midpoint of [MIN, MAX]. norm > 0 =
left -> toward AKM_SERVO_MIN; norm < 0 = right -> toward AKM_SERVO_MAX;
norm == 0 -> SERVO_INIT. Each side is scaled to its own end stop so the full
mechanical travel is used even though the neutral is off-center. */
#if AKM_DIRECT_MAP || AKM_YAW_ASSIST
static int Akm_Norm_To_Servo(float norm)
{
float span, pwm;
norm = target_limit_float(norm, -1.0f, 1.0f);
span = (norm >= 0.0f) ? (float)(SERVO_INIT - AKM_SERVO_MIN)
: (float)(AKM_SERVO_MAX - SERVO_INIT);
pwm = (float)SERVO_INIT - norm * span;
return (int)(pwm + (pwm >= 0.0f ? 0.5f : -0.5f));
}
#endif
/* Needed by Mode 0 (calibrated Ackermann) and Mode 2 (yaw-rate assist), i.e.
whenever direct-map is off and CH1 is not overriding the servo. Direct-map
(Mode 1) and the CH1 debug override never call it. */
#if !AKM_DIRECT_MAP && !AKM_SERVO_DEBUG_REMOTE_CH1
static int Akm_Curvature_To_Servo(float kappa)
{
float pwm;
kappa = target_limit_float(kappa, -AKM_KAPPA_MAX, AKM_KAPPA_MAX);
pwm = AKM_SERVO_C0 + AKM_SERVO_C1 * kappa + AKM_SERVO_C2 * kappa * kappa;
return (int)(pwm + (pwm >= 0.0f ? 0.5f : -0.5f));
}
#endif
/**************************************************************************
Function: The inverse kinematics solution is used to calculate the target speed of each wheel according to the target speed of three axes
Input : X and Y, Z axis direction of the target movement speed
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD>˶<EFBFBD>ѧ<EFBFBD><EFBFBD>⣬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD><EFBFBD>ٶȼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD><EFBFBD>Y<EFBFBD><EFBFBD>Z<EFBFBD>᷽<EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD>ٶ<EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void Drive_Motor(float Vx,float Vy,float Vz)
{
float amplitude=3.5; //Wheel target speed limit //<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD><D9B6>޷<EFBFBD>
//Speed smoothing is enabled when moving the omnidirectional trolley
//ȫ<><C8AB><EFBFBD>ƶ<EFBFBD>С<EFBFBD><D0A1><EFBFBD>ſ<EFBFBD><C5BF><EFBFBD><EFBFBD>ٶ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
{
Smooth_control(Vx,Vy,Vz); //Smoothing the input speed //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȽ<D9B6><C8BD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//Get the smoothed data
//<2F><>ȡƽ<C8A1><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Vx=smooth_control.VX;
Vy=smooth_control.VY;
Vz=smooth_control.VZ;
}
//Mecanum wheel car
//<2F><><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><C4B7>С<EFBFBD><D0A1>
if (Car_Mode==Mec_Car)
{
//Inverse kinematics //<2F>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD>
MOTOR_A.Target = +Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
MOTOR_B.Target = -Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
MOTOR_C.Target = +Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
MOTOR_D.Target = -Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
//Wheel (motor) target speed limit //<2F><><EFBFBD><EFBFBD>(<28><><EFBFBD>)Ŀ<><C4BF><EFBFBD>ٶ<EFBFBD><D9B6>޷<EFBFBD>
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude);
MOTOR_D.Target=target_limit_float(MOTOR_D.Target,-amplitude,amplitude);
}
//Omni car
//ȫ<><C8AB><EFBFBD><EFBFBD>С<EFBFBD><D0A1>
else if (Car_Mode==Omni_Car)
{
//Inverse kinematics //<2F>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD>
MOTOR_A.Target = Vy + Omni_turn_radiaus*Vz;
MOTOR_B.Target = -X_PARAMETER*Vx - Y_PARAMETER*Vy + Omni_turn_radiaus*Vz;
MOTOR_C.Target = +X_PARAMETER*Vx - Y_PARAMETER*Vy + Omni_turn_radiaus*Vz;
//Wheel (motor) target speed limit //<2F><><EFBFBD><EFBFBD>(<28><><EFBFBD>)Ŀ<><C4BF><EFBFBD>ٶ<EFBFBD><D9B6>޷<EFBFBD>
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude);
MOTOR_D.Target=0; //Out of use //û<><C3BB>ʹ<EFBFBD>õ<EFBFBD>
}
//Ackermann structure car
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>
else if (Car_Mode==Akm_Car)
{
#if AKM_DIRECT_MAP
// Direct passthrough mode (tuning/debug, not physically Ackermann):
// - Vz is linearly mapped across the full servo travel, independent
// of speed. Vz > 0 = left (ROS) -> AKM_SERVO_MIN (left end).
// - Vx is sent to both drive wheels unchanged (no differential).
// The AKM_SERVO_DEBUG_REMOTE_CH1 override below still takes priority
// over this servo value.
// vz_norm > 0 (left) -> AKM_SERVO_MIN; 0 -> SERVO_INIT (straight).
float vz_norm = target_limit_float(Vz / AKM_DIRECT_VZ_FULL, -1.0f, 1.0f);
Servo = Akm_Norm_To_Servo(vz_norm);
MOTOR_A.Target = Vx;
MOTOR_B.Target = Vx;
MOTOR_C.Target = 0;
MOTOR_D.Target = 0;
#elif AKM_YAW_ASSIST
// Mode 2: DECOUPLED steering + IMU yaw-rate differential assist
// (simplified torque vectoring). This deliberately breaks the Ackermann
// w-v coupling that shrinks the steering angle at speed:
//
// * Steering (w): the servo is a DIRECT map of Vz across the full
// curvature range, INDEPENDENT of Vx (like Mode 1). So a large Vz
// always yields a large front-wheel angle, even at high Vx.
// * Drive (v): Vx sets the base wheel speed independently.
// * Ackermann is used only as the FEEDFORWARD reference for the rear
// differential; an IMU yaw-rate PI loop trims on top.
//
// Sign bookkeeping (fit domain, kappa>0 = right; ROS Vz>0 = left = CCW):
// vz_norm = clamp(Vz/AKM_DIRECT_VZ_FULL, +-1)
// Servo = linear FULL-TRAVEL map of vz_norm (SAME as Mode 1 /
// direct map), NOT the calibration fit -- this is what
// gives full steering authority at any speed.
// kappa_cmd = -vz_norm * AKM_KAPPA_MAX is used only to build the
// Ackermann feedforward reference for the rear diff.
// The yaw loop works in the ROS/geometric frame (r>0 = left).
static float yaw_integral = 0.0f; // PI integrator state [m/s]
static float r_filt = 0.0f; // low-pass filtered yaw rate [rad/s]
float vz_norm, kappa_cmd;
float r_ref, r_meas, e_r, dv_ff, dv_fb, dv, dv_max;
// Decoupled steering command: Vz -> normalized steering, NOT via Vx.
vz_norm = target_limit_float(Vz / AKM_DIRECT_VZ_FULL, -1.0f, 1.0f);
// Servo = linear full-travel map, identical to Mode 1 (direct map),
// centered on SERVO_INIT. vz_norm > 0 (left) -> AKM_SERVO_MIN.
Servo = Akm_Norm_To_Servo(vz_norm);
// Ackermann feedforward reference yaw rate for the commanded steering.
// kappa_cmd maps full stick to the car's max trackable curvature.
// Back in the ROS/geometric frame: r_ref > 0 = left turn.
kappa_cmd = -vz_norm * AKM_KAPPA_MAX;
r_ref = -kappa_cmd * Vx;
// Measured yaw rate from the gyro, de-biased LSB -> rad/s, with an
// optional sign flip and a light first-order low-pass.
r_meas = AKM_GYRO_Z_SIGN * (float)gyro[2] / AKM_GYRO_Z_TO_RADPS;
r_filt += AKM_YAW_IMU_LPF * (r_meas - r_filt);
if(float_abs(Vx) < AKM_YAW_MIN_SPEED)
{
// Too slow for a meaningful yaw rate: freeze the loop, no assist.
yaw_integral = 0.0f;
dv = 0.0f;
}
else
{
e_r = r_ref - r_filt;
// Feedforward: alpha=1 reproduces the Mode-0 geometric differential
// exactly (dv = 0.5*track*|kappa|*Vx expressed via r_ref).
dv_ff = AKM_YAW_FF_ALPHA * 0.5f * Wheel_spacing * r_ref;
// PI feedback with rectangular integration at the fixed control
// period (Drive_Motor runs at CONTROL_FREQUENCY Hz).
yaw_integral += e_r * (1.0f / (float)CONTROL_FREQUENCY);
dv_fb = AKM_YAW_KP * e_r + AKM_YAW_KI * yaw_integral;
dv = dv_ff + dv_fb;
// Clamp the differential to a fraction of Vx and anti-windup: if the
// PI part alone saturates, roll the integrator back.
dv_max = AKM_YAW_MAX_DIFF_RATIO * float_abs(Vx);
if(dv > dv_max)
{
if(AKM_YAW_KI > 0.0f)
yaw_integral -= (dv - dv_max) / AKM_YAW_KI;
dv = dv_max;
}
else if(dv < -dv_max)
{
if(AKM_YAW_KI > 0.0f)
yaw_integral -= (dv + dv_max) / AKM_YAW_KI;
dv = -dv_max;
}
}
// r_ref > 0 (left) means the left wheel is inner (slower). This matches
// Mode 0's MOTOR_A = Vx*(1 + 0.5*track*kappa_fit) once dv_ff is expanded,
// because kappa_fit = -r_ref/Vx.
MOTOR_A.Target = Vx - dv; // left
MOTOR_B.Target = Vx + dv; // right
MOTOR_C.Target = 0;
MOTOR_D.Target = 0;
#else
// Inputs: Vx = rear-axle-center linear speed [m/s],
// Vz = rotation speed wz about the turn center [rad/s].
// Both refer to the rear-axle center -- exactly the point the servo
// calibration measured R against -- so the geometric curvature is
// kappa_geom = wz / v = Vz / Vx = 1/R_rear [1/m]
// with the ROS sign convention: Vz > 0 = CCW = left turn.
// The calibration table / servo fit use the opposite sign (kappa > 0
// = right turn), so we negate to get the fit-domain curvature:
// kappa_fit = -kappa_geom -> Vz > 0 gives kappa_fit < 0 = left.
float kappa_geom, kappa_fit;
if(float_abs(Vx) > 0.001f)
/* Vx sign is drive direction, not steering direction. */
kappa_geom = Vz / float_abs(Vx);
else
kappa_geom = 0.0f; // Ackermann geometry cannot steer without forward motion.
kappa_fit = target_limit_float(-kappa_geom, -AKM_KAPPA_MAX, AKM_KAPPA_MAX);
// Rear-wheel differential about the rear-axle center, expressed in the
// fit-domain curvature. On a right turn (kappa_fit > 0) the turn center
// is to the right, so the right wheel is inner (slower) and the left
// wheel is outer (faster):
// MOTOR_A (left) = Vx*(1 + 0.5*track*kappa_fit)
// MOTOR_B (right) = Vx*(1 - 0.5*track*kappa_fit)
MOTOR_A.Target = Vx * (1.0f + 0.5f * Wheel_spacing * kappa_fit);
MOTOR_B.Target = Vx * (1.0f - 0.5f * Wheel_spacing * kappa_fit);
// The PWM value of the servo controls the steering Angle of the front wheel
//<2F><><EFBFBD>PWMֵ<4D><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0>ת<EFBFBD><D7AA>Ƕ<EFBFBD>
Servo=Akm_Curvature_To_Servo(kappa_fit);
#endif /* AKM_DIRECT_MAP */
// Servo source override: when enabled, the remote CH1 drives the servo
// directly, taking priority over BOTH control laws above. The remote
// neutral (AKM_REMOTER_CH1_MID) is shifted onto the servo straight-ahead
// neutral (SERVO_INIT) so stick-center = wheels-straight.
#if AKM_SERVO_DEBUG_REMOTE_CH1
Servo = Remoter_Ch1 + (SERVO_INIT - AKM_REMOTER_CH1_MID);
#endif
//Wheel (motor) target speed limit //<2F><><EFBFBD><EFBFBD>(<28><><EFBFBD>)Ŀ<><C4BF><EFBFBD>ٶ<EFBFBD><D9B6>޷<EFBFBD>
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=0; //Out of use //û<><C3BB>ʹ<EFBFBD>õ<EFBFBD>
MOTOR_D.Target=0; //Out of use //û<><C3BB>ʹ<EFBFBD>õ<EFBFBD>
Servo=target_limit_int(Servo,AKM_SERVO_MIN,AKM_SERVO_MAX); //Servo PWM value limit //PWMֵ޷
}
//Differential car
//<2F><><EFBFBD><EFBFBD>С<EFBFBD><D0A1>
else if (Car_Mode==Diff_Car)
{
//Inverse kinematics //<2F>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD>
MOTOR_A.Target = Vx - Vz * Wheel_spacing / 2.0f; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD>
MOTOR_B.Target = Vx + Vz * Wheel_spacing / 2.0f; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD>
//Wheel (motor) target speed limit //<2F><><EFBFBD><EFBFBD>(<28><><EFBFBD>)Ŀ<><C4BF><EFBFBD>ٶ<EFBFBD><D9B6>޷<EFBFBD>
MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=0; //Out of use //û<><C3BB>ʹ<EFBFBD>õ<EFBFBD>
MOTOR_D.Target=0; //Out of use //û<><C3BB>ʹ<EFBFBD>õ<EFBFBD>
}
//FourWheel car
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else if(Car_Mode==FourWheel_Car)
{
//Inverse kinematics //<2F>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD>
MOTOR_A.Target = Vx - Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD>
MOTOR_B.Target = Vx - Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD>
MOTOR_C.Target = Vx + Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD>
MOTOR_D.Target = Vx + Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD>
//Wheel (motor) target speed limit //<2F><><EFBFBD><EFBFBD>(<28><><EFBFBD>)Ŀ<><C4BF><EFBFBD>ٶ<EFBFBD><D9B6>޷<EFBFBD>
MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=target_limit_float( MOTOR_C.Target,-amplitude,amplitude);
MOTOR_D.Target=target_limit_float( MOTOR_D.Target,-amplitude,amplitude);
}
//Tank Car
//<2F>Ĵ<EFBFBD><C4B4><EFBFBD>
else if (Car_Mode==Tank_Car)
{
//Inverse kinematics //<2F>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD>
MOTOR_A.Target = Vx - Vz * (Wheel_spacing) / 2.0f; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD>
MOTOR_B.Target = Vx + Vz * (Wheel_spacing) / 2.0f; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD>
//Wheel (motor) target speed limit //<2F><><EFBFBD><EFBFBD>(<28><><EFBFBD>)Ŀ<><C4BF><EFBFBD>ٶ<EFBFBD><D9B6>޷<EFBFBD>
MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=0; //Out of use //û<><C3BB>ʹ<EFBFBD>õ<EFBFBD>
MOTOR_D.Target=0; //Out of use //û<><C3BB>ʹ<EFBFBD>õ<EFBFBD>
}
}
/**************************************************************************
Function: FreerTOS task, core motion control task
Input : none
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>FreeRTOS<EFBFBD><EFBFBD><EFBFBD>񣬺<EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void Balance_task(void *pvParameters)
{
u32 lastWakeTime = getSysTickCnt();
u32 lastControlTime = lastWakeTime;
while(1)
{
u32 currentControlTime;
float controlDt;
// Run the wheel-speed control loop at 200 Hz.
vTaskDelayUntil(&lastWakeTime, F2T(RATE_200_HZ));
currentControlTime = getSysTickCnt();
controlDt = (float)(currentControlTime - lastControlTime) /
(float)configTICK_RATE_HZ;
lastControlTime = currentControlTime;
// Avoid a large integral jump if the task is stalled while debugging.
controlDt = target_limit_float(controlDt, 0.001f, 0.050f);
//Time count is no longer needed after 30 seconds
//ʱ<><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>30<33><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ
if(Time_count<(30*CONTROL_FREQUENCY))Time_count++;
//Get the encoder data, that is, the real time wheel speed,
//and convert to transposition international units
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵʱ<CAB5>ٶȣ<D9B6><C8A3><EFBFBD>ת<EFBFBD><D7AA>λ<EFBFBD><CEBB><EFBFBD>ʵ<EFBFBD>λ
Get_Velocity_Form_Encoder();
if(Check==0) //If self-check mode is not enabled //<2F><><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD>ģʽ
{
// command_lost_count++; //<2F><><EFBFBD>ڡ<EFBFBD>CAN<41><4E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧʱ<CAA7><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ1<CAA7><31><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
// if(command_lost_count>RATE_100_HZ && APP_ON_Flag==0 && Remote_ON_Flag==0 && PS2_ON_Flag==0) //<2F><><EFBFBD><EFBFBD>APP<50><50>PS2<53><32><EFBFBD><EFBFBD>ģң<C4A3><D2A3>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>CAN<41><4E><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD>ģʽ
// Move_X=0, Move_Y=0, Move_Z=0;
if (APP_ON_Flag) Get_RC(); //Handle the APP remote commands //<2F><><EFBFBD><EFBFBD>APPң<50><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else if (Remote_ON_Flag) Remote_Control(); //Handle model aircraft remote commands //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģң<C4A3><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else if (PS2_ON_Flag) PS2_control(); //Handle PS2 controller commands //<2F><><EFBFBD><EFBFBD>PS2<53>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//CAN, Usart 1, Usart 3, Uart5 control can directly get the three axis target speed,
//without additional processing
//CAN<41><4E><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3(ROS)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>5<EFBFBD><35><EFBFBD><EFBFBD>ֱ<EFBFBD>ӵõ<D3B5><C3B5><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⴦<EFBFBD><E2B4A6>
else Drive_Motor(Move_X, Move_Y, Move_Z);
//Click the user button to update the gyroscope zero
//<2F><><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Key();
//If there is no abnormity in the battery voltage, and the enable switch is in the ON position,
//and the software failure flag is 0
//<2F><><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD>ܿ<EFBFBD><DCBF><EFBFBD><EFBFBD><EFBFBD>ON<4F><4E>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>ܱ<EFBFBD>־λΪ0
if(Turn_Off(Voltage)==0)
{
//Speed closed-loop control to calculate the PWM value of each motor,
//PWM represents the actual wheel speed
//<2F>ٶȱջ<C8B1><D5BB><EFBFBD><EFBFBD>Ƽ<EFBFBD><C6BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWMֵ<4D><D6B5>PWM<57><4D><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>ת<EFBFBD><D7AA>
MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target, controlDt);
MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target, controlDt);
MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target, controlDt);
MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target, controlDt);
Limit_Pwm(16700);
//Set different PWM control polarity according to different car models
//<2F><><EFBFBD>ݲ<EFBFBD>ͬС<CDAC><D0A1><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD>ò<EFBFBD>ͬ<EFBFBD><CDAC>PWM<57><4D><EFBFBD>Ƽ<EFBFBD><C6BC><EFBFBD>
switch(Car_Mode)
{
case Mec_Car: Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Mecanum wheel car //<2F><><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><C4B7>С<EFBFBD><D0A1>
case Omni_Car: Set_Pwm(-MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Omni car //ȫ<><C8AB><EFBFBD><EFBFBD>С<EFBFBD><D0A1>
case Akm_Car: Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, 16799,-16799 , Servo); break; //Ackermann structure car //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>
case Diff_Car: Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Differential car //<2F><><EFBFBD>ֲ<EFBFBD><D6B2><EFBFBD>С<EFBFBD><D0A1>
case FourWheel_Car: Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //FourWheel car //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case Tank_Car: Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Tank Car //<2F>Ĵ<EFBFBD><C4B4><EFBFBD>
}
}
//If Turn_Off(Voltage) returns to 1, the car is not allowed to move, and the PWM value is set to 0
//<2F><><EFBFBD>Turn_Off(Voltage)<29><><EFBFBD><EFBFBD>ֵΪ1<CEAA><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD>PWMֵ<4D><D6B5><EFBFBD><EFBFBD>Ϊ0
else Set_Pwm(0,0,0,0,(Car_Mode == Akm_Car) ? SERVO_INIT : 0);
}
}
}
/**************************************************************************
Function: Assign a value to the PWM register to control wheel speed and direction
Input : PWM
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>PWM<EFBFBD>Ĵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƴ<EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD>뷽<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWM
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo)
{
//Forward and reverse control of motor
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
if(motor_a<0) PWMA1=16799,PWMA2=16799+motor_a;
else PWMA2=16799,PWMA1=16799-motor_a;
//Forward and reverse control of motor
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
if(motor_b<0) PWMB1=16799,PWMB2=16799+motor_b;
else PWMB2=16799,PWMB1=16799-motor_b;
// PWMB1=10000,PWMB2=5000;
//Forward and reverse control of motor
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
if(motor_c<0) PWMC1=16799,PWMC2=16799+motor_c;
else PWMC2=16799,PWMC1=16799-motor_c;
//Forward and reverse control of motor
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
if(motor_d<0) PWMD1=16799,PWMD2=16799+motor_d;
else PWMD2=16799,PWMD1=16799-motor_d;
//Servo control
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Servo_PWM =servo;
}
/**************************************************************************
Function: Limit PWM value
Input : Value
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWMֵ
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void Limit_Pwm(int amplitude)
{
MOTOR_A.Motor_Pwm=target_limit_float(MOTOR_A.Motor_Pwm,-amplitude,amplitude);
MOTOR_B.Motor_Pwm=target_limit_float(MOTOR_B.Motor_Pwm,-amplitude,amplitude);
MOTOR_C.Motor_Pwm=target_limit_float(MOTOR_C.Motor_Pwm,-amplitude,amplitude);
MOTOR_D.Motor_Pwm=target_limit_float(MOTOR_D.Motor_Pwm,-amplitude,amplitude);
}
/**************************************************************************
Function: Limiting function
Input : Value
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD>޷<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
float target_limit_float(float insert,float low,float high)
{
if (insert < low)
return low;
else if (insert > high)
return high;
else
return insert;
}
int target_limit_int(int insert,int low,int high)
{
if (insert < low)
return low;
else if (insert > high)
return high;
else
return insert;
}
/**************************************************************************
Function: Check the battery voltage, enable switch status, software failure flag status
Input : Voltage
Output : Whether control is allowed, 1: not allowed, 0 allowed
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD>ѹ<EFBFBD><EFBFBD>ʹ<EFBFBD>ܿ<EFBFBD><EFBFBD><EFBFBD>״̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>ܱ<EFBFBD>־λ״̬
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹ
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>
**************************************************************************/
u8 Turn_Off( int voltage)
{
u8 temp;
if(voltage<10||EN==0||Flag_Stop==1)
{
temp=1;
PWMA1=0;PWMA2=0;
PWMB1=0;PWMB2=0;
PWMC1=0;PWMC1=0;
PWMD1=0;PWMD2=0;
}
else
temp=0;
return temp;
}
/**************************************************************************
Function: Calculate absolute value
Input : long int
Output : unsigned int
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>long int
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5>unsigned int
**************************************************************************/
u32 myabs(long int a)
{
u32 temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
Function: Incremental PI controller
Input : Encoder measured value (actual speed), target speed
Output : Motor PWM
According to the incremental discrete PID formula
pwm+=Kp[e<><65>k<EFBFBD><6B>-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k) represents the current deviation
e(k-1) is the last deviation and so on
PWM stands for incremental output
In our speed control closed loop system, only PI control is used
pwm+=Kp[e<><65>k<EFBFBD><6B>-e(k-1)]+Ki*e(k)*dt
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽPI<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ(ʵ<><CAB5><EFBFBD>ٶ<EFBFBD>)<29><>Ŀ<EFBFBD><C4BF><EFBFBD>ٶ<EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD><EFBFBD>PWM
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD>ɢPID<EFBFBD><EFBFBD>ʽ
pwm+=Kp[e<><65>k<EFBFBD><6B>-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
e(k-1)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ε<EFBFBD>ƫ<EFBFBD><C6AB> <20>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD><EFBFBD>
pwm<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><EFBFBD>ٶȿ<EFBFBD><EFBFBD>Ʊջ<EFBFBD>ϵͳ<EFBFBD><EFBFBD><EFBFBD>棬ֻʹ<EFBFBD><EFBFBD>PI<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pwm+=Kp[e<><65>k<EFBFBD><6B>-e(k-1)]+Ki*e(k)*dt
**************************************************************************/
int Incremental_PI_A (float Encoder,float Target,float dt)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //Calculate the deviation //<2F><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias; //Save the last deviation //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƫ<EFBFBD><C6AB>
return Pwm;
}
int Incremental_PI_B (float Encoder,float Target,float dt)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //Calculate the deviation //<2F><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias; //Save the last deviation //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƫ<EFBFBD><C6AB>
return Pwm;
}
int Incremental_PI_C (float Encoder,float Target,float dt)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //Calculate the deviation //<2F><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias; //Save the last deviation //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƫ<EFBFBD><C6AB>
return Pwm;
}
int Incremental_PI_D (float Encoder,float Target,float dt)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //Calculate the deviation //<2F><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias; //Save the last deviation //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƫ<EFBFBD><C6AB>
return Pwm;
}
/**************************************************************************
Function: Processes the command sent by APP through usart 2
Input : none
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>APPͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><EFBFBD><EFBFBD>͹<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void Get_RC(void)
{
u8 Flag_Move=1;
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car) //The omnidirectional wheel moving trolley can move laterally //ȫ<><C8AB><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD>к<EFBFBD><D0BA><EFBFBD><EFBFBD>ƶ<EFBFBD>
{
switch(Flag_Direction) //Handle direction control commands //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
case 1: Move_X=RC_Velocity; Move_Y=0; Flag_Move=1; break;
case 2: Move_X=RC_Velocity; Move_Y=-RC_Velocity; Flag_Move=1; break;
case 3: Move_X=0; Move_Y=-RC_Velocity; Flag_Move=1; break;
case 4: Move_X=-RC_Velocity; Move_Y=-RC_Velocity; Flag_Move=1; break;
case 5: Move_X=-RC_Velocity; Move_Y=0; Flag_Move=1; break;
case 6: Move_X=-RC_Velocity; Move_Y=RC_Velocity; Flag_Move=1; break;
case 7: Move_X=0; Move_Y=RC_Velocity; Flag_Move=1; break;
case 8: Move_X=RC_Velocity; Move_Y=RC_Velocity; Flag_Move=1; break;
default: Move_X=0; Move_Y=0; Flag_Move=0; break;
}
if(Flag_Move==0)
{
//If no direction control instruction is available, check the steering control status
//<2F><><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EEA3AC><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>״̬
if (Flag_Left ==1) Move_Z= PI/2*(RC_Velocity/500); //left rotation //<2F><><EFBFBD><EFBFBD>ת
else if(Flag_Right==1) Move_Z=-PI/2*(RC_Velocity/500); //right rotation //<2F><><EFBFBD><EFBFBD>ת
else Move_Z=0; //stop //ֹͣ
}
}
else //Non-omnidirectional moving trolley //<2F><>ȫ<EFBFBD><C8AB><EFBFBD>ƶ<EFBFBD>С<EFBFBD><D0A1>
{
switch(Flag_Direction) //Handle direction control commands //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
case 1: Move_X=+RC_Velocity; Move_Z=0; break;
case 2: Move_X=+RC_Velocity; Move_Z=-PI/2; break;
case 3: Move_X=0; Move_Z=-PI/2; break;
case 4: Move_X=-RC_Velocity; Move_Z=-PI/2; break;
case 5: Move_X=-RC_Velocity; Move_Z=0; break;
case 6: Move_X=-RC_Velocity; Move_Z=+PI/2; break;
case 7: Move_X=0; Move_Z=+PI/2; break;
case 8: Move_X=+RC_Velocity; Move_Z=+PI/2; break;
default: Move_X=0; Move_Z=0; break;
}
if (Flag_Left ==1) Move_Z= PI/2; //left rotation //<2F><><EFBFBD><EFBFBD>ת
else if(Flag_Right==1) Move_Z=-PI/2; //right rotation //<2F><><EFBFBD><EFBFBD>ת
}
//Z-axis data conversion //Z<><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
if(Car_Mode==Akm_Car)
{
//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹС<E1B9B9><D0A1>ת<EFBFBD><D7AA>Ϊǰ<CEAA><C7B0>ת<EFBFBD><D7AA>Ƕ<EFBFBD>
Move_Z=Move_Z*2/9;
}
else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
{
if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //<2F><><EFBFBD>ٿ<EFBFBD><D9BF><EFBFBD>ԭ<EFBFBD><D4AD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>Ҫ<EFBFBD>˴<EFBFBD><CBB4><EFBFBD>
Move_Z=Move_Z*RC_Velocity/500;
}
//Unit conversion, mm/s -> m/s
//<2F><>λת<CEBB><D7AA><EFBFBD><EFBFBD>mm/s -> m/s
Move_X=Move_X/1000; Move_Y=Move_Y/1000; Move_Z=Move_Z;
//Control target value is obtained and kinematics analysis is performed
//<2F>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD>
Drive_Motor(Move_X,Move_Y,Move_Z);
}
/**************************************************************************
Function: Handle PS2 controller control commands
Input : none
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>PS2<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void PS2_control(void)
{
int LX,LY,RY;
int Threshold=20; //Threshold to ignore small movements of the joystick //<2F><>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҡ<EFBFBD><D2A1>С<EFBFBD><D0A1><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD>
//128 is the median.The definition of X and Y in the PS2 coordinate system is different from that in the ROS coordinate system
//128Ϊ<38><CEAA>ֵ<EFBFBD><D6B5>PS2<53><32><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>ROS<4F><53><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>X<EFBFBD><58>Y<EFBFBD>Ķ<EFBFBD><C4B6>岻һ<E5B2BB><D2BB>
LY=-(PS2_LX-128);
LX=-(PS2_LY-128);
RY=-(PS2_RX-128);
//Ignore small movements of the joystick //<2F><><EFBFBD><EFBFBD>ҡ<EFBFBD><D2A1>С<EFBFBD><D0A1><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD>
if(LX>-Threshold&&LX<Threshold)LX=0;
if(LY>-Threshold&&LY<Threshold)LY=0;
if(RY>-Threshold&&RY<Threshold)RY=0;
if (PS2_KEY==11) RC_Velocity+=5; //To accelerate//<2F><><EFBFBD><EFBFBD>
else if(PS2_KEY==9) RC_Velocity-=5; //To slow down //<2F><><EFBFBD><EFBFBD>
if(RC_Velocity<0) RC_Velocity=0;
//Handle PS2 controller control commands
//<2F><>PS2<53>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>
Move_X=LX*RC_Velocity/128;
Move_Y=LY*RC_Velocity/128;
Move_Z=RY*(PI/2)/128;
//Z-axis data conversion //Z<><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
{
Move_Z=Move_Z*RC_Velocity/500;
}
else if(Car_Mode==Akm_Car)
{
//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹС<E1B9B9><D0A1>ת<EFBFBD><D7AA>Ϊǰ<CEAA><C7B0>ת<EFBFBD><D7AA>Ƕ<EFBFBD>
Move_Z=Move_Z*2/9;
}
else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
{
if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //<2F><><EFBFBD>ٿ<EFBFBD><D9BF><EFBFBD>ԭ<EFBFBD><D4AD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>Ҫ<EFBFBD>˴<EFBFBD><CBB4><EFBFBD>
Move_Z=Move_Z*RC_Velocity/500;
}
//Unit conversion, mm/s -> m/s
//<2F><>λת<CEBB><D7AA><EFBFBD><EFBFBD>mm/s -> m/s
Move_X=Move_X/1000;
Move_Y=Move_Y/1000;
Move_Z=Move_Z;
//Control target value is obtained and kinematics analysis is performed
//<2F>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD>
Drive_Motor(Move_X,Move_Y,Move_Z);
}
/**************************************************************************
Function: The remote control command of model aircraft is processed
Input : none
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD>Ժ<EFBFBD>ģң<EFBFBD>ؿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void Remote_Control(void)
{
//Data within 1 second after entering the model control mode will not be processed
//<2F>Խ<EFBFBD><D4BD>뺽ģ<EBBABD><C4A3><EFBFBD><EFBFBD>ģʽ<C4A3><CABD>1<EFBFBD><31><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD>
static u8 thrice=CONTROL_FREQUENCY;
int Threshold=100; //Threshold to ignore small movements of the joystick //<2F><>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҡ<EFBFBD><D2A1>С<EFBFBD><D0A1><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD>
//limiter //<2F>޷<EFBFBD>
int LX,LY,RY,RX,Remote_RCvelocity;
Remoter_Ch1=target_limit_int(Remoter_Ch1,1000,2000);
Remoter_Ch2=target_limit_int(Remoter_Ch2,1000,2000);
Remoter_Ch3=target_limit_int(Remoter_Ch3,1000,2000);
Remoter_Ch4=target_limit_int(Remoter_Ch4,1000,2000);
// Front and back direction of left rocker. Control forward and backward.
//<2F><>ҡ<EFBFBD><D2A1>ǰ<EFBFBD><C7B0><EFBFBD>򡣿<EFBFBD><F2A1A3BF><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ˡ<EFBFBD>
LX=Remoter_Ch2-1500;
//Left joystick left and right.Control left and right movement. Only the wheelie omnidirectional wheelie will use the channel.
//Ackerman trolleys use this channel as a PWM output to control the steering gear
//<2F><>ҡ<EFBFBD><D2A1><EFBFBD><EFBFBD><EFBFBD>ҷ<EFBFBD><D2B7>򡣿<EFBFBD><F2A1A3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB><EFBFBD>ֲŻ<D6B2>ʹ<EFBFBD>õ<EFBFBD><C3B5><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>ʹ<EFBFBD>ø<EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD>ΪPWM<57><4D><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6>
LY=Remoter_Ch4-1500;
//Front and back direction of right rocker. Throttle/acceleration/deceleration.
//<2F><>ҡ<EFBFBD><D2A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>/<2F>Ӽ<EFBFBD><D3BC>١<EFBFBD>
RX=Remoter_Ch3-1500;
//Right stick left and right. To control the rotation.
//<2F><>ҡ<EFBFBD><D2A1><EFBFBD><EFBFBD><EFBFBD>ҷ<EFBFBD><D2B7>򡣿<EFBFBD><F2A1A3BF><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
RY=Remoter_Ch1-1500;
if(LX>-Threshold&&LX<Threshold)LX=0;
if(LY>-Threshold&&LY<Threshold)LY=0;
if(RX>-Threshold&&RX<Threshold)RX=0;
if(RY>-Threshold&&RY<Threshold)RY=0;
//Throttle related //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Remote_RCvelocity=RC_Velocity+RX;
if(Remote_RCvelocity<0)Remote_RCvelocity=0;
//The remote control command of model aircraft is processed
//<2F>Ժ<EFBFBD>ģң<C4A3>ؿ<EFBFBD><D8BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>
Move_X= LX*Remote_RCvelocity/500;
Move_Y=-LY*Remote_RCvelocity/500;
Move_Z=-RY*(PI/2)/500;
//Z<><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
{
Move_Z=Move_Z*Remote_RCvelocity/500;
}
else if(Car_Mode==Akm_Car)
{
//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹС<E1B9B9><D0A1>ת<EFBFBD><D7AA>Ϊǰ<CEAA><C7B0>ת<EFBFBD><D7AA>Ƕ<EFBFBD>
Move_Z=Move_Z*2/9;
}
else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
{
if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //<2F><><EFBFBD>ٿ<EFBFBD><D9BF><EFBFBD>ԭ<EFBFBD><D4AD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>Ҫ<EFBFBD>˴<EFBFBD><CBB4><EFBFBD>
Move_Z=Move_Z*Remote_RCvelocity/500;
}
//Unit conversion, mm/s -> m/s
//<2F><>λת<CEBB><D7AA><EFBFBD><EFBFBD>mm/s -> m/s
Move_X=Move_X/1000;
Move_Y=Move_Y/1000;
Move_Z=Move_Z;
//Data within 1 second after entering the model control mode will not be processed
//<2F>Խ<EFBFBD><D4BD>뺽ģ<EBBABD><C4A3><EFBFBD><EFBFBD>ģʽ<C4A3><CABD>1<EFBFBD><31><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD>
if(thrice>0) Move_X=0,Move_Z=0,thrice--;
//Control target value is obtained and kinematics analysis is performed
//<2F>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD>
Drive_Motor(Move_X,Move_Y,Move_Z);
}
/**************************************************************************
Function: Click the user button to update gyroscope zero
Input : none
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void Key(void)
{
u8 tmp;
tmp=click_N_Double_MPU6050(50);
if(tmp==2)memcpy(Deviation_gyro,Original_gyro,sizeof(gyro)),memcpy(Deviation_accel,Original_accel,sizeof(accel));
}
/**************************************************************************
Function: Read the encoder value and calculate the wheel speed, unit m/s
Input : none
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㳵<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD>λm/s
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void Get_Velocity_Form_Encoder(void)
{
//Retrieves the original data of the encoder
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr;
//Stamp the moment the encoder counters are latched, as close to the
//real sample instant as possible.
//<2F>ڶ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD>ɼ<EFBFBD>ʱ<EFBFBD>̡<EFBFBD>
g_speed_sample_time_us = mcu_time_us();
OriginalEncoder.A=Read_Encoder(2);
OriginalEncoder.B=Read_Encoder(3);
OriginalEncoder.C=Read_Encoder(4);
OriginalEncoder.D=Read_Encoder(5);
//test_num=OriginalEncoder.B;
//Decide the encoder numerical polarity according to different car models
//<2F><><EFBFBD>ݲ<EFBFBD>ͬС<CDAC><D0A1><EFBFBD>ͺž<CDBA><C5BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
switch(Car_Mode)
{
case Mec_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C; Encoder_D_pr=-OriginalEncoder.D; break;
case Omni_Car: Encoder_A_pr=-OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C; Encoder_D_pr=-OriginalEncoder.D; break;
case Akm_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break;
case Diff_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break;
case FourWheel_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C; Encoder_D_pr=-OriginalEncoder.D; break;
case Tank_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break;
}
//The encoder converts the raw data to wheel speed in m/s
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD>λm/s
MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_D.Encoder= Encoder_D_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
}
/**************************************************************************
Function: Smoothing the three axis target velocity
Input : Three-axis target velocity
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void Smooth_control(float vx,float vy,float vz)
{
float step=0.01;
if (vx>0) smooth_control.VX+=step;
else if(vx<0) smooth_control.VX-=step;
else if(vx==0) smooth_control.VX=smooth_control.VX*0.9f;
if (vy>0) smooth_control.VY+=step;
else if(vy<0) smooth_control.VY-=step;
else if(vy==0) smooth_control.VY=smooth_control.VY*0.9f;
if (vz>0) smooth_control.VZ+=step;
else if(vz<0) smooth_control.VZ-=step;
else if(vz==0) smooth_control.VZ=smooth_control.VZ*0.9f;
smooth_control.VX=target_limit_float(smooth_control.VX,-float_abs(vx),float_abs(vx));
smooth_control.VY=target_limit_float(smooth_control.VY,-float_abs(vy),float_abs(vy));
smooth_control.VZ=target_limit_float(smooth_control.VZ,-float_abs(vz),float_abs(vz));
}
/**************************************************************************
Function: Floating-point data calculates the absolute value
Input : float
Output : The absolute value of the input number
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>ֵ
**************************************************************************/
float float_abs(float insert)
{
if(insert>=0) return insert;
else return -insert;
}
/**************************************************************************
Function: Prevent the potentiometer to choose the wrong mode, resulting in initialization error caused by the motor spinning.Out of service
Input : none
Output : none
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD>ģʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>³<EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣʹ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> ֵ<><D6B5><EFBFBD><EFBFBD>
**************************************************************************/
void robot_mode_check(void)
{
static u8 error=0;
if(abs(MOTOR_A.Motor_Pwm)>2500||abs(MOTOR_B.Motor_Pwm)>2500||abs(MOTOR_C.Motor_Pwm)>2500||abs(MOTOR_D.Motor_Pwm)>2500) error++;
//If the output is close to full amplitude for 6 times in a row, it is judged that the motor rotates wildly and makes the motor incapacitated
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>6<EFBFBD>νӽ<CEBD><D3BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>õ<EFBFBD><C3B5>ʧ<EFBFBD><CAA7>
if(error>6) EN=0,Flag_Stop=1,robot_mode_check_flag=1;
}
Reference in New Issue View Git Blame Copy Permalink