void Motor_Set(int Motor1,int Motor2) {Motor1 =-Motor1; Motor2 =-Motor2; if(Motor1 <0) BIN1_SET; else BIN1_RESET; if(Motor2 <0) AIN1_SET; else AIN1_RESET; if(Motor1 <0) { if(Motor1 <-99) Motor1 =-99; __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (100+Motor1)); } else { if(Motor1 >99) Motor1 = 99; __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,Motor1); } if(Motor2<0) { if(Motor2 <-99) Motor2=-99; __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, (100+Motor2)); } else { if(Motor2 >99) Motor2 =99; __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, Motor2); } }
时间: 2024-03-30 13:33:59 浏览: 13
是的,这段代码是用来控制直流电机转动的。下面是各个部分的作用:
1. `Motor1 =-Motor1; Motor2 =-Motor2;`:将传入的电机速度参数取反,是为了保证电机正反转的控制。
2. `if(Motor1 <0) BIN1_SET; else BIN1_RESET;` 和 `if(Motor2 <0) AIN1_SET; else AIN1_RESET;`:根据电机速度参数的正负,选择对应的GPIO输出高电平或低电平,控制电机正反转。
3. `if(Motor1 <0)` 和 `if(Motor2<0)`:判断电机速度参数的正负,选择对应的定时器通道(TIM_CHANNEL)。
4. `if(Motor1 <-99) Motor1 =-99;` 和 `if(Motor2 <-99) Motor2=-99;`:对电机速度参数进行限幅,保证在-99到99之间。
5. `__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (100+Motor1));` 和 `__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, (100+Motor2));`:设置定时器比较值,控制电机转速。其中,100是因为定时器的比较值范围是0-199,+100是为了将-99到99转换为1到199。
相关问题
对下列代码进行注释:#define Init_motor4B Motor_LA = 0,Motor_LB = 0,Motor_LC = 0,Motor_LD = 0 void Init_Motor(void); void Set_Motor( bit a, bit b, int c );//a是否开启,b是否,c步进间隔1ms * c static bit fangxiang = 1;//0是逆时针 static bit kaiqi = 0;//0是停止 static int sudu = 10;//每次步进间隔时间为 1ms * sudu(最低2.5ms) void Init_Motor(void) { TMOD |= 0x01; TH0 = 0x0FC; TL0 = 0x18; EA = 1; ET0 = 1; TR0 = 1; Init_motor4B; } void Set_Motor( bit a, bit b, int c ) { kaiqi = a; fangxiang = b; if( sudu < 3 ) sudu = 3; sudu = c; } void Motor_Star(void) interrupt 1 { static int times = 0, star = 0; TH0 = 0x0FC; TL0 = 0x18; times++; if( kaiqi == 1 && times >= sudu ) { times = 0; if( star >= 9 ) { star = 1; } if( star <= 0 ) { star = 8; } switch ( star ) { case 1: Motor_LA = 1,Motor_LB = 0,Motor_LC = 0,Motor_LD = 0; break; case 2: Motor_LA = 1,Motor_LB = 1,Motor_LC = 0,Motor_LD = 0; break; case 3: Motor_LA = 0,Motor_LB = 1,Motor_LC = 0,Motor_LD = 0; break; case 4: Motor_LA = 0,Motor_LB = 1,Motor_LC = 1,Motor_LD = 0; break; case 5: Motor_LA = 0,Motor_LB = 0,Motor_LC = 1,Motor_LD = 0; break; case 6: Motor_LA = 0,Motor_LB = 0,Motor_LC = 1,Motor_LD = 1; break; case 7: Motor_LA = 0,Motor_LB = 0,Motor_LC = 0,Motor_LD = 1; break; case 8: Motor_LA = 1,Motor_LB = 0,Motor_LC = 0,Motor_LD = 1; break; } if( fangxiang == 1 ) { star++; } else { star--; } } }
这段代码是控制步进电机的程序,主要包括以下几个函数:
1. `#define Init_motor4B Motor_LA = 0,Motor_LB = 0,Motor_LC = 0,Motor_LD = 0`:定义了一个宏,用于初始化电机引脚。
2. `void Init_Motor(void)`:初始化步进电机的参数,并开启定时器。
3. `void Set_Motor( bit a, bit b, int c )`:设置步进电机的运行参数,包括电机是否开启、旋转方向、步进间隔时间。
4. `void Motor_Star(void) interrupt 1`:定时器中断函数,用于控制电机的旋转方向、速度和步进。
整个程序的主要流程是:通过调用 `Init_Motor()` 函数进行电机的初始化和定时器的开启,然后通过调用 `Set_Motor()` 函数设置电机的运行参数,最后在定时器中断函数 `Motor_Star()` 中控制电机的旋转方向、速度和步进。具体实现方式是通过改变电机引脚的电平状态来控制电机的旋转方向和步进。
解释这段代码static void chassis_control_loop(chassis_move_t *chassis_move_control_loop) { fp32 max_vector = 0.0f, vector_rate = 0.0f; fp32 temp = 0.0f; fp32 wheel_speed[4] = {0.0f, 0.0f, 0.0f, 0.0f}; uint8_t i = 0; float position_error, speed_error; float position_output, speed_output; float current_position, current_speed; float target_position, target_speed; chassis_move_control_loop->vx_set=vx_set; chassis_move_control_loop->vy_set=vy_set; chassis_move_control_loop->wz_set=angle_set; chassis_vector_to_mecanum_wheel_speed(chassis_move_control_loop->vx_set, chassis_move_control_loop->vy_set, chassis_move_control_loop->wz_set, wheel_speed); if (chassis_move_control_loop->chassis_mode == CHASSIS_VECTOR_RAW) { for (i = 0; i < 4; i++) { chassis_move_control_loop->motor_chassis[i].give_current = (int16_t)(wheel_speed[i]); } } for (i = 0; i < 4; i++) { chassis_move_control_loop->motor_chassis[i].speed_set = wheel_speed[i]; temp = fabs(chassis_move_control_loop->motor_chassis[i].speed_set); if (max_vector < temp) { max_vector = temp; } } if (max_vector > MAX_WHEEL_SPEED) { vector_rate = MAX_WHEEL_SPEED / max_vector; for (i = 0; i < 4; i++) { chassis_move_control_loop->motor_chassis[i].speed_set *= vector_rate; } } for (i = 0; i < 4; i++) { PID_Calc(&chassis_move_control_loop->motor_speed_pid[i], chassis_move_control_loop->motor_chassis[i].speed, chassis_move_control_loop->motor_chassis[i].speed_set); } for (i = 0; i < 4; i++) { chassis_move_control_loop->motor_chassis[i].give_current = (int16_t)(chassis_move_control_loop->motor_speed_pid[i].out); } }
chassis_move_control_loop->motor_chassis[i].position_pid, chassis_move_control_loop->motor_chassis[i].speed_pid, chassis_move_control_loop->motor_chassis[i].position_get, chassis_move_control_loop->motor_chassis[i].speed_get, chassis_move_control_loop->motor_chassis[i].speed_set, &position_error, &speed_error, &position_output, &speed_output); current_position = chassis_move_control_loop->motor_chassis[i].position_get; current_speed = chassis_move_control_loop->motor_chassis[i].speed_get; target_position = chassis_move_control_loop->motor_chassis[i].position_set; target_speed = speed_output; if(chassis_move_control_loop->motor_chassis[i].position_pid.enable == 1) //PID启动 { chassis_move_control_loop->motor_chassis[i].give_current = PID_Calc(&chassis_move_control_loop->motor_chassis[i].position_pid, current_position, target_position); } else { chassis_move_control_loop->motor_chassis[i].give_current = PID_Calc(&chassis_move_control_loop->motor_chassis[i].speed_pid, current_speed, target_speed); } } }
该函数为底盘控制代码,主要实现底盘的位置和速度控制。
具体实现方法为:
1.根据控制指令,将底盘的速度向量转换为各个驱动电机的速度。
2.根据底盘模式选择不同的控制方式:
如果模式为 CHASSIS_VECTOR_RAW,直接将每个驱动电机的输出电流设置为对应速度。
3.根据电机的最大速度设置,对转换后的速度进行限制。
4.对每个驱动电机进行PID控制,计算出目标位置和目标速度,并根据位置控制或速度控制模式下达电机电流控制指令。
5.将目标速度和目标位置在PID计算中使用,并将计算出的电流控制指令转换为电机的输出电流。