实时控制软件大作业二

实时控制软件机器人子系统

团队工作

• 针对机器人子系统对线性规划算法进行了改进
• 针对机器人子系统减速功能，在算法基础上添加了代码

减速功能

• 添加了减速功能
• 暂定减速为匀减速，减速度可以通过函数进行设定

代码

 if (Timer > targ_time)
	{
		
		if ((Timer/1000.0) <= ((targ_time/1000.0) + Deceleration_time_x) || (Timer/1000.0) <= ((targ_time/1000.0) + Deceleration_time_y))
		{
			//减速
			if (Velocity_new_X > Deceleration_x)
			{
				Velocity_new_X = Velocity_new_X - Deceleration_x*((Timer/1000.0) - (targ_time/1000.0));
				Position_new_X = Position_new_X + 0.5*Deceleration_x*((Timer/1000.0) - (targ_time/1000.0))*((Timer/1000.0) - (targ_time/1000.0));
				std::cout << "when t= " << Timer << " ms,Velocity_X= " << Velocity_new_X << " mm/s,Position_X= " << Position_new_X << " mm\n";
                if (Velocity_new_Y <= Deceleration_y)
				{
					Position_new_Y = Position_new_Y + 0.5*Velocity_new_Y*Velocity_new_Y / Deceleration_y;
					Velocity_new_Y = 0;
					std::cout << "when t= " << Timer << " ms,Velocity_Y= " << Velocity_new_Y << " mm/s,Position_Y= " << Position_new_Y << " mm\n";
					std::cout << "next cycle: \n";

				}
			    if (Velocity_new_Y > Deceleration_y)
			    {
				    Velocity_new_Y = Velocity_new_Y - Deceleration_y*((Timer/1000.0) - (targ_time/1000.0));
				    Position_new_Y = Position_new_Y + 0.5*Deceleration_y*((Timer/1000.0) - (targ_time/1000.0))*((Timer/1000.0) - (targ_time/1000.0));
				    std::cout << "when t= " << Timer << " ms,Velocity_Y= " << Velocity_new_Y << " mm/s,Position_Y= " << Position_new_Y << " mm\n";
				    std::cout << "next cycle: \n";
			    }
				
			}
			
			if (Velocity_new_X <= Deceleration_x)
			{
				Position_new_X = Position_new_X + 0.5*Velocity_new_X*Velocity_new_X / Deceleration_x;
				Velocity_new_X = 0;
				std::cout << "when t= " << Timer << " ms,Velocity_X= " << Velocity_new_X << " mm/s,Position_X= " << Position_new_X << " mm\n";
                if (Velocity_new_Y <= Deceleration_y)
				{
					Position_new_Y = Position_new_Y + 0.5*Velocity_new_Y*Velocity_new_Y / Deceleration_y;
					Velocity_new_Y = 0;
					std::cout << "when t= " << Timer << " ms,Velocity_Y= " << Velocity_new_Y << " mm/s,Position_Y= " << Position_new_Y << " mm\n";
					std::cout << "next cycle: \n";
					Request = false;
					Done = true;
					position_x = targ_position_x;
					position_y = targ_position_y;
					velocity_y = targ_velocity_x;
					velocity_y = targ_velocity_y;


				}
				if (Velocity_new_Y > Deceleration_y)
				{
					Velocity_new_Y = Velocity_new_Y - Deceleration_y*((Timer / 1000.0) - (targ_time/1000.0));
					Position_new_Y = Position_new_Y + 0.5*Deceleration_y*((Timer / 1000.0) - (targ_time/1000.0))*((Timer / 1000.0) - (targ_time/1000.0));
					std::cout << "when t= " << Timer << " ms,Velocity_Y= " << Velocity_new_Y << " mm/s,Position_Y= " << Position_new_Y << " mm\n";
					std::cout << "next cycle: \n";
				}
			
			}
			
		}
		
	}

参考

完整代码已经上传至github中，github仓库链接