#include "control.h"
#include "Lidar.h"
float Move_X = 0, Move_Z = 0; // Ä¿±êËٶȺÍÄ¿±êתÏòËÙ¶È
float PWM_Left, PWM_Right; // ×óÓÒµç»úPWMÖµ
float RC_Velocity, RC_Turn_Velocity; // Ò£¿Ø¿ØÖƵÄËÙ¶È
u8 Mode = Normal_Mode; // ģʽѡÔñ£¬Ä¬ÈÏÊÇÆÕͨµÄ¿ØÖÆģʽ
Motor_parameter MotorA, MotorB; // ×óÓÒµç»úÏà¹Ø±äÁ¿
int Servo_PWM = SERVO_INIT; // °¢¿ËÂü¶æ»úÏà¹Ø±äÁ¿
u8 Lidar_Detect = Lidar_Detect_ON; // µç´ÅѲÏßģʽÀ×´ï¼ì²âÕÏ°ÎĬÈÏ¿ªÆô
float CCD_Move_X = 0.3; // CCDѲÏßËÙ¶È
float ELE_Move_X = 0.3; // µç´ÅѲÏßËÙ¶È
u8 Ros_count = 0, Lidar_flag_count = 0;
u8 cnt = 0;
Encoder OriginalEncoder; // Encoder raw data //±àÂëÆ÷ÔʼÊý¾Ý
short Accel_Y, Accel_Z, Accel_X, Accel_Angle_x, Accel_Angle_y, Gyro_X, Gyro_Z, Gyro_Y;
int forward_cnt = 800;
int left_cnt = 120;
int right_cnt = 125;
int stop_cnt = 200;
int stop_flag = 0;
int mode_cnt = 0;
int state_cnt = 0;
int stop_protect = 0;
/**************************************************************************
Function: Control Function
Input : none
Output : none
º¯Êý¹¦ÄÜ£º5ms¶¨Ê±ÖжϿØÖƺ¯Êý
Èë¿Ú²ÎÊý: ÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void forward(){
MotorA.Motor_Pwm = 1999;
MotorB.Motor_Pwm = 2040;
}
void left(){
MotorA.Motor_Pwm = 0;
MotorB.Motor_Pwm = 2050;
}
void right(){
MotorA.Motor_Pwm = 1999;
MotorB.Motor_Pwm = 0;
}
void stop(){
MotorA.Motor_Pwm = 0;
MotorB.Motor_Pwm = 0;
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim == &htim5)
{
Get_KeyVal();
if(mode_cnt == 0)
stop();
//ģʽһ
if(mode_cnt == 1){
if(forward_cnt > 0){
forward();
forward_cnt--;
// if(left_cnt > 0){
// left();
// left_cnt--;
// if(right_cnt > 0){
// right();
// right_cnt--;
}
else{
stop();
}
}
//ģʽ¶þ
else if(mode_cnt == 2){
if(forward_cnt > 0){
forward();
forward_cnt--;
}
else if(stop_cnt > 0 && forward_cnt == 0){
stop();
stop_cnt--;
}
else if(stop_cnt == 0 && stop_flag == 0){
forward_cnt = 800;
stop_flag = 1;
}
else{
stop();
}
}
//ģʽÈý
else if(mode_cnt == 3){
if(forward_cnt > 0 && state_cnt == 0){//µÚ1״̬£ºÖ±ÐÐ
forward();
forward_cnt--;
}
else if(right_cnt > 0 && state_cnt == 1){//µÚ2״̬£ºÓÒת
right();
right_cnt--;
}
else if(forward_cnt > 0 && state_cnt == 2){//µÚ3״̬£ºÖ±ÐÐ
forward();
forward_cnt--;
}
else if(left_cnt > 0 && state_cnt == 3){//µÚ4״̬£º×óת
left();
left_cnt--;
}
else if(forward_cnt > 0 && state_cnt == 4){//µÚ5״̬£ºÖ±ÐÐ
forward();
forward_cnt--;
}
else if(left_cnt > 0 && state_cnt == 5){//µÚ6״̬£º×óת
left();
left_cnt--;
}
else if(forward_cnt > 0 && state_cnt == 6){//µÚ7״̬£ºÖ±ÐÐ
forward();
forward_cnt--;
}
else{
if(stop_protect == 0 && stop_cnt == 0){
stop_cnt = 100;
stop_protect = 1;
}
else if(stop_cnt > 0){
stop();
stop_cnt--;
}
else if(stop_protect == 1 && stop_cnt == 0){
stop_protect = 0;
if(state_cnt == 0){//״̬1Çл»×´Ì¬2
state_cnt++;
right_cnt = 125;
}
else if(state_cnt == 1){//״̬2Çл»×´Ì¬3
state_cnt++;
forward_cnt = 585;
}
else if(state_cnt == 2){//״̬3Çл»×´Ì¬4
state_cnt++;
left_cnt = 120;
}
else if(state_cnt == 3){//״̬4Çл»×´Ì¬5
state_cnt++;
forward_cnt = 585;
}
else if(state_cnt == 4){//״̬5Çл»×´Ì¬6
state_cnt++;
left_cnt = 120;
}
else if(state_cnt == 5){//״̬6Çл»×´Ì¬7
state_cnt++;
forward_cnt = 585;
}
}
}
}
Set_Pwm(-MotorA.Motor_Pwm, MotorB.Motor_Pwm); // Çý¶¯µç»ú
}
}
/**************************************************************************
Function: Bluetooth_Control
Input : none
Output : none
º¯Êý¹¦ÄÜ£ºÊÖ»úÀ¶ÑÀ¿ØÖÆ
Èë¿Ú²ÎÊý: ÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void Bluetooth_Control(void)
{
if (Flag_Direction == 0)
Move_X = 0, Move_Z = 0; // ֹͣ
else if (Flag_Direction == 1)
Move_X = RC_Velocity, Move_Z = 0; // Ç°½ø
else if (Flag_Direction == 2)
Move_X = RC_Velocity, Move_Z = Pi / 2; // ÓÒÇ°
else if (Flag_Direction == 3)
Move_X = 0, Move_Z = Pi / 2; // ÏòÓÒ
else if (Flag_Direction == 4)
Move_X = -RC_Velocity, Move_Z = Pi / 2; // ÓÒºó
else if (Flag_Direction == 5)
Move_X = -RC_Velocity, Move_Z = 0; // ºóÍË
else if (Flag_Direction == 6)
Move_X = -RC_Velocity, Move_Z = -Pi / 2; // ×óºó
else if (Flag_Direction == 7)
Move_X = 0, Move_Z = -Pi / 2; // Ïò×ó
else if (Flag_Direction == 8)
Move_X = RC_Velocity, Move_Z = -Pi / 2; // ×óÇ°
else
Move_X = 0, Move_Z = 0;
if (Car_Num == Akm_Car)
{
// Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
// °¢¿ËÂü½á¹¹Ð¡³µ×ª»»ÎªÇ°ÂÖתÏò½Ç¶È
Move_Z = Move_Z * 2 / 10;
}
Move_X = Move_X / 1000;
Move_Z = -Move_Z; // ת»»ÎªËÙ¶ÈתΪm/s
}
/**************************************************************************
Function: PS2_Control
Input : none
Output : none
º¯Êý¹¦ÄÜ£ºPS2ÊÖ±ú¿ØÖÆ
Èë¿Ú²ÎÊý: ÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void PS2_Control(void)
{
int LY, RX; // ÊÖ±úADCµÄÖµ
int Threshold = 20; // ãÐÖµ£¬ºöÂÔÒ¡¸ËС·ù¶È¶¯×÷
static u8 Key1_Count = 0, Key2_Count = 0; // ÓÃÓÚ¿ØÖƶÁÈ¡Ò¡¸ËµÄËÙ¶È
// ת»¯Îª128µ½-128µÄÊýÖµ
LY = -(PS2_LY - 128); // ×ó±ßYÖá¿ØÖÆÇ°½øºóÍË
RX = -(PS2_RX - 128); // ÓÒ±ßXÖá¿ØÖÆתÏò
if (LY > -Threshold && LY < Threshold)
LY = 0;
if (RX > -Threshold && RX < Threshold)
RX = 0; // ºöÂÔÒ¡¸ËС·ù¶È¶¯×÷
Move_X = (RC_Velocity / 128) * LY; // ËٶȿØÖÆ£¬Á¦¶È±íʾËٶȴóС
if (Car_Num == Akm_Car) // °¢¿ËÂü³µ×ªÏò¿ØÖÆ£¬Á¦¶È±íʾתÏò½Ç¶È
Move_Z = -(RC_Turn_Velocity / 128) * RX;
else // ÆäËû³µÐÍתÏò¿ØÖÆ
{
if (Move_X >= 0)
Move_Z = -(RC_Turn_Velocity / 128) * RX; // תÏò¿ØÖÆ£¬Á¦¶È±íʾתÏòËÙ¶È
else
Move_Z = (RC_Turn_Velocity / 128) * RX;
}
if (PS2_KEY == PSB_L1) // °´ÏÂ×ó1¼ü¼ÓËÙ£¨°´¼üÔÚ¶¥ÉÏ£©
{
if ((++Key1_Count) == 20) // µ÷½Ú°´¼ü·´Ó¦ËÙ¶È
{
PS2_KEY = 0;
Key1_Count = 0;
if ((RC_Velocity += X_Step) > MAX_RC_Velocity) // Ç°½ø×î´óËÙ¶È800mm/s
RC_Velocity = MAX_RC_Velocity;
if (Car_Num != Akm_Car) // ·Ç°¢¿ËÂü³µ¿Éµ÷½ÚתÏòËÙ¶È
{
if ((RC_Turn_Velocity += Z_Step) > MAX_RC_Turn_Bias) // תÏò×î´óËÙ¶È325
RC_Turn_Velocity = MAX_RC_Turn_Bias;
}
}
}
else if (PS2_KEY == PSB_R1) // °´ÏÂÓÒ1¼ü¼õËÙ
{
if ((++Key2_Count) == 20)
{
PS2_KEY = 0;
Key2_Count = 0;
if ((RC_Velocity -= X_Step) < MINI_RC_Velocity) // Ç°ºó×îСËÙ¶È210mm/s
RC_Velocity = MINI_RC_Velocity;
if (Car_Num != Akm_Car) // ·Ç°¢¿ËÂü³µ¿Éµ÷½ÚתÏòËÙ¶È
{
if ((RC_Turn_Velocity -= Z_Step) < MINI_RC_Turn_Velocity) // תÏò×îСËÙ¶È45
RC_Turn_Velocity = MINI_RC_Turn_Velocity;
}
}
}
else
Key2_Count = 0, Key2_Count = 0; // ¶ÁÈ¡µ½ÆäËû°´¼üÖØмÆÊý
Move_X = Move_X / 1000;
Move_Z = -Move_Z; // ËÙ¶ÈMove_XתΪm/s
}
/**************************************************************************
Function: Get_Velocity_From_Encoder
Input : none
Output : none
º¯Êý¹¦ÄÜ£º¶ÁÈ¡±àÂëÆ÷ºÍת»»³ÉËÙ¶È
Èë¿Ú²ÎÊý: ÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void Get_Velocity_From_Encoder(void)
{
// Retrieves the original data of the encoder
// »ñÈ¡±àÂëÆ÷µÄÔʼÊý¾Ý
float Encoder_A_pr, Encoder_B_pr;
OriginalEncoder.A = Read_Encoder(Encoder1);
OriginalEncoder.B = Read_Encoder(Encoder2);
// Decide the encoder numerical polarity according to different car models
// ¸ù¾Ý²»Í¬Ð¡³µÐͺžö¶¨±àÂëÆ÷ÊýÖµ¼«ÐÔ
switch (Car_Num)
{
case Akm_Car:
Encoder_A_pr = OriginalEncoder.A;
Encoder_B_pr = -OriginalEncoder.B;
break;
case Diff_Car:
Encoder_A_pr = OriginalEncoder.A;
Encoder_B_pr = -OriginalEncoder.B;
break;
case Small_Tank_Car:
Encoder_A_pr = OriginalEncoder.A;
Encoder_B_pr = -OriginalEncoder.B;
break;
case Big_Tank_Car:
Encoder_A_pr = OriginalEncoder.A;
Encoder_B_pr = -OriginalEncoder.B;
break;
}
// The encoder converts the raw data to wheel speed in m/s
// ±àÂëÆ÷ÔʼÊý¾Ýת»»Îª³µÂÖËٶȣ¬µ¥Î»m/s
MotorA.Current_Encoder = Encoder_A_pr * Frequency * Perimeter / 60000.0f; // 60000 = 4*500*30
MotorB.Current_Encoder = Encoder_B_pr * Frequency * Perimeter / 60000.0f; // 1560=4*13*30=2£¨Á½Â·Âö³å£©*2£¨ÉÏÏÂÑؼÆÊý£©*»ô¶û±àÂëÆ÷13Ïß*µç»úµÄ¼õËÙ±È
// MotorA.Current_Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Akm_wheelspacing//(4*13*30);
// MotorB.Current_Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Akm_wheelspacing/Encoder_precision;
}
/**************************************************************************
Function: Drive_Motor
Input : none
Output : none
º¯Êý¹¦ÄÜ£ºÔ˶¯Ñ§Äæ½â
Èë¿Ú²ÎÊý: ÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
// Ô˶¯Ñ§Äæ½â£¬ÓÉxºÍyµÄËٶȵõ½±àÂëÆ÷µÄËÙ¶È,VxÊÇm/s,Vzµ¥Î»ÊǶÈ/s(½Ç¶ÈÖÆ)
// °¢¿ËÂü³µVzÊǶæ»úתÏòµÄ½Ç¶È(»¡¶ÈÖÆ)
void Get_Target_Encoder(float Vx, float Vz)
{
float MotorA_Velocity, MotorB_Velocity;
float amplitude = 3.5f; // Wheel target speed limit //³µÂÖÄ¿±êËÙ¶ÈÏÞ·ù
Move_X = target_limit_float(Move_X, -1.2, 1.2);
Move_Z = target_limit_float(Move_Z, -Pi / 3, Pi / 3);
if (Car_Num == Akm_Car) // °¢¿ËÂü³µ
{
// Ackerman car specific related variables //°¢¿ËÂüС³µ×¨ÓÃÏà¹Ø±äÁ¿
float R, ratio = 636.56, AngleR, Angle_Servo;
// For Ackerman small car, Vz represents the front wheel steering Angle
// ¶ÔÓÚ°¢¿ËÂüС³µVz´ú±íÓÒÇ°ÂÖתÏò½Ç¶È
AngleR = Vz;
R = Akm_axlespacing / tan(AngleR) - 0.5f * Wheelspacing;
// Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad
// Ç°ÂÖתÏò½Ç¶ÈÏÞ·ù(¶æ»ú¿ØÖÆÇ°ÂÖתÏò½Ç¶È)£¬µ¥Î»£ºrad
AngleR = target_limit_float(AngleR, -0.49f, 0.32f);
// Inverse kinematics //Ô˶¯Ñ§Äæ½â
if (AngleR != 0)
{
MotorA.Target_Encoder = Vx * (R - 0.081f) / R;
MotorB.Target_Encoder = Vx * (R + 0.081f) / R;
}
else
{
MotorA.Target_Encoder = Vx;
MotorB.Target_Encoder = Vx;
}
// The PWM value of the servo controls the steering Angle of the front wheel
// ¶æ»úPWMÖµ£¬¶æ»ú¿ØÖÆÇ°ÂÖתÏò½Ç¶È
Angle_Servo = -0.628f * pow(AngleR, 3) + 1.269f * pow(AngleR, 2) - 1.772f * AngleR + 1.573f;
Servo_PWM = SERVO_INIT + (Angle_Servo - 1.572f) * ratio;
// printf("%d\r\n",Servo_PWM);
// Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù
MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude);
MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude);
Servo_PWM = target_limit_int(Servo_PWM, 800, 2200); // Servo PWM value limit //¶æ»úPWMÖµÏÞ·ù
}
else if (Car_Num == Diff_Car) // ²îËÙС³µ
{
if (Vx < 0)
Vz = -Vz;
else
Vz = Vz;
// Inverse kinematics //Ô˶¯Ñ§Äæ½â
MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È
MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È
// Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù
MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude);
MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude);
}
else if (Car_Num == Small_Tank_Car)
{
if (Vx < 0)
Vz = -Vz;
else
Vz = Vz;
MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È
MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È
// Wheel (motor) target speed limit //³µÂÖ(µç»ú)Ä¿±êËÙ¶ÈÏÞ·ù
MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude);
MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude);
}
else if (Car_Num == Big_Tank_Car)
{
if (Vx < 0)
Vz = -Vz;
else
Vz = Vz;
MotorA.Target_Encoder = Vx - Vz * Wheelspacing / 2.0f; // ¼ÆËã³ö×óÂÖµÄÄ¿±êËÙ¶È
MotorB.Target_Encoder = Vx + Vz * Wheelspacing / 2.0f; // ¼ÆËã³öÓÒÂÖµÄÄ¿±êËÙ¶È
MotorA.Target_Encoder = target_limit_float(MotorA.Target_Encoder, -amplitude, amplitude);
MotorB.Target_Encoder = target_limit_float(MotorB.Target_Encoder, -amplitude, amplitude);
}
}
/**************************************************************************
Function: Get_Motor_PWM
Input : none
Output : none
º¯Êý¹¦ÄÜ£º×ª»»³ÉÇý¶¯µç»úµÄPWM
Èë¿Ú²ÎÊý: ÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void Get_Motor_PWM(void)
{
// ¼ÆËã×óÓÒµç»ú¶ÔÓ¦µÄPWM
MotorA.Motor_Pwm = Incremental_PI_Left(MotorA.Current_Encoder, MotorA.Target_Encoder);
MotorB.Motor_Pwm = Incremental_PI_Right(MotorB.Current_Encoder, MotorB.Target_Encoder);
if (Mode == Normal_Mode || Mode == Measure_Distance_Mode)
{
// Â˲¨£¬Ê¹Æ𲽺ÍÍ£Ö¹ÉÔ΢ƽ»¬Ò»Ð©
MotorA.Motor_Pwm = Mean_Filter_Left(MotorA.Motor_Pwm);
MotorB.Motor_Pwm = Mean_Filter_Right(MotorB.Motor_Pwm);
}
// ÏÞ·ù
MotorA.Motor_Pwm = PWM_Limit(MotorA.Motor_Pwm, PWM_MAX, PWM_MIN);
MotorB.Motor_Pwm = PWM_Limit(MotorB.Motor_Pwm, PWM_MAX, PWM_MIN);
}
/**************************************************************************
Function: PWM_Limit
Input : IN;max;min
Output : OUT
º¯Êý¹¦ÄÜ£ºÏÞÖÆPWM¸³Öµ
Èë¿Ú²ÎÊý: IN£ºÊäÈë²ÎÊý max£ºÏÞ·ù×î´óÖµ min£ºÏÞ·ù×îСֵ
·µ»Ø Öµ£ºÏÞ·ùºóµÄÖµ
**************************************************************************/
float PWM_Limit(float IN, int max, int min)
{
float OUT = IN;
if (OUT > max)
OUT = max;
if (OUT < min)
OUT = min;
return OUT;
}
/**************************************************************************
Function: Limiting function
Input : Value
Output : none
º¯Êý¹¦ÄÜ£ºÏÞ·ùº¯Êý
Èë¿Ú²ÎÊý£º·ùÖµ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
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 whether it is abnormal
Input : none
Output : 1:Abnormal;0:Normal
º¯Êý¹¦ÄÜ£ºÒì³£¹Ø±Õµç»ú
Èë¿Ú²ÎÊý: ÎÞ
·µ»Ø Öµ£º1£ºÒì³£ 0£ºÕý³£
**************************************************************************/
u8 Turn_Off(void)
{
u8 temp = Normal;
Flag_Stop = KEY2_STATE; // ¶ÁÈ¡°´¼ü2״̬£¬°´¼ü2¿ØÖƵç»úµÄ¿ª¹Ø
if (Voltage < 1000) // µç³ØµçѹµÍÓÚ10V¹Ø±Õµç»ú,LEDµÆ¿ìËÙÉÁ˸
LED_Flash(50), temp = Abnormal;
else
LED_Flash(200); // ÿһÃëÉÁÒ»´Î£¬Õý³£ÔËÐÐ
if (Flag_Stop)
temp = Abnormal;
return temp;
}
/**************************************************************************
Function: Data sliding filtering
Input : data
Output : Filtered data
º¯Êý¹¦ÄÜ£ºÊý¾Ý»¬¶¯Â˲¨
Èë¿Ú²ÎÊý£ºÊý¾Ý
·µ»Ø Öµ£ºÂ˲¨ºóµÄÊý¾Ý
**************************************************************************/
float Mean_Filter_Left(float data)
{
u8 i;
float Sum_Data = 0;
float Filter_Data;
static float Speed_Buf[FILTERING_TIMES] = {0};
for (i = 1; i < FILTERING_TIMES; i++)
{
Speed_Buf[i - 1] = Speed_Buf[i];
}
Speed_Buf[FILTERING_TIMES - 1] = data;
for (i = 0; i < FILTERING_TIMES; i++)
{
Sum_Data += Speed_Buf[i];
}
Filter_Data = (s32)(Sum_Data / FILTERING_TIMES);
return Filter_Data;
}
/**************************************************************************
Function: Data sliding filtering
Input : data
Output : Filtered data
º¯Êý¹¦ÄÜ£ºÊý¾Ý»¬¶¯Â˲¨
Èë¿Ú²ÎÊý£ºÊý¾Ý
·µ»Ø Öµ£ºÂ˲¨ºóµÄÊý¾Ý
**************************************************************************/
float Mean_Filter_Right(float data)
{
u8 i;
float Sum_Data = 0;
float Filter_Data;
static float Speed_Buf[FILTERING_TIMES] = {0};
for (i = 1; i < FILTERING_TIMES; i++)
{
Speed_Buf[i - 1] = Speed_Buf[i];
}
Speed_Buf[FILTERING_TIMES - 1] = data;
for (i = 0; i < FILTERING_TIMES; i++)
{
Sum_Data += Speed_Buf[i];
}
Filter_Data = (s32)(Sum_Data / FILTERING_TIMES);
return Filter_Data;
}
/**************************************************************************
Function: Lidar_Avoid
Input : none
Output : none
º¯Êý¹¦ÄÜ£ºÀ×´ï±ÜÕÏģʽ
Èë¿Ú²ÎÊý£ºÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void Lidar_Avoid(void)
{
int i = 0;
u8 calculation_angle_cnt = 0; // ÓÃÓÚÅжÏ100¸öµãÖÐÐèÒª×ö±ÜÕϵĵã
float angle_sum = 0; // ´ÖÂÔ¼ÆËãÕÏ°ÎïλÓÚ×ó»òÕßÓÒ
u8 distance_count = 0; // ¾àÀëСÓÚijֵµÄ¼ÆÊý
int distance = 350; // É趨±ÜÕϾàÀë,ĬÈÏÊÇ300
if (Car_Num == Akm_Car)
distance = 400; // °¢¿ËÂü³µÉ趨ÊÇ400mm
else if (Car_Num == Big_Tank_Car)
distance = 500; // ´óÂÄ´ø³µÉ趨ÊÇ500mm
for (i = 0; i < lap_count; i++)
{
if ((Dataprocess[i].angle > 310) || (Dataprocess[i].angle < 50))
{
if ((0 < Dataprocess[i].distance) && (Dataprocess[i].distance < distance)) // ¾àÀëСÓÚ350mmÐèÒª±ÜÕÏ,Ö»ÐèÒª100¶È·¶Î§ÄÚµã
{
calculation_angle_cnt++; // ¼ÆËã¾àÀëСÓÚ±ÜÕϾàÀëµÄµã¸öÊý
if (Dataprocess[i].angle < 50)
angle_sum += Dataprocess[i].angle;
else if (Dataprocess[i].angle > 310)
angle_sum += (Dataprocess[i].angle - 360); // 310¶Èµ½50¶Èת»¯Îª-50¶Èµ½50¶È
if (Dataprocess[i].distance < 200) // ¼Ç¼СÓÚ200mmµÄµãµÄ¼ÆÊý
distance_count++;
}
}
}
if (calculation_angle_cnt < 8) // СÓÚ8µã²»ÐèÒª±ÜÕÏ£¬È¥³ýһЩÔëµã
{
if ((Move_X += 0.1) >= Aovid_Speed) // ±ÜÕϵÄËÙ¶ÈÉ趨Ϊ260£¬Öð½¥Ôö¼Óµ½260¿ÉÉÔ΢ƽ»¬Ò»Ð©
Move_X = Aovid_Speed;
Move_Z = 0; // ²»±ÜÕÏʱ²»ÐèҪתÍä
}
else // ÐèÒª±ÜÕÏ£¬¼òµ¥µØÅжÏÕÏ°Î﷽λ
{
if (Car_Num == Akm_Car) // °¢¿ËÂü³µÐÍÓжæ»ú£¬ÐèÒªÌØÊâ´¦Àí
{
if (distance_count > 8) // ¾àÀëСÓÚ±ÜÕ½¾àÀë
Move_X = -Aovid_Speed, Move_Z = 0; // ÍùºóÍË
else
{
if ((Move_X -= 0.1) <= (Aovid_Speed * 0.5)) // ±ÜÕÏʱËٶȽµµ½µÍËÙ0.25
Move_X = Aovid_Speed * 0.5;
if (angle_sum > 0) // ÕÏ°ÎïÆ«ÓÒ
Move_Z = -Pi / 5; // ÿ´ÎתÍä½Ç¶ÈΪPI/5£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕÏ°Îï¾ÍÍ£Ö¹
else // Æ«×ó
Move_Z = Pi / 5;
}
}
else
{
if (distance_count > 8) // СÓÚ±ÜÕ½¾àÀëµÄʱºò
Move_X = -Aovid_Speed, Move_Z = 0; // ÍùºóÍË
else
{
if ((Move_X -= 0.1) <= (Aovid_Speed * 0.5)) // ±ÜÕÏʱËٶȽµµ½µÍËÙ¶È0.15
Move_X = (Aovid_Speed * 0.5);
if (angle_sum > 0) // ÕÏ°ÎïÆ«ÓÒ
{
if (Car_Num == Diff_Car) // ÿ´ÎתÍäËÙ¶ÈΪX¶È£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕÏ°Îï¾ÍÍ£Ö¹
Move_Z = -1;
else if (Car_Num == Small_Tank_Car)
Move_Z = -1;
else
Move_Z = -1;
}
else // Æ«×ó
{
if (Car_Num == Diff_Car) // ÿ´ÎתÍäËÙ¶ÈΪX¶È£¬Ö±µ½100¶È·¶Î§ÄÚÎÞÕÏ°Îï¾ÍÍ£Ö¹
Move_Z = 1;
else if (Car_Num == Small_Tank_Car)
Move_Z = 1;
else
Move_Z = 1;
}
}
}
}
Move_Z = -Move_Z;
}
/**************************************************************************
Function: Lidar_Follow
Input : none
Output : none
º¯Êý¹¦ÄÜ£ºÀ×´ï¸úËæģʽ
Èë¿Ú²ÎÊý£ºÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
float angle1 = 0; // ¸úËæµÄ½Ç¶È
u16 mini_distance1;
void Lidar_Follow(void)
{
static u16 cnt = 0;
int i;
int calculation_angle_cnt = 0;
static float angle = 0; // ¸úËæµÄ½Ç¶È
static float last_angle = 0; //
u16 mini_distance = 65535;
static u8 data_count = 0; // ÓÃÓÚÂ˳ýһдÔëµãµÄ¼ÆÊý±äÁ¿
// ÐèÒªÕÒ³ö¸úËæµÄÄǸöµãµÄ½Ç¶È
for (i = 0; i < lap_count; i++)
{
if (100 < Dataprocess[i].distance && Dataprocess[i].distance < Follow_Distance) // 1200·¶Î§ÄÚ¾ÍÐèÒª¸úËæ
{
calculation_angle_cnt++;
if (Dataprocess[i].distance < mini_distance) // ÕÒ³ö¾àÀë×îСµÄµã
{
mini_distance = Dataprocess[i].distance;
angle = Dataprocess[i].angle;
}
}
}
if (angle > 180)
angle -= 360; // 0--360¶Èת»»³É0--180£»-180--0£¨Ë³Ê±Õ룩
if (angle - last_angle > 10 || angle - last_angle < -10) // ×öÒ»¶¨Ïû¶¶£¬²¨¶¯´óÓÚ10¶ÈµÄÐèÒª×öÅжÏ
{
if (++data_count == 60) // Á¬Ðø60´Î²É¼¯µ½µÄÖµ(300msºó)ºÍÉϴεıȴóÓÚ10¶È£¬´Ëʱ²ÅÊÇÈÏΪÊÇÓÐЧֵ
{
data_count = 0;
last_angle = angle;
}
}
else // ²¨¶¯Ð¡ÓÚ10¶ÈµÄ¿ÉÒÔÖ±½ÓÈÏΪÊÇÓÐЧֵ
{
data_count = 0;
last_angle = angle;
}
if (calculation_angle_cnt < 6) // ÔÚ¸úË淶ΧÄڵĵãÉÙÓÚ6¸ö
{
if (cnt < 40) // Á¬Ðø¼ÆÊý³¬40´ÎûÓÐÒª¸úËæµÄµã£¬´Ëʱ²ÅÊDz»ÓøúËæ
cnt++;
if (cnt >= 40)
{
Move_X = 0; // ËÙ¶ÈΪ0
Move_Z = 0;
}
}
else
{
cnt = 0;
if (Car_Num == Akm_Car)
{
if ((((angle > 15) && (angle < 180)) || ((angle > -180) && angle < -15)) && (mini_distance < 500)) // °¢¿¨Âü³µÐÍ´¦Àí³µÍ·²»¶ÔןúËæÎÏ൱ÓÚºó³µÒ»Ñù£¬Ò»´Î²»¶Ô×¼£¬ÄǺóÍËÔÙÀ´¶Ô×¼
{
Move_X = -0.20;
Move_Z = -Follow_Turn_PID(last_angle, 0);
}
else
{
Move_X = Distance_Adjust_PID(mini_distance, Keep_Follow_Distance); // ±£³Ö¾àÀë±£³ÖÔÚ400mm
Move_Z = Follow_Turn_PID(last_angle, 0);
}
}
else // ÆäÓà³µÐÍ
{
if ((angle > 50 || angle < -50) && (mini_distance > 400))
{
Move_Z = -0.0298f * last_angle; // ½Ç¶È²î¾à¹ý´óÖ±½Ó¿ìËÙתÏò
Move_X = 0; // ²îËÙС³µºÍÂÄ´øС³µ¿ÉÒÔʵÏÖÔµØת¶¯
}
else
{
Move_X = Distance_Adjust_PID(mini_distance, Keep_Follow_Distance); // ±£³Ö¾àÀë±£³ÖÔÚ400mm
Move_Z = Follow_Turn_PID(last_angle, 0); // תÏòPID£¬³µÍ·ÓÀÔ¶¶ÔןúËæÎïÆ·
}
}
}
Move_Z = target_limit_float(Move_Z, -Pi / 6, Pi / 6); // ÏÞ·ù
Move_X = target_limit_float(Move_X, -0.6, 0.6);
}
/**************************************************************************
º¯Êý¹¦ÄÜ£ºÐ¡³µ×ßÖ±Ïßģʽ
Èë¿Ú²ÎÊý£ºÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void Lidar_along_wall(void)
{
static u32 target_distance = 0;
static int n = 0;
u32 distance;
u8 data_count = 0; // ÓÃÓÚÂ˳ýһдÔëµãµÄ¼ÆÊý±äÁ¿
for (int i = 0; i < lap_count; i++)
{
if (Dataprocess[i].angle > 75 && Dataprocess[i].angle < 77)
{
if (n == 0)
{
target_distance = Dataprocess[i].distance; // »ñÈ¡µÄµÚÒ»¸öµã×÷ΪĿ±ê¾àÀë
n++;
}
if (Dataprocess[i].distance < target_distance + 100) //+100ÏÞÖÆ»ñÈ¡¾àÀëµÄ·¶Î§Öµ
{
distance = Dataprocess[i].distance; // »ñȡʵʱ¾àÀë
data_count++;
}
}
}
// if(data_count <= 0)
// Move_X = 0;
//
Move_X = forward_velocity; // ³õʼËÙ¶È
Move_Z = -Along_Adjust_PID(distance, target_distance);
if (Car_Num == Akm_Car)
{
Move_Z = target_limit_float(Move_Z, -Pi / 4, Pi / 4); // ÏÞ·ù
}
else if (Car_Num == Diff_Car)
Move_Z = target_limit_float(Move_Z, -Pi / 5, Pi / 5); // ÏÞ·ù
}
/**************************************************************************
Function: Car_Perimeter_Init
Input : none
Output : none
º¯Êý¹¦ÄÜ£º¼ÆËãС³µ¸÷ÂÖ×ÓµÄÖܳ¤
Èë¿Ú²ÎÊý£ºÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void Car_Perimeter_Init(void)
{
if (Car_Num == Diff_Car || Car_Num == Akm_Car)
{
Perimeter = Diff_Car_Wheel_diameter * Pi;
Wheelspacing = Diff_wheelspacing;
}
else if (Car_Num == Small_Tank_Car)
{
Perimeter = Small_Tank_WheelDiameter * Pi;
Wheelspacing = Small_Tank_wheelspacing;
}
else
{
Perimeter = Big_Tank_WheelDiameter * Pi;
Wheelspacing = Big_Tank_wheelspacing;
}
}
/**************************************************************************
Function: Ultrasonic_Follow
Input : none
Output : none
º¯Êý¹¦ÄÜ£º³¬Éù²¨¸úËæģʽ
Èë¿Ú²ÎÊý£ºÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void Ultrasonic_Follow(void) // ³¬Éù²¨¸úË棬ֻÄܵ¥·½Ïò¸úËæ
{
Move_Z = 0;
Read_Distane(); // ¶ÁÈ¡³¬Éù²¨µÄ¾àÀë
if (Distance1 < 200) // ¾àÀëСÓÚ200mm£¬Í˺ó
{
if ((Move_X -= 3) < -210)
Move_X = -210; // ¸øÒ»210ºóÍËËÙ¶È
}
else if (Distance1 > 270 && Distance1 < 750) // ¾àÀëÔÚ270µ½750Ö®¼äÊÇÐèÒª¸úËæÇ°½ø
{
if ((Move_X += 3) > 210) // ËÙ¶ÈÖð½¥Ôö¼Ó£¬¸øÇ°½øËÙ¶È
Move_X = 210;
}
else
{
if (Move_X > 0)
{
if ((Move_X -= 20) < 0) // ËÙ¶ÈÖð½¥¼õµ½0
Move_X = 0;
}
else
{
if ((Move_X += 20) > 0) // ËÙ¶ÈÖð½¥¼õµ½0
Move_X = 0;
}
}
}
/**************************************************************************
Function: Get angle
Input : way£ºThe algorithm of getting angle 1£ºDMP 2£ºkalman 3£ºComplementary filtering
Output : none
º¯Êý¹¦ÄÜ£º»ñÈ¡½Ç¶È
Èë¿Ú²ÎÊý£ºway£º»ñÈ¡½Ç¶ÈµÄËã·¨ 1£ºDMP 2£º¿¨¶ûÂü 3£º»¥²¹Â˲¨
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void Get_Angle(u8 way)
{
if (way == 1) // DMPµÄ¶ÁÈ¡ÔÚÊý¾Ý²É¼¯Öж϶ÁÈ¡£¬Ñϸñ×ñÑʱÐòÒªÇó
{
Read_DMP(); // ¶ÁÈ¡¼ÓËٶȡ¢½ÇËٶȡ¢Çã½Ç
}
else
{
Gyro_X = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_XOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_XOUT_L); // ¶ÁÈ¡XÖáÍÓÂÝÒÇ
Gyro_Y = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_YOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_YOUT_L); // ¶ÁÈ¡YÖáÍÓÂÝÒÇ
Gyro_Z = (I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_ZOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_GYRO_ZOUT_L); // ¶ÁÈ¡ZÖáÍÓÂÝÒÇ
Accel_X = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_XOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_XOUT_L); // ¶ÁÈ¡XÖá¼ÓËٶȼÆ
Accel_Y = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_YOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_YOUT_L); // ¶ÁÈ¡XÖá¼ÓËٶȼÆ
Accel_Z = (I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_ZOUT_H) << 8) + I2C_ReadOneByte(devAddr, MPU6050_RA_ACCEL_ZOUT_L); // ¶ÁÈ¡ZÖá¼ÓËٶȼÆ
// if(Gyro_X>32768) Gyro_X-=65536; //Êý¾ÝÀàÐÍת»» Ò²¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»»
// if(Gyro_Y>32768) Gyro_Y-=65536; //Êý¾ÝÀàÐÍת»» Ò²¿Éͨ¹ýshortÇ¿ÖÆÀàÐÍת»»
// if(Gyro_Z>32768) Gyro_Z-=65536; //Êý¾ÝÀàÐÍת»»
// if(Accel_X>32768) Accel_X-=65536; //Êý¾ÝÀàÐÍת»»
// if(Accel_Y>32768) Accel_Y-=65536; //Êý¾ÝÀàÐÍת»»
// if(Accel_Z>32768) Accel_Z-=65536; //Êý¾ÝÀàÐÍת»»
Accel_Angle_x = atan2(Accel_Y, Accel_Z) * 180 / Pi; // ¼ÆËãÇã½Ç£¬×ª»»µ¥Î»Îª¶È
Accel_Angle_y = atan2(Accel_X, Accel_Z) * 180 / Pi; // ¼ÆËãÇã½Ç£¬×ª»»µ¥Î»Îª¶È
Gyro_X = Gyro_X / 65.5; // ÍÓÂÝÒÇÁ¿³Ìת»»£¬Á¿³Ì¡À500¡ã/s¶ÔÓ¦ÁéÃô¶È65.5£¬¿É²éÊÖ²á
Gyro_Y = Gyro_Y / 65.5; // ÍÓÂÝÒÇÁ¿³Ìת»»
if (way == 2)
{
Roll = -Kalman_Filter_x(Accel_Angle_x, Gyro_X); // ¿¨¶ûÂüÂ˲¨
Pitch = -Kalman_Filter_y(Accel_Angle_y, Gyro_Y);
}
else if (way == 3)
{
Roll = -Complementary_Filter_x(Accel_Angle_x, Gyro_X); // »¥²¹Â˲¨
Pitch = -Complementary_Filter_y(Accel_Angle_y, Gyro_Y);
}
}
}
/**************************************************************************
Function: The remote control command of model aircraft is processed
Input : none
Output : none
º¯Êý¹¦ÄÜ£º¶Ôº½Ä£Ò£¿Ø¿ØÖÆÃüÁî½øÐд¦Àí
Èë¿Ú²ÎÊý£ºÎÞ
·µ»Ø Öµ£ºÎÞ
**************************************************************************/
void Remote_Control(void)
{
// Data within 1 second after entering the model control mode will not be processed
// ¶Ô½øÈ뺽ģ¿ØÖÆģʽºó1ÃëÄÚµÄÊý¾Ý²»´¦Àí
static u8 thrice = 200;
int Threshold = 100;
// limiter //ÏÞ·ù
int LX, RY;
// static float Target_LX,Target_LY,Target_RY,Target_RX;
Remoter_Ch1 = target_limit_int(Remoter_Ch1, 1000, 2000);
Remoter_Ch2 = target_limit_int(Remoter_Ch2, 1000, 2000);
// Front and back direction of left rocker. Control forward and backward.
// ×óÒ¡¸ËÇ°ºó·½Ïò¡£¿ØÖÆÇ°½øºóÍË¡£
LX = Remoter_Ch2 - 1500;
// //Left joystick left and right. Control left and right movement.
// //×óÒ¡¸Ë×óÓÒ·½Ïò¡£¿ØÖÆ×óÓÒÒƶ¯¡£¡£
// LY=Remoter_Ch2-1500;
// Right stick left and right. To control the rotation.
// ÓÒÒ¡¸Ë×óÓÒ·½Ïò¡£¿ØÖÆ×Ôת¡£
RY = Remoter_Ch1 - 1500; //
if (LX > -Threshold && LX < Threshold)
LX = 0;
if (RY > -Threshold && RY < Threshold)
RY = 0;
// if(LX==0) Target_LX=Target_LX/1.2f;
// if(LY==0) Target_LY=Target_LY/1.2f;
// if(RY==0) Target_RY=Target_RY/1.2f;
// //Throttle related //ÓÍÃÅÏà¹Ø
// Remote_RCvelocity=RC_Velocity+RX;
// if(Remote_RCvelocity<0)Remote_RCvelocity=0;
// The remote control command of model aircraft is processed
// ¶Ôº½Ä£Ò£¿Ø¿ØÖÆÃüÁî½øÐд¦Àí
Move_X = LX;
Move_Z = -RY;
Move_X = Move_X * 1.3; //*1.3ÊÇΪÁËÀ«´óËÙ¶È
if (Car_Num == Akm_Car)
Move_Z = Move_Z * (Pi / 8) / 350.0;
else
Move_Z = Move_Z * 2 * (Pi / 4) / 350.0;
// Unit conversion, mm/s -> m/s
// µ¥Î»×ª»»£¬mm/s -> m/s
Move_X = Move_X / 1000;
// ZÖáÊý¾Ýת»¯
#if _4WD
if (Move_X < 0)
Move_Z = -Move_Z;
#endif
// Data within 1 second after entering the model control mode will not be processed
// ¶Ô½øÈ뺽ģ¿ØÖÆģʽºó1ÃëÄÚµÄÊý¾Ý²»´¦Àí
if (thrice > 0)
Move_X = 0, Move_Z = 0, thrice--;
// Control target value is obtained and kinematics analysis is performed
// µÃµ½¿ØÖÆÄ¿±êÖµ£¬½øÐÐÔ˶¯Ñ§·ÖÎö
// Get_Target_Encoder(Move_X,Move_Z);
}
怎么把OpenMV与STM32串口通信实现二维码/APRILTag码识别及控制的代码加到上述代码中，现在openmv的二维码识别已做好，二维码采用的是Apriltag的36h11类型，id:0表示停止，id:1表示直行，id:2表示左转，id:3表示右转，现在需要再在STM32中修改或添加代码，使其能在接收到openmv识别到的指令后作出相应的动作
一位大学同学通过QQ询问另一位同学是否在线，后者巧妙地用一句话回应了自己的在线状态。
扫一扫
3120
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
