L2-001 紧急情况/Emergency

本文介绍了一种解决最短路径问题的方法,并通过DFS优化来寻找路径数量最多且权重和最大的路径。文章提供了完整的C++代码实现,展示了如何避免不必要的复杂度并修正常见错误。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

题目:https://www.patest.cn/contests/gplt/L2-001

其实根本不用dfs,还因为忘加反向路径和路径数搞错wa了好久。。。

#include<bits/stdc++.h>
using namespace std;
const int inf=99999999;
const int maxn=505;
vector<int> pre[maxn],temppath,path;
int dis[maxn],g[maxn][maxn],n,m,s,t,a[maxn],maxnum=-1,pathnum=0;
bool vis[maxn];

void dijkstra(){
    for(int i=0;i<n;i++)
        dis[i]=(i==s)?0:inf;
    memset(vis,0,sizeof(vis));
    for(int i=0;i<n;i++){
        int mark=-1,minis=inf;
        for(int j=0;j<n;j++){
            if(!vis[j]&&dis[j]<minis){
                minis=dis[j];
                mark=j;
            }
        }
        vis[mark]=1;
        for(int j=0;j<n;j++)
            if(!vis[j]&&g[mark][j]!=inf){
                if(dis[mark]+g[mark][j]<dis[j]){
                    dis[j]=dis[mark]+g[mark][j];
                    pre[j].clear();
                    pre[j].push_back(mark);
                }
                else if(dis[mark]+g[mark][j]==dis[j])
                    pre[j].push_back(mark);
            }
    }
}
void dfs(int v){
    if(v==s){
        pathnum++;
        temppath.push_back(v);
        int cnt=0;
        for(int i=0;i<temppath.size();i++)
            cnt+=a[temppath[i]];
        if(cnt>maxnum){
            maxnum=cnt;
            path=temppath;  //vector能直接赋
        }
        temppath.pop_back();
        return;
    }
    temppath.push_back(v);
    for(int i=0;i<pre[v].size();i++)
        dfs(pre[v][i]);
    temppath.pop_back();
}
int main(){
    fill(g[0],g[0]+505*505,inf);
    fill(dis,dis+505,inf);
    cin>>n>>m>>s>>t;
    for(int i=0;i<n;i++)
        cin>>a[i];
    for(int i=0;i<m;i++){
        int u,v,len;
        cin>>u>>v>>len;
        if(len<g[u][v])
            g[u][v]=g[v][u]=len; //好久没写,双向路径都忘了。。
    }
    dijkstra();
    dfs(t);
    cout<<pathnum<<" "<<maxnum<<endl;  //错在这里路径树根本不是pre[t].size,而是每次dfs到s的次数
    for(int i=path.size();i>0;i--){
        cout<<path[i-1];
        if(i==1) cout<<endl;
        else cout<<" ";
    }
    return 0;
}

#include "bsp_at8236.h" static int16_t Motor_Ignore_Dead_Zone(int16_t pulse) { if (pulse > 0) return pulse + MOTOR_IGNORE_PULSE; if (pulse < 0) return pulse - MOTOR_IGNORE_PULSE; return 0; } void init_motor(void) { DL_TimerA_startCounter(PWM_L_INST); DL_TimerA_startCounter(PWM_R_INST); } void L_control(uint16_t motor_speed,uint8_t dir) { if(dir) { DL_TimerA_setCaptureCompareValue(PWM_L_INST, motor_speed, DL_TIMER_CC_0_INDEX); DL_TimerA_setCaptureCompareValue(PWM_L_INST, 0, DL_TIMER_CC_1_INDEX); } else { DL_TimerA_setCaptureCompareValue(PWM_L_INST, 0, DL_TIMER_CC_0_INDEX); DL_TimerA_setCaptureCompareValue(PWM_L_INST, motor_speed, DL_TIMER_CC_1_INDEX); } } void R_control(uint16_t motor_speed,uint8_t dir) { if(dir) { DL_TimerA_setCaptureCompareValue(PWM_R_INST, motor_speed, DL_TIMER_CC_0_INDEX); DL_TimerA_setCaptureCompareValue(PWM_R_INST, 0, DL_TIMER_CC_1_INDEX); } else { DL_TimerA_setCaptureCompareValue(PWM_R_INST, 0, DL_TIMER_CC_0_INDEX); DL_TimerA_setCaptureCompareValue(PWM_R_INST, motor_speed, DL_TIMER_CC_1_INDEX); } } // 控制小车运动,Motor_X=[-3600, 3600],超过范围则无效。 void Motion_Set_Pwm(int16_t Motor_1, int16_t Motor_2) { if (Motor_1 >= -MOTOR_MAX_PULSE && Motor_1 <= MOTOR_MAX_PULSE) { Motor_Set_Pwm(Motor_M1, Motor_1); } if (Motor_2 >= -MOTOR_MAX_PULSE && Motor_2 <= MOTOR_MAX_PULSE) { Motor_Set_Pwm(Motor_M2, Motor_2); } } // 设置电机速度,speed:±3600, 0为停止 void Motor_Set_Pwm(uint8_t id, int16_t speed) { int16_t pulse = Motor_Ignore_Dead_Zone(speed); // 限制输入 if (pulse >= MOTOR_MAX_PULSE) pulse = MOTOR_MAX_PULSE; if (pulse <= -MOTOR_MAX_PULSE) pulse = -MOTOR_MAX_PULSE; switch (id) { case Motor_M1: { if (pulse >= 0) { DL_TimerA_setCaptureCompareValue(PWM_L_INST, pulse, DL_TIMER_CC_0_INDEX); DL_TimerA_setCaptureCompareValue(PWM_L_INST, 0, DL_TIMER_CC_1_INDEX); } else { pulse = -pulse; //把负数转成正数 DL_TimerA_setCaptureCompareValue(PWM_L_INST, 0, DL_TIMER_CC_0_INDEX); DL_TimerA_setCaptureCompareValue(PWM_L_INST, pulse, DL_TIMER_CC_1_INDEX); } break; } case Motor_M2: { if (pulse >= 0) { DL_TimerA_setCaptureCompareValue(PWM_R_INST, pulse, DL_TIMER_CC_0_INDEX); DL_TimerA_setCaptureCompareValue(PWM_R_INST, 0, DL_TIMER_CC_1_INDEX); } else { pulse = -pulse; //把负数转成正数 DL_TimerA_setCaptureCompareValue(PWM_R_INST, 0, DL_TIMER_CC_0_INDEX); DL_TimerA_setCaptureCompareValue(PWM_R_INST, pulse, DL_TIMER_CC_1_INDEX); } break; } default: break; } } 上述代码为at8236驱动模块驱动电机的代码,帮我改成tb6612驱动模块的代码,并配合以下代码实现控制速度 #include "app_motor.h" extern volatile int32_t gEncoderCount_L,gEncoderCount_R; pid_t pid_motor[2]; int Encoder_speed_offset[2] = {0}; int Encoder_speed_last[2] = {0}; int Encoder_speed_now[2] = {0}; uint8_t g_start_ctrl = 0; car_data_t car_data; motor_data_t motor_data; // 获取开机到现在总共的两路编码器计数。 void Encoder_Get_ALL(int* Encoder_all) { Encoder_all[0] -= gEncoderCount_L; Encoder_all[1] += gEncoderCount_R; gEncoderCount_L = 0;//清0 gEncoderCount_R = 0;//清0 } // 获取编码器数据,并计算偏差脉冲数 void Motion_Get_Encoder(void) { Encoder_Get_ALL(Encoder_speed_now); for(uint8_t i = 0; i < Motor_MAX; i++) { // 记录两次测试时间差的脉冲数 Encoder_speed_offset[i] = Encoder_speed_now[i] - Encoder_speed_last[i]; // 记录上次编码器数据 Encoder_speed_last[i] = Encoder_speed_now[i]; } } //返回编码器脉冲 static float Motion_Get_Circle_Pulse(void) { return ENCODER_CIRCLE_450; } // 返回当前小车轮子轴间距和的一半 static float Motion_Get_APB(void) { return STM32Car_APB; } // 返回当前小车轮子转一圈的多少毫米 static float Motion_Get_Circle_MM(void) { return WHEEL_CIRCLE_MM; } void Motor_PID_Init(void) { for (int i = 0; i < Motor_MAX; i++) { pid_motor[i].target_val = 0.0; pid_motor[i].pwm_output = 0.0; pid_motor[i].err = 0.0; pid_motor[i].err_last = 0.0; pid_motor[i].err_next = 0.0; pid_motor[i].integral = 0.0; pid_motor[i].Kp = MOTOR_P; pid_motor[i].Ki = MOTOR_I; pid_motor[i].Kd = MOTOR_D; } } // 设置PID目标速度,单位为:mm/s void PID_Set_Motor_Target(uint8_t motor_id, float target) { if (motor_id > Motor_MAX) return; if (motor_id == Motor_MAX) { for (int i = 0; i < Motor_MAX; i++) { pid_motor[i].target_val = target; } } else { pid_motor[motor_id].target_val = target; } } // 清除PID数据 void PID_Clear_Motor(uint8_t motor_id) { if (motor_id > Motor_MAX) return; if (motor_id == Motor_MAX) { for (int i = 0; i < Motor_MAX; i++) { pid_motor[i].pwm_output = 0.0; pid_motor[i].err = 0.0; pid_motor[i].err_last = 0.0; pid_motor[i].err_next = 0.0; pid_motor[i].integral = 0.0; } } else { pid_motor[motor_id].pwm_output = 0.0; pid_motor[motor_id].err = 0.0; pid_motor[motor_id].err_last = 0.0; pid_motor[motor_id].err_next = 0.0; pid_motor[motor_id].integral = 0.0; } } // 增量式PID计算公式 float PID_Incre_Calc(pid_t *pid, float actual_val) { /*计算目标值与实际值的误差*/ pid->err = pid->target_val - actual_val; /*PID算法实现*/ pid->pwm_output += pid->Kp *(pid->err - pid->err_next) + pid->Ki * pid->err + pid->Kd *(pid->err - 2 * pid->err_next + pid->err_last); /*传递误差*/ pid->err_last = pid->err_next; pid->err_next = pid->err; /*返回PWM输出值*/ if (pid->pwm_output > (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE)) pid->pwm_output = (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE); if (pid->pwm_output < (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE)) pid->pwm_output = (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE); return pid->pwm_output; } // PID计算输出值 void PID_Calc_Motor(motor_data_t* motor) { int i; for (i = 0; i < Motor_MAX; i++) { motor->speed_pwm[i] = PID_Incre_Calc(&pid_motor[i], motor->speed_mm_s[i]); } } void Motion_Set_Speed(int16_t speed_m1, int16_t speed_m2) { if(speed_m1 == 0 && speed_m2 == 0) { Motion_Stop(); } else { //因为有毛刺,编码器不一定为0,根据pid会进行控制,会导致停止不了。 //所以产生if g_start_ctrl = 1; } motor_data.speed_set[0] = speed_m1; motor_data.speed_set[1] = speed_m2; for (uint8_t i = 0; i < Motor_MAX; i++) { PID_Set_Motor_Target(i, motor_data.speed_set[i]*1.0); } } // 从编码器读取当前各轮子速度,单位mm/s void Motion_Get_Speed(car_data_t* car) { int i = 0; float speed_mm[Motor_MAX] = {0}; float circle_mm = Motion_Get_Circle_MM(); float circle_pulse = Motion_Get_Circle_Pulse(); float robot_APB = Motion_Get_APB(); Motion_Get_Encoder(); // 计算轮子速度,单位mm/s。 for (i = 0; i < Motor_MAX; i++) { speed_mm[i] = (Encoder_speed_offset[i]) * 100 * circle_mm / circle_pulse; } car->Vx = (speed_mm[0] + speed_mm[1]) / 2.0f; car->Vy = 0; car->Vz = -(speed_mm[0] - speed_mm[1] ) / 2.0f / robot_APB * 1000; if (g_start_ctrl) { PID_Calc_Motor(&motor_data); } } // 小车停止 void Motion_Stop(void) { Motion_Set_Speed(0,0); PID_Clear_Motor(Motor_MAX); g_start_ctrl = 0; Motion_Set_Pwm(0,0); } static float speed_lr = 0; static float speed_fb = 0; static float speed_spin = 0; static int speed_L_setup = 0; static int speed_R_setup = 0; void wheel_Ctrl(int16_t V_x, int16_t V_y, int16_t V_z) //巡线PID直接输出此值 { float robot_APB = Motion_Get_APB(); // speed_lr = -V_y; speed_lr = 0; speed_fb = V_x; speed_spin = (V_z / 1000.0f) * robot_APB; if (V_x == 0 && V_y == 0 && V_z == 0) { Motion_Stop(); return; } speed_L_setup = speed_fb + speed_lr + speed_spin; speed_R_setup = speed_fb - speed_lr - speed_spin; if (speed_L_setup > 1000) speed_L_setup = 1000; if (speed_L_setup < -1000) speed_L_setup = -1000; if (speed_R_setup > 1000) speed_R_setup = 1000; if (speed_R_setup < -1000) speed_R_setup = -1000; //printf("%d\t,%d\t,%d\t,%d\r\n",speed_L1_setup,speed_L2_setup,speed_R1_setup,speed_R2_setup); Motion_Set_Speed(speed_L_setup, speed_R_setup); } // 运动控制句柄,每10ms调用一次,主要处理速度相关的数据 void Motion_Handle(void) { Motion_Get_Speed(&car_data); if (g_start_ctrl) { Motion_Set_Pwm(motor_data.speed_pwm[0], motor_data.speed_pwm[1]); } } //直接控制pwm void motion_car_control(int16_t V_x, int16_t V_y, int16_t V_z) { float robot_APB = Motion_Get_APB(); speed_lr = 0; speed_fb = V_x; speed_spin = (V_z / 1000.0f) * robot_APB; if (V_x == 0 && V_y == 0 && V_z == 0) { Motion_Set_Pwm(0,0); return; } speed_L_setup = speed_fb + speed_spin; speed_R_setup = speed_fb - speed_spin; if (speed_L_setup > 1000) speed_L_setup = 1000; if (speed_L_setup < -1000) speed_L_setup = -1000; if (speed_R_setup > 1000) speed_R_setup = 1000; if (speed_R_setup < -1000) speed_R_setup = -1000; //printf("%d\t,%d\t,%d\t,%d\r\n",speed_L1_setup,speed_L2_setup,speed_R1_setup,speed_R2_setup); Motion_Set_Pwm(speed_L_setup, speed_R_setup); }
07-27
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值