int16_t Get_MotorB_Speed(void) { int16_t TEST; //Read_EncoderB(); // 获取编码器产生的脉冲数 转速n = 总脉冲数/四倍频/单圈脉冲数/减速比/时间 ( EncoderB / 2 / 13 / 30 /10 * 1000) TEST =1; return TEST; } void TIM2_IRQHandler(void) //TM2的中断函数 { if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET) { GetMotorB_Speed =Read_EncoderB(); TIM_ClearITPendingBit(TIM2, TIM_IT_Update); } }

时间: 2024-04-09 12:29:51 浏览: 107
这段代码是一个C语言函数和中断处理函数的例代码。函数`Get_MotorB_Speed`返回一个`int16_t`类型的变量,它的值在这个示例中被设置为1。中断处理函数`TIM2_IRQHandler`用于处理TIM2定时器的中断事件。在这个示例中,当TIM2的更新中断被触发时,会调用`Read_EncoderB`函数获取编码器的脉冲数,并将其赋值给变量`GetMotorB_Speed`。然后,通过调用`TIM_ClearITPendingBit`函数清除中断标志位。请注意,这只是一个代码片段,缺少了一些关键信息,如函数`Read_EncoderB`的具体实现和相关的初始化代码。
相关问题

#include "stm32f10x.h" // Device header #include "Delay.h" #include "OLED.h" #include "Encoder.h" #include "Motor.h" #include "Timer.h" #include "Serial.h" int8_t L=0; int8_t l_stop=0; int16_t Larr[]={1000,2000,3000,4000,5000}; int16_t countL=0; int16_t Llocation=0; float Lkp=0.5,Lki=0.14,Lkd=0; int16_t Lspeed_goal=50,Lspeed_current; int16_t Lspeederror_current,Lspeederror_add=0; int16_t Lspeederror_before=0; float Loutput=0; int8_t R=0; int8_t r_stop=0; int16_t Rarr[]={5000,5000,5000,5000,5000}; int16_t countR; int16_t Rlocation=0; float Rkp=0.4,Rki=0.15,Rkd=0; int16_t Rspeed_goal=50,Rspeed_current; int16_t Rspeederror_current,Rspeederror_add=0; int16_t Rspeederror_before=0; float Routput=0; int main(void) { OLED_Init(); Motor_Init(); Encoder_Init(); Timer_Init(); Serial_Init(); while (1) { Serial_Printf("%d,%d\r\n",Rlocation,Llocation); OLED_ShowChar(0,0,'L',OLED_8X16); OLED_ShowSignedNum(8,0,Lspeed_current,3,OLED_8X16); OLED_ShowSignedNum(0,16,Llocation,5,OLED_8X16); OLED_ShowChar(0,32,'R',OLED_8X16); OLED_ShowSignedNum(8,32,Rspeed_current,3,OLED_8X16); OLED_ShowSignedNum(0,48,Rlocation,5,OLED_8X16); OLED_Update(); } } void TIM3_IRQHandler(void)//1毫秒进一次中断 { if(TIM_GetITStatus(TIM3, TIM_IT_Update) == SET) { countL++; if(countL==40)//每四十毫秒进入一次 { //*************************************************************************************************************** //这是左轮子 Lspeed_current=Encoder_GetA(); Lspeederror_current=Lspeed_goal-Lspeed_current; Lspeederror_add+=Lspeederror_current; //积分限幅 if(Lspeederror_add>350) { Lspeederror_add=350; } else if(Lspeederror_add<-350) { Lspeederror_add=-350; } //计算输出 Loutput=Lkp*Lspeederror_current+Lki*Lspeederror_add+Lkd*(Lspeederror_current-Lspeederror_before); //输出限幅 if(Loutput>100) { Loutput=100; } else if(Loutput<-100) { Loutput=-100; } Llocation+=Lspeed_current; if(Llocation>=Larr[L]-50&&Larr[L]-50<=Llocation) { L++; Llocation=0; l_stop=1; } else { l_stop=0; } if(l_stop==0|| (l_stop&&r_stop) ) { Motor_SetSpeedA(Loutput);//输出 l_stop=0; } else { Motor_SetSpeedA(0);//等一下另一个轮子 } Lspeederror_before=Lspeederror_current; countL=0; } countR++; if(countR==40) { //******************************************************************************************************************** //这是右轮 Rspeed_current=Encoder_GetB(); Rspeederror_current=Rspeed_goal-Rspeed_current; Rspeederror_add+=Rspeederror_current; //积分限幅 if(Rspeederror_add>300) { Rspeederror_add=300; } else if(Rspeederror_add<-300) { Rspeederror_add=-300; } //计算输出 Routput=Rkp*Rspeederror_current+Rki*Rspeederror_add+Rkd*(Rspeederror_current-Rspeederror_before); //输出限幅 if(Routput>100) { Routput=100; } else if(Routput<-100) { Routput=-100; } Rlocation+=Rspeed_current; if(Rlocation>=Rarr[R]-50&&Rarr[R]-50<=Rlocation) { R++; Rlocation=0; r_stop=1; } else { r_stop=0; } if(r_stop==0|| (l_stop&&r_stop) ) { Motor_SetSpeedB(Routput);//输出 r_stop=0; } else { Motor_SetSpeedB(0);//等一下另一个轮子 } Rspeederror_before=Rspeederror_current; countR=0; } TIM_ClearITPendingBit(TIM3, TIM_IT_Update); } } 我希望小车轮子能停下来等另一个小车轮子,为什么停不下来,是哪里写错了吗

### 小车轮子同步问题分析 在双轮小车的 PID 控制中,实现两个轮子的速度同步是一个常见的挑战。通过分析提供的代码片段以及 STM32F10x 的特性,可以发现一些可能导致轮子不同步的原因。 #### 1. **停止条件中的绝对值判断** 代码中使用了 `if (abs(encountA) <= 5 && pidMotor1Speed.target_val == 0)` 来决定是否停止电机 A 和 B。这种逻辑存在潜在问题: - 如果编码器计数值 (`encountA` 或 `encountB`) 波动较大,则可能频繁触发该条件,导致电机反复启停。 - 编码器分辨率较低时,`abs(encountA) <= 5` 这一阈值可能不够精确,无法有效检测到真正的静止状态[^1]。 #### 2. **增量式 PID 输出的影响** 增量式 PID 控制器计算的是每次调节所需的 PWM 值变化量,而非直接的目标 PWM 值。因此,在目标速度为零的情况下,如果当前误差较小,PWM 调整可能会非常缓慢甚至停滞不前。这可能导致一侧轮子先达到完全停止的状态,而另一侧仍然有微弱转动,从而破坏同步性。 以下是改进后的伪代码示例: ```c // 定义全局变量用于记录上次输出值 static int last_motorA_output = 0; static int last_motorB_output = 0; void Timer_ISR(void) { if (pidMotor1Speed.target_val == 0) { if (abs(encountA) <= THRESHOLD_STOP) { // 更严格的停止判定 PWMA = 0; } else { motorA = Incremental_PID(&pidMotor1Speed, encountA); last_motorA_output += motorA; // 累加增量输出 ControlLeftMotor(last_motorA_output); } } if (pidMotor2Speed.target_val == 0) { if (abs(encountB) <= THRESHOLD_STOP) { // 更严格的停止判定 PWMB = 0; } else { motorB = Incremental_PID(&pidMotor2Speed, encountB); last_motorB_output += motorB; // 累加增量输出 ControlRightMotor(last_motorB_output); } } } ``` #### 3. **左右轮独立控制带来的偏差** 即使两路 PID 参数相同,由于硬件差异(如电机性能、齿轮磨损)、环境干扰等因素,实际运行过程中仍可能出现速度不对称的情况。为了改善这一现象,可以在软件层面引入额外补偿机制,例如动态调整两侧目标转速以维持一致距离差。 一种简单的方法是在主循环中加入如下校正算法: ```c int error_distance = encoder_left_count - encoder_right_count; float correction_factor = Kp * error_distance / MAX_ENCODER_COUNTS_PER_REVOLUTION; pidMotor1Speed.target_val -= correction_factor; // 减少左轮目标值 pidMotor2Speed.target_val += correction_factor; // 增大右轮目标值 ``` 其中 `Kp` 是比例增益系数,需根据实验调优;`MAX_ENCODER_COUNTS_PER_REVOLUTION` 表示每圈脉冲数。 --- ### 总结 通过对现有代码逻辑的剖析可知,主要存在问题包括但不限于:过宽泛的停止判据设定容易引起误操作;单纯依赖增量型PID难以快速响应至零速指令等问题均会影响最终效果达成度。针对这些问题提出了相应的优化措施并给出了具体实施办法以便于后续开发人员参考应用实践当中去解决此类难题[^1]。

int GetMotorB_Speed=1; void TIM2_IRQHandler(void) //TM2的中断函数 { if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET) { GetMotorB_Speed = Read_EncoderB(); TIM_ClearITPendingBit(TIM2, TIM_IT_Update); } } 这段代码错误的原因

根据你提供的代码,我无法确定是否存在错误。代码中的函数`TIM2_IRQHandler`是用来处理TIM2的中断的。在该函数中,首先通过调用`TIM_GetITStatus`函数来检查TIM2的更新中断是否被触发。如果中断被触发,那么会调用`Read_EncoderB`函数来获取`MotorB`的速度,并将其赋值给`GetMotorB_Speed`变量。最后,通过调用`TIM_ClearITPendingBit`函数来清除中断标志位。 如果你遇到了错误,可能是由于以下原因之一: 1. `Read_EncoderB`函数未定义或未声明:请确保你在代码的其他地方定义或声明了这个函数。 2. `TIM2`未正确配置:在使用TIM2之前,你需要进行正确的配置,包括时钟使能、定时器模式、中断使能等。 3. TIM2中断优先级设置不正确:请确保中断优先级设置正确,并且中断向量表正确地指向了`TIM2_IRQHandler`函数。 4. 其他代码逻辑问题:请仔细检查代码逻辑,确保没有其他错误导致该段代码无法正常运行。 如果你能提供更多的上下文信息或错误提示,我可以更具体地帮助你找到错误所在。
阅读全文

相关推荐

/* USER CODE BEGIN Header */ /** ****************************************************************************** * @file : main.c * @brief : Main program body ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "main.h" #include "adc.h" #include "tim.h" #include "gpio.h" #include "mpu6500.h" I2C_HandleTypeDef hi2c1; /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ #include "motor_control.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ /* USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); /* USER CODE BEGIN PFP */ /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ /** * @brief The application entry point. * @retval int */ int main(void) { /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* USER CODE BEGIN Init */ /* USER CODE END Init */ /* Configure the system clock */ SystemClock_Config(); /* USER CODE BEGIN SysInit */ /* USER CODE END SysInit */ /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_ADC1_Init(); MX_TIM3_Init(); MX_TIM2_Init(); MX_TIM4_Init(); MX_I2C1_Init(); // 手动初始化I2C1 /* USER CODE BEGIN 2 */ // 启动编码器模式 HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL); // 启动电机A编码器 HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); // 启动电机B编码器 // 启动PWM输出 HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1); // 电机A控制引脚1 HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2); // 电机A控制引脚2 HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3); // 电机B控制引脚1 HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4); // 电机B控制引脚2 // 启动ADC HAL_ADC_Start(&hadc1); // MPU6500初始化 if (MPU6500_Init(&hi2c1) != HAL_OK) { // 初始化失败,可在此处添加错误处理 Error_Handler(); } /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ // 示例:读取编码器值计算转速 int32_t encoderA_value = __HAL_TIM_GET_COUNTER(&htim2); // 读取电机A编码器值 int32_t encoderB_value = __HAL_TIM_GET_COUNTER(&htim4); // 读取电机B编码器值 // 示例:读取ADC值计算输入电压 uint32_t adc_value = 0; HAL_ADC_PollForConversion(&hadc1, 100); // 等待转换完成 adc_value = HAL_ADC_GetValue(&hadc1); // 读取ADC值 float input_voltage = adc_value * (3.3f * 11.0f / 4096.0f); // 计算输入电压 // 示例:使用电机控制函数 // 方式1:设置具体速度 set_motor_speed(1, 900); // 电机A正转,速度90% set_motor_speed(2, 900); // 电机B正转,速度90% // 方式2:使用直线前进函数 //motorStraight(800); // 两个电机同时以80%速度前进 //motorStraight(-600); // 两个电机同时以60%速度后退 // 方式3:使用转向函数 //motorTurnLeft(30); // 左转1秒 //motorTurnRight(30); // 右转0.5秒 // 方式4:使用简化函数 // motor_forward(1); // 电机A正转 (50%速度) // motor_backward(2); // 电机B反转 (50%速度) // motor_stop(1); // 电机A停止 // 方式5:使用专用函数 // set_motorA_speed(700); // 电机A 70%速度正转 // set_motorB_speed(-400); // 电机B 40%速度反转 // motorA_forward(); // 电机A正转 // motorB_backward(); // 电机B反转 // MPU6500数据读取示例 MPU6500_Data_t mpu_data; if (MPU6500_ReadAccelGyro(&hi2c1, &mpu_data) == HAL_OK) { // 这里可以处理mpu_data.accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z // 比如:将数据通过串口输出,或用于姿态解算等 } HAL_Delay(100); // 延时100ms } /* USER CODE END 3 */ } /** * @brief System Clock Configuration * @retval None */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /** Configure the main internal regulator output voltage */ __HAL_RCC_PWR_CLK_ENABLE(); __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3); /** Initializes the RCC Oscillators according to the specified parameters * in the RCC_OscInitTypeDef structure. */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; RCC_OscInitStruct.PLL.PLLM = 16; RCC_OscInitStruct.PLL.PLLN = 336; RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4; RCC_OscInitStruct.PLL.PLLQ = 2; RCC_OscInitStruct.PLL.PLLR = 2; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); } /** Initializes the CPU, AHB and APB buses clocks */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) { Error_Handler(); } } /* USER CODE BEGIN 4 */ /* USER CODE END 4 */ /** * @brief Period elapsed callback in non blocking mode * @note This function is called when TIM1 interrupt took place, inside * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment * a global variable "uwTick" used as application time base. * @param htim : TIM handle * @retval None */ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { /* USER CODE BEGIN Callback 0 */ /* USER CODE END Callback 0 */ if (htim->Instance == TIM1) { HAL_IncTick(); } /* USER CODE BEGIN Callback 1 */ /* USER CODE END Callback 1 */ } /** * @brief This function is executed in case of error occurrence. * @retval None */ void Error_Handler(void) { /* USER CODE BEGIN Error_Handler_Debug */ /* User can add his own implementation to report the HAL error return state */ __disable_irq(); while (1) { } /* USER CODE END Error_Handler_Debug */ } #ifdef USE_FULL_ASSERT /** * @brief Reports the name of the source file and the source line number * where the assert_param error has occurred. * @param file: pointer to the source file name * @param line: assert_param error line source number * @retval None */ void assert_failed(uint8_t *file, uint32_t line) { /* USER CODE BEGIN 6 */ /* User can add his own implementation to report the file name and line number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ /* USER CODE END 6 */ } #endif /* USE_FULL_ASSERT */ 该主函数报错上述内容 提出修改建议

检验下列代码 ​#include <Stepper.h> #include <Servo.h> #define SERVO_PIN 3 // 舵机信号线接3 #define MOTOR_A_PIN1 8 // 电机A引脚 #define MOTOR_A_PIN2 9 #define MOTOR_A_PIN3 10 #define MOTOR_A_PIN4 11 #define MOTOR_B_PIN1 4 // 电机B引脚 #define MOTOR_B_PIN2 5 #define MOTOR_B_PIN3 6 #define MOTOR_B_PIN4 7 #define STEPS_PER_REV 2048 // 单圈步数(根据电机调整) #define MAX_STEPS 2048*20 // 最大允许步数(20圈保护) // ====== 全局对象声明 ====== Stepper motorA(STEPS_PER_REV, MOTOR_A_PIN1, MOTOR_A_PIN3, MOTOR_A_PIN2, MOTOR_A_PIN4); Stepper motorB(STEPS_PER_REV, MOTOR_B_PIN1, MOTOR_B_PIN3, MOTOR_B_PIN2, MOTOR_B_PIN4); int currentAngle = 90; // 舵机初始角度 // ====== 初始化设置 ====== void setup() { Serial.begin(9600); myServo.attach(SERVO_PIN);//舵机初始化 // 电机初始化 motorA.setSpeed(12); // 设置转速(RPM) motorB.setSpeed(12); myServo.write(currentAngle);// 舵机归中 Serial.println("系统就绪"); Serial.println("命令格式:"); Serial.println("舵机角度:0-180"); Serial.println("A±步数(例:A+2048)"); Serial.println("B±步数(例:B-1024)"); } // ====== 主循环 ====== void loop() { if (Serial.available() > 0) { String input = Serial.readStringUntil('\n'); input.trim(); // 舵机控制命令(纯数字) if (isValidNumber(input)) { int angle = input.toInt(); controlServo(angle); } // 步进电机控制命令 if (input.startsWith("A")) { controlMotor(input, motorA, 'A'); } else if (input.startsWith("B")) { controlMotor(input, motorB, 'B'); } } } // ====== 舵机控制函数 ====== void controlServo(int targetAngle) { // 角度安全限制 targetAngle = constrain(targetAngle, 0, 180); // 平滑转动 int step = (targetAngle > currentAngle) ? 1 : -1; while (currentAngle != targetAngle) { currentAngle += step; myServo.write(currentAngle); delay(15); // 防止舵机过冲 } Serial.print("舵机已到达:"); Serial.println(currentAngle); } // ====== 通用电机控制函数 ====== void controlMotor(String cmd, Stepper &motor, char motorID) { // 参数验证 if (cmd.length() < 2 || (cmd[1] != '+' && cmd[1] != '-')) { Serial.println("错误:无效命令格式"); return; } // 解析步数 long steps = cmd.substring(1).toInt(); steps = constrain(abs(steps), 0, MAX_STEPS); // 执行转动 if (cmd[1] == '+') { motor.step(steps); Serial.print("电机"); Serial.print(motorID); Serial.print(" 顺时针转动 "); } else { motor.step(-steps); Serial.print("电机"); Serial.print(motorID); Serial.print(" 逆时针转动 "); } Serial.print(steps); Serial.println("步"); delay(300); // 动作间隔保护 }

检验并输出下列改进后的完整代码 ​#include <Stepper.h> #include <Servo.h> #define SERVO_PIN 3 // 舵机信号线接3 #define MOTOR_A_PIN1 8 // 电机A引脚 #define MOTOR_A_PIN2 9 #define MOTOR_A_PIN3 10 #define MOTOR_A_PIN4 11 #define MOTOR_B_PIN1 4 // 电机B引脚 #define MOTOR_B_PIN2 5 #define MOTOR_B_PIN3 6 #define MOTOR_B_PIN4 7 #define STEPS_PER_REV 2048 // 单圈步数(根据电机调整) #define MAX_STEPS 2048*20 // 最大允许步数(20圈保护) // ====== 全局对象声明 ====== Stepper motorA(STEPS_PER_REV, MOTOR_A_PIN1, MOTOR_A_PIN3, MOTOR_A_PIN2, MOTOR_A_PIN4); Stepper motorB(STEPS_PER_REV, MOTOR_B_PIN1, MOTOR_B_PIN3, MOTOR_B_PIN2, MOTOR_B_PIN4); int currentAngle = 90; // 舵机初始角度 // ====== 初始化设置 ====== void setup() { Serial.begin(9600); myServo.attach(SERVO_PIN);//舵机初始化 // 电机初始化 motorA.setSpeed(12); // 设置转速(RPM) motorB.setSpeed(12); myServo.write(currentAngle);// 舵机归中 Serial.println(“系统就绪”); Serial.println(“命令格式:”); Serial.println(“舵机角度:0-180”); Serial.println(“A±步数(例:A+2048)”); Serial.println(“B±步数(例:B-1024)”); } // ====== 主循环 ====== void loop() { if (Serial.available() > 0) { String input = Serial.readStringUntil(‘\n’); input.trim(); // 舵机控制命令(纯数字) if (isValidNumber(input)) { int angle = input.toInt(); controlServo(angle); } // 步进电机控制命令 if (input.startsWith("A")) { controlMotor(input, motorA, 'A'); } else if (input.startsWith("B")) { controlMotor(input, motorB, 'B'); } } } // ====== 舵机控制函数 ====== void controlServo(int targetAngle) { // 角度安全限制 targetAngle = constrain(targetAngle, 0, 180); // 平滑转动 int step = (targetAngle > currentAngle) ? 1 : -1; while (currentAngle != targetAngle) { currentAngle += step; myServo.write(currentAngle); delay(15); // 防止舵机过冲 } Serial.print(“舵机已到达:”); Serial.println(currentAngle); } // ====== 通用电机控制函数 ====== void controlMotor(String cmd, Stepper &motor, char motorID) { // 参数验证 if (cmd.length() < 2 || (cmd[1] != ‘+’ && cmd[1] != ‘-’)) { Serial.println(“错误:无效命令格式”); return; } // 解析步数 long steps = cmd.substring(1).toInt(); steps = constrain(abs(steps), 0, MAX_STEPS); // 执行转动 if (cmd[1] == ‘+’) { motor.step(steps); Serial.print(“电机”); Serial.print(motorID); Serial.print(" 顺时针转动 “); } else { motor.step(-steps); Serial.print(“电机”); Serial.print(motorID); Serial.print(” 逆时针转动 "); } Serial.print(steps); Serial.println(“步”); delay(300); // 动作间隔保护 }

#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识别到的指令后作出相应的动作

在下列代码中加入两个舵机控制,输入角度,舵机能转到指定角度,将步进电机转动最大步数改到40960,输出完整代码。#include <Stepper.h> #define MOTOR_A_PIN1 8 // 电机A引脚 #define MOTOR_A_PIN2 9 #define MOTOR_A_PIN3 10 #define MOTOR_A_PIN4 11 #define MOTOR_B_PIN1 4 // 电机B引脚 #define MOTOR_B_PIN2 5 #define MOTOR_B_PIN3 6 #define MOTOR_B_PIN4 7 #define STEPS_PER_REV 2048 // 单圈步数(根据电机调整) #define MAX_STEPS 2048*10 // 最大允许步数(10圈保护) // ====== 全局对象声明 ====== Stepper motorA(STEPS_PER_REV, MOTOR_A_PIN1, MOTOR_A_PIN3, MOTOR_A_PIN2, MOTOR_A_PIN4); Stepper motorB(STEPS_PER_REV, MOTOR_B_PIN1, MOTOR_B_PIN3, MOTOR_B_PIN2, MOTOR_B_PIN4); // ====== 初始化设置 ====== void setup() { Serial.begin(9600); // 电机初始化 motorA.setSpeed(12); // 设置转速(RPM) motorB.setSpeed(12); Serial.println(“双步进电机控制系统就绪”); Serial.println(“命令格式:”); Serial.println(“A±步数(例:A+2048)”); Serial.println(“B±步数(例:B-1024)”); } // ====== 主循环 ====== void loop() { if (Serial.available() > 0) { String input = Serial.readStringUntil(‘\n’); input.trim(); if (input.startsWith("A")) { controlMotor(input, motorA, 'A'); } else if (input.startsWith("B")) { controlMotor(input, motorB, 'B'); } } } // ====== 通用电机控制函数 ====== void controlMotor(String cmd, Stepper &motor, char motorID) { // 参数验证 if (cmd.length() < 2 || (cmd[1] != ‘+’ && cmd[1] != ‘-’)) { Serial.println(“错误:无效命令格式”); return; } // 解析步数 long steps = cmd.substring(1).toInt(); steps = constrain(abs(steps), 0, MAX_STEPS); // 执行转动 if (cmd[1] == ‘+’) { motor.step(steps); Serial.print(“电机”); Serial.print(motorID); Serial.print(" 顺时针转动 “); } else { motor.step(-steps); Serial.print(“电机”); Serial.print(motorID); Serial.print(” 逆时针转动 "); } Serial.print(steps); Serial.println(“步”); delay(300); // 动作间隔保护 }

将下列代码改为通过esp8266与点灯科技相结合,实现能够在点灯科技上能够实现下列代码功能,在点灯科技上控制步进电机转动。#include <Stepper.h> #define MOTOR_A_PIN1 8 // 电机A引脚 #define MOTOR_A_PIN2 9 #define MOTOR_A_PIN3 10 #define MOTOR_A_PIN4 11 #define MOTOR_B_PIN1 4 // 电机B引脚 #define MOTOR_B_PIN2 5 #define MOTOR_B_PIN3 6 #define MOTOR_B_PIN4 7 #define STEPS_PER_REV 2048 // 单圈步数(根据电机调整) #define MAX_STEPS 2048*12 // 最大允许步数(12圈保护) // ====== 全局对象声明 ====== Stepper motorA(STEPS_PER_REV, MOTOR_A_PIN1, MOTOR_A_PIN3, MOTOR_A_PIN2, MOTOR_A_PIN4); Stepper motorB(STEPS_PER_REV, MOTOR_B_PIN1, MOTOR_B_PIN3, MOTOR_B_PIN2, MOTOR_B_PIN4); // ====== 初始化设置 ====== void setup() { Serial.begin(9600); // 电机初始化 motorA.setSpeed(12); // 设置转速(RPM) motorB.setSpeed(12); Serial.println("双步进电机控制系统就绪"); Serial.println("命令格式:"); Serial.println("A±步数(例:A+2048)"); Serial.println("B±步数(例:B-1024)"); } // ====== 主循环 ====== void loop() { if (Serial.available() > 0) { String input = Serial.readStringUntil('\n'); input.trim(); if (input.startsWith("A")) { controlMotor(input, motorA, 'A'); } else if (input.startsWith("B")) { controlMotor(input, motorB, 'B'); } } } // ====== 通用电机控制函数 ====== void controlMotor(String cmd, Stepper &motor, char motorID) { // 参数验证 if (cmd.length() < 2 || (cmd[1] != '+' && cmd[1] != '-')) { Serial.println("错误:无效命令格式"); return; } // 解析步数 long steps = cmd.substring(1).toInt(); steps = constrain(abs(steps), 0, MAX_STEPS); // 执行转动 if (cmd[1] == '+') { motor.step(steps); Serial.print("电机"); Serial.print(motorID); Serial.print(" 顺时针转动 "); } else { motor.step(-steps); Serial.print("电机"); Serial.print(motorID); Serial.print(" 逆时针转动 "); } Serial.print(steps); Serial.println("步"); delay(300); // 动作间隔保护 }

将下列代码改为通过esp8266与点灯科技相结合,实现能够在点灯科技上能够实现下列代码功能。#include <Stepper.h> #include <Servo.h> // 步进电机引脚定义 #define MOTOR_A_PIN1 8 #define MOTOR_A_PIN2 9 #define MOTOR_A_PIN3 10 #define MOTOR_A_PIN4 11 #define MOTOR_B_PIN1 4 #define MOTOR_B_PIN2 5 #define MOTOR_B_PIN3 6 #define MOTOR_B_PIN4 7 // 舵机引脚定义 #define SERVO_C_PIN 2 // 新增舵机C引脚 #define SERVO_D_PIN 3 // 新增舵机D引脚 #define STEPS_PER_REV 2048 #define MAX_STEPS 40960 // 40960步(20圈保护) // 全局对象声明 Stepper motorA(STEPS_PER_REV, MOTOR_A_PIN1, MOTOR_A_PIN3, MOTOR_A_PIN2, MOTOR_A_PIN4); Stepper motorB(STEPS_PER_REV, MOTOR_B_PIN1, MOTOR_B_PIN3, MOTOR_B_PIN2, MOTOR_B_PIN4); Servo servoC; // 声明舵机C对象 Servo servoD; // 声明舵机D对象 // 初始化设置 void setup() { Serial.begin(9600); // 步进电机初始化 motorA.setSpeed(12); motorB.setSpeed(12); // 舵机初始化 servoC.attach(SERVO_C_PIN); // 连接舵机C servoD.attach(SERVO_D_PIN); // 连接舵机D Serial.println("双步进电机+双舵机控制系统就绪"); Serial.println("命令格式:"); Serial.println("步进电机:A±步数 或 B±步数(例:A+2048)"); Serial.println("舵机:C角度 或 D角度(例:C90,范围0-180)"); } // 主循环 void loop() { if (Serial.available() > 0) { String input = Serial.readStringUntil('\n'); input.trim(); if (input.startsWith("A")) { controlMotor(input, motorA, 'A'); } else if (input.startsWith("B")) { controlMotor(input, motorB, 'B'); } else if (input.startsWith("C")) { // 舵机C控制 controlServo(input, servoC, 'C'); } else if (input.startsWith("D")) { // 舵机D控制 controlServo(input, servoD, 'D'); } } } // 步进电机控制函数 void controlMotor(String cmd, Stepper &motor, char motorID) { if (cmd.length() < 2 || (cmd[1] != '+' && cmd[1] != '-')) { Serial.println("错误:无效命令格式"); return; } long steps = cmd.substring(1).toInt(); steps = constrain(abs(steps), 0, MAX_STEPS); if (cmd[1] == '+') { motor.step(steps); Serial.print("电机"); Serial.print(motorID); Serial.print(" 顺时针转动 "); } else { motor.step(-steps); Serial.print("电机"); Serial.print(motorID); Serial.print(" 逆时针转动 "); } Serial.print(steps); Serial.println("步"); delay(300); } // 新增舵机控制函数 void controlServo(String cmd, Servo &servo, char servoID) { if (cmd.length() < 2) { Serial.println("错误:无效舵机命令"); return; } int angle = cmd.substring(1).toInt(); angle = constrain(angle, 0, 180); // 限制角度范围 servo.write(angle); Serial.print("舵机"); Serial.print(servoID); Serial.print(" 已转到:"); Serial.print(angle); Serial.println("度"); delay(300); }

将下列代码改为通过esp8266与点灯科技相结合,实现能够在点灯科技上能够实现下列代码功能,输出完整代码。#include <Stepper.h> #include <Servo.h> // 步进电机引脚定义 #define MOTOR_A_PIN1 8 #define MOTOR_A_PIN2 9 #define MOTOR_A_PIN3 10 #define MOTOR_A_PIN4 11 #define MOTOR_B_PIN1 4 #define MOTOR_B_PIN2 5 #define MOTOR_B_PIN3 6 #define MOTOR_B_PIN4 7 // 舵机引脚定义 #define SERVO_C_PIN 2 // 新增舵机C引脚 #define SERVO_D_PIN 3 // 新增舵机D引脚 #define STEPS_PER_REV 2048 #define MAX_STEPS 40960 // 40960步(20圈保护) // 全局对象声明 Stepper motorA(STEPS_PER_REV, MOTOR_A_PIN1, MOTOR_A_PIN3, MOTOR_A_PIN2, MOTOR_A_PIN4); Stepper motorB(STEPS_PER_REV, MOTOR_B_PIN1, MOTOR_B_PIN3, MOTOR_B_PIN2, MOTOR_B_PIN4); Servo servoC; // 声明舵机C对象 Servo servoD; // 声明舵机D对象 // 初始化设置 void setup() { Serial.begin(9600); // 步进电机初始化 motorA.setSpeed(12); motorB.setSpeed(12); // 舵机初始化 servoC.attach(SERVO_C_PIN); // 连接舵机C servoD.attach(SERVO_D_PIN); // 连接舵机D Serial.println(“双步进电机+双舵机控制系统就绪”); Serial.println(“命令格式:”); Serial.println(“步进电机:A±步数 或 B±步数(例:A+2048)”); Serial.println(“舵机:C角度 或 D角度(例:C90,范围0-180)”); } // 主循环 void loop() { if (Serial.available() > 0) { String input = Serial.readStringUntil(‘\n’); input.trim(); if (input.startsWith("A")) { controlMotor(input, motorA, 'A'); } else if (input.startsWith("B")) { controlMotor(input, motorB, 'B'); } else if (input.startsWith("C")) { // 舵机C控制 controlServo(input, servoC, 'C'); } else if (input.startsWith("D")) { // 舵机D控制 controlServo(input, servoD, 'D'); } } } // 步进电机控制函数 void controlMotor(String cmd, Stepper &motor, char motorID) { if (cmd.length() < 2 || (cmd[1] != ‘+’ && cmd[1] != ‘-’)) { Serial.println(“错误:无效命令格式”); return; } long steps = cmd.substring(1).toInt(); steps = constrain(abs(steps), 0, MAX_STEPS); if (cmd[1] == ‘+’) { motor.step(steps); Serial.print(“电机”); Serial.print(motorID); Serial.print(" 顺时针转动 “); } else { motor.step(-steps); Serial.print(“电机”); Serial.print(motorID); Serial.print(” 逆时针转动 "); } Serial.print(steps); Serial.println(“步”); delay(300); } // 新增舵机控制函数 void controlServo(String cmd, Servo &servo, char servoID) { if (cmd.length() < 2) { Serial.println(“错误:无效舵机命令”); return; } int angle = cmd.substring(1).toInt(); angle = constrain(angle, 0, 180); // 限制角度范围 servo.write(angle); Serial.print(“舵机”); Serial.print(servoID); Serial.print(" 已转到:"); Serial.print(angle); Serial.println(“度”); delay(300); }

2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 /* XY-2.5AD-Demo * 太极创客 www.taichi-maker.com * 2018-08-02 * * 通过串行通讯使用XY-2.5AD控制两个DC电机 * 通过digitalWrite HIGH LOW 控制电机运行和停止。 * * 如果需要获取更多有关XY-2.5AD控制电机的相关知识,请前往太极创客网站 * www.taichi-maker.com * * XY-2.5AD 控制电机简介 * * DC电机 运行状态 IN1 IN2 IN3 IN4 * 电机A 正转(调速) 1/PWM 0 * 电机A 反转(调速) 0 1/PWM * 空转 0 0 * 刹车 1 1 * 电机B 正转(调速) 1/PWM 0 * 电机B 反转(调速) 0 1/PWM * 空转 0 0 * 刹车 1 1 * This example code is in the public domain. */ // XY-2.5AD 连接Arduino引脚编号 int IN1 = 3; int IN2 = 5; int IN3 = 6; int IN4 = 9; int pinNum; // 控制引脚号 int ctrlVal; // 电机运行控制 void setup() { pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT); Serial.begin(9600); Serial.println("++++++++++++++++++++++++++++++"); Serial.println("+ Taichi-Maker XY-2.5AD Demo +"); Serial.println("+ www.taichi-maker.com +"); Serial.println("++++++++++++++++++++++++++++++"); } void loop() { if (Serial.available()) { // 检查串口缓存是否有数据等待传输 char cmd = Serial.read(); // 获取电机指令中电机编号信息 switch(cmd){ case 'p': // 设置引脚编号 pinNum = Serial.parseInt(); Serial.print("Pin Number "); Serial.print(pinNum); Serial.print(" ,"); break; case 'a': // 模拟模式控制电机 ctrlVal = Serial.parseInt(); analogWrite(pinNum, ctrlVal); Serial.print("Set Value "); Serial.print(ctrlVal); Serial.println("."); break; case 'd': // 数字模式控制电机 ctrlVal = Serial.parseInt(); digitalWrite(pinNum, ctrlVal); Serial.print("Set Value "); Serial.print(ctrlVal); Serial.println("."); break; default: // 未知指令 Serial.println("Unknown Command"); break; } } }请修改为两个电机

大家在看

最新推荐

recommend-type

基于PLC控制的锅炉自动输煤系统标准设计.doc

基于PLC控制的锅炉自动输煤系统标准设计.doc
recommend-type

tiktoken-0.3.2-cp38-cp38-musllinux_1_1_aarch64.whl

该资源为tiktoken-0.3.2-cp38-cp38-musllinux_1_1_aarch64.whl,欢迎下载使用哦!
recommend-type

ASP.NET新闻管理系统:用户管理与内容发布功能

知识点: 1. ASP.NET 概念:ASP.NET 是一个开源、服务器端 Web 应用程序框架,用于构建现代 Web 应用程序。它是 .NET Framework 的一部分,允许开发者使用 .NET 语言(例如 C# 或 VB.NET)来编写网页和 Web 服务。 2. 新闻发布系统功能:新闻发布系统通常具备用户管理、新闻分级、编辑器处理、发布、修改、删除等功能。用户管理指的是系统对不同角色的用户进行权限分配,比如管理员和普通编辑。新闻分级可能是为了根据新闻的重要程度对它们进行分类。编辑器处理涉及到文章内容的编辑和排版,常见的编辑器有CKEditor、TinyMCE等。而发布、修改、删除功能则是新闻发布系统的基本操作。 3. .NET 2.0:.NET 2.0是微软发布的一个较早版本的.NET框架,它是构建应用程序的基础,提供了大量的库和类。它在当时被广泛使用,并支持了大量企业级应用的构建。 4. 文件结构分析:根据提供的压缩包子文件的文件名称列表,我们可以看到以下信息: - www.knowsky.com.txt:这可能是一个文本文件,包含着Knowsky网站的一些信息或者某个页面的具体内容。Knowsky可能是一个技术社区或者文档分享平台,用户可以通过这个链接获取更多关于动态网站制作的资料。 - 源码下载.txt:这同样是一个文本文件,顾名思义,它可能包含了一个新闻系统示例的源代码下载链接或指引。用户可以根据指引下载到该新闻发布系统的源代码,进行学习或进一步的定制开发。 - 动态网站制作指南.url:这个文件是一个URL快捷方式,它指向一个网页资源,该资源可能包含关于动态网站制作的教程、指南或者最佳实践,这对于理解动态网站的工作原理和开发技术将非常有帮助。 - LixyNews:LixyNews很可能是一个项目文件夹,里面包含新闻发布系统的源代码文件。通常,ASP.NET项目会包含多个文件,如.aspx文件(用户界面)、.cs文件(C#代码后台逻辑)、.aspx.cs文件(页面的代码后台)等。这个文件夹中应该还包含Web.config配置文件,它用于配置整个项目的运行参数和环境。 5. 编程语言和工具:ASP.NET主要是使用C#或者VB.NET这两种语言开发的。在该新闻发布系统中,开发者可以使用Visual Studio或其他兼容的IDE来编写、调试和部署网站。 6. 新闻分级和用户管理:新闻分级通常涉及到不同的栏目分类,分类可以是按照新闻类型(如国际、国内、娱乐等),也可以是按照新闻热度或重要性(如头条、焦点等)进行分级。用户管理则是指系统需具备不同的用户身份验证和权限控制机制,保证只有授权用户可以进行新闻的发布、修改和删除等操作。 7. 编辑器处理:一个新闻发布系统的核心组件之一是所使用的Web编辑器。这个编辑器可以是内置的简单文本框,也可以是富文本编辑器(WYSIWYG,即所见即所得编辑器),后者能够提供类似于Word的编辑体验,并能输出格式化后的HTML代码。CKEditor和TinyMCE是常用的开源Web编辑器,它们支持插入图片、视频、表格等多种媒体,并能对文本进行复杂的格式化操作。 8. 发布、修改和删除功能:这是新闻发布系统的基本操作功能。发布功能允许用户将编辑好的新闻内容上线;修改功能可以对已发布的新闻内容进行更新;删除功能则用于移除不再需要的新闻文章。这些操作通常需要后台管理界面来支持,并且系统会在数据库中记录相关操作的记录,以便管理历史版本和审计日志。 以上知识点覆盖了从ASP.NET基础、新闻发布系统的具体功能实现到系统开发过程中的细节处理等多个方面。开发者在构建类似系统时,需要深入了解这些知识点,才能设计和实现一个功能完备、易用、安全的新闻发布系统。
recommend-type

【实战派量化投资秘籍】:Pair Trading策略全方位解析

# 摘要 量化投资中的Pair Trading策略是一种依赖统计套利和市场效率假说的交易方法,其核心在于选择相关性高的资产作为交易对并应用协整理论进行市场中立投资。本文首先概述了Pair Trading策略的理论基础,随后详细探讨了策略的实操技巧,包括数据预处理、模型建立和交易信号生成。接着,文章重点介绍了策略的编程实现,提供了环境搭建
recommend-type

fpga中保持时间建立时间时序约束

<think>我们讨论的是FPGA中的建立时间(Setup Time)和保持时间(Hold Time)时序约束问题。建立时间是指在时钟有效边沿到来之前,数据必须保持稳定的最小时间。保持时间是指在时钟有效边沿到来之后,数据必须保持稳定的最小时间。时序约束就是确保设计满足这些时间要求。 在FPGA设计中,我们通过时序约束(如时钟约束、输入输出延迟约束等)来告知工具设计的时序要求,工具会根据这些约束进行优化和验证。 以下是关于建立时间和保持时间时序约束的详细说明: ### 1. 建立时间和保持时间的基本概念 - **建立时间(Setup Time)**:时钟边沿到达前,数据必须稳定的时间。 -
recommend-type

Notepad2: 高效替代XP系统记事本的多功能文本编辑器

### 知识点详解 #### 标题解析 - **Vista记事本(Notepad2)**: Vista记事本指的是一款名为Notepad2的文本编辑器,它不是Windows Vista系统自带的记事本,而是一个第三方软件,具备高级编辑功能,使得用户在编辑文本文件时拥有更多便利。 - **可以替换xp记事本Notepad**: 这里指的是Notepad2拥有替换Windows XP系统自带记事本(Notepad)的能力,意味着用户可以安装Notepad2来获取更强大的文本处理功能。 #### 描述解析 - **自定义语法高亮**: Notepad2支持自定义语法高亮显示,可以对编程语言如HTML, XML, CSS, JavaScript等进行关键字着色,从而提高代码的可读性。 - **支持多种编码互换**: 用户可以在不同的字符编码格式(如ANSI, Unicode, UTF-8)之间进行转换,确保文本文件在不同编码环境下均能正确显示和编辑。 - **无限书签功能**: Notepad2支持设置多个书签,用户可以根据需要对重要代码行或者文本行进行标记,方便快捷地进行定位。 - **空格和制表符的显示与转换**: 该编辑器可以将空格和制表符以不同颜色高亮显示,便于区分,并且可以将它们互相转换。 - **文本块操作**: 支持使用ALT键结合鼠标操作,进行文本的快速选择和编辑。 - **括号配对高亮显示**: 对于编程代码中的括号配对,Notepad2能够高亮显示,方便开发者查看代码结构。 - **自定义代码页和字符集**: 支持对代码页和字符集进行自定义,以提高对中文等多字节字符的支持。 - **标准正则表达式**: 提供了标准的正则表达式搜索和替换功能,增强了文本处理的灵活性。 - **半透明模式**: Notepad2支持半透明模式,这是一个具有视觉效果的功能,使得用户体验更加友好。 - **快速调整页面大小**: 用户可以快速放大或缩小编辑器窗口,而无需更改字体大小。 #### 替换系统记事本的方法 - **Windows XP/2000系统替换方法**: 首先关闭系统文件保护,然后删除系统文件夹中的notepad.exe,将Notepad2.exe重命名为notepad.exe,并将其复制到C:\Windows和C:\Windows\System32目录下,替换旧的记事本程序。 - **Windows 98系统替换方法**: 直接将重命名后的Notepad2.exe复制到C:\Windows和C:\Windows\System32目录下,替换旧的记事本程序。 #### 关闭系统文件保护的方法 - 通过修改Windows注册表中的"SFCDisable"键值,可以临时禁用Windows系统的文件保护功能。设置键值为"FFFFFF9D"则关闭文件保护,设置为"0"则重新启用。 #### 下载地址 - 提供了Notepad2的下载链接,用户可以通过该链接获取安装包。 #### 文件压缩包内文件名 - **Notepad2MOD1.1.0.8CN.exe**: 这是压缩包内所含的Notepad2编译版本,表明这是一个中文版的安装程序,版本号为1.1.0.8。 ### 总结 Notepad2是一款强大的文本编辑器,它继承了传统的记事本程序界面,同时引入了诸多增强功能,如语法高亮、编码格式转换、书签管理、文本操作快捷键、括号高亮匹配等。这使得它在处理代码、标记语言和其他文本文件时具备极大的优势。用户可以通过替换系统默认记事本的方式,将Notepad2融入到操作系统中,充分享受这些高级功能带来的便捷。同时,提供了关闭系统文件保护的方法,以便用户能够顺利完成替换工作。最后,给出了下载地址,方便用户获取软件安装包。
recommend-type

【mPower1203驱动故障全攻略】:排除新手疑难杂症,提升部署效率

# 1. mPower1203驱动概述与故障诊断基础 在当今信息技术飞速发展的时代,高效准确地诊断和解决驱动故障对于确保企业级IT系统的稳定运行至关重要。mPower1203驱动作为一个广泛应用于高性能计算和数据处理领域的驱动程序,它的稳定性和性能优化对于很多关键业务系统都是不可忽视的。本章节将为您提供一个mPower1203驱动的概述,并对故障诊断的
recommend-type

keil5打不开

<think>我们正在处理Keil 5无法打开的问题。根据引用资料,可能的原因和解决方案如下: 1. 工程路径过长:将工程移动到较短的路径下(如D:\Project)[^3]。 2. 需要安装C51支持包(如果涉及51单片机)[^2]。 3. 编译器配置问题:重新安装或添加ARM编译器[^4]。 4. 软件冲突:以管理员身份运行Keil,并确保安装路径正确。 下面详细说明解决方案:</think>### Keil 5无法打开的解决方法 根据引用资料,Keil 5无法打开通常有以下几种原因及解决方案: #### 1. **工程路径过长问题** [^3] 当工程文件路径嵌套过深时(
recommend-type

远程进程注入技术详解:DLL注入的实现步骤

标题中提到的"RemoteCall"是一种远程进程注入技术,其关键知识点围绕着如何在不直接操作目标进程的情况下,在远程进程内存空间中加载和执行代码。这一技术广泛应用于多个领域,包括但不限于恶意软件开发、安全测试、系统管理工具等。下面,我们将深入探讨这一技术的关键步骤以及涉及的相关技术概念。 ### 进程ID的获取 要对远程进程进行操作,首先需要知道该进程的标识符,即进程ID(Process Identifier,PID)。每个运行中的进程都会被操作系统分配一个唯一的进程ID。通过系统调用或使用各种操作系统提供的工具,如Windows的任务管理器或Linux的ps命令,可以获取到目标进程的PID。 ### 远程进程空间内存分配 进程的内存空间是独立的,一个进程不能直接操作另一个进程的内存空间。要注入代码,需要先在远程进程的内存空间中分配一块内存区域。这一操作通常通过调用操作系统提供的API函数来实现,比如在Windows平台下可以使用VirtualAllocEx函数来在远程进程空间内分配内存。 ### 写入DLL路径到远程内存 分配完内存后,接下来需要将要注入的动态链接库(Dynamic Link Library,DLL)的完整路径字符串写入到刚才分配的内存中。这一步是通过向远程进程的内存写入数据来完成的,同样需要使用到如WriteProcessMemory这样的API函数。 ### 获取Kernel32.dll中的LoadLibrary地址 Kernel32.dll是Windows操作系统中的一个基本的系统级动态链接库,其中包含了许多重要的API函数。LoadLibrary函数用于加载一个动态链接库模块到指定的进程。为了远程调用LoadLibrary函数,必须首先获取到这个函数在远程进程内存中的地址。这一过程涉及到模块句柄的获取和函数地址的解析,可以通过GetModuleHandle和GetProcAddress这两个API函数来完成。 ### 创建远程线程 在有了远程进程的PID、分配的内存地址、DLL文件路径以及LoadLibrary函数的地址后,最后一步是创建一个远程线程来加载DLL。这一步通过调用CreateRemoteThread函数来完成,该函数允许调用者指定一个线程函数地址和一个参数。在这里,线程函数地址就是LoadLibrary函数的地址,参数则是DLL文件的路径。当远程线程启动后,它将在目标进程中执行LoadLibrary函数,从而加载DLL,实现代码注入。 ### 远程进程注入的应用场景与风险 远程进程注入技术的应用场景十分广泛。在系统管理方面,它允许用户向运行中的应用程序添加功能,如插件支持、模块化更新等。在安全领域,安全工具会使用注入技术来提供深度防护或监控。然而,远程进程注入技术也具有极高的风险性,特别是当被用于恶意软件时,它能够被用来注入恶意代码,对用户系统的安全性和稳定性造成威胁。因此,了解这一技术的同时,也必须对其潜在的安全风险有所认识,特别是在进行系统安全防护时,需要对该技术进行检测和防护。 ### 结语 通过对"RemoteCall"远程线程注入技术的知识点分析,我们了解到这一技术的强大能力,以及它在安全测试、系统维护和潜在恶意软件开发中的双重作用。掌握远程进程注入技术不仅要求对操作系统和编程有深入了解,还要求具备应对潜在安全风险的能力。在未来,随着技术的发展和安全挑战的增加,对这类技术的掌握和应用将变得更加重要。
recommend-type

【驱动安装背后的故事】:mPower1203机制深度剖析及优化技巧

# 1. 驱动安装机制概述 ## 简介 驱动安装机制是指操作系统与计算机硬件设备之间交互的过程。这一过程涉及到驱动软件的识别、加载和初始化,确保硬件设备能够在操作系统中正确、高效地运行。本章节将从宏观角度对驱动安装机制进行概述,为深入探讨特定驱动如mPower1203提供基础。 ## 驱动安装的步骤 一