Turn Task 转弯

首发原创,LEEMANCAFFE

# include "Aria.h"
# include <iostream>

using namespace std;


double sp=200,ds=1000;
int Go(ArRobot* robot,double Speed,double Dist)
{
	cout << "Go Forward :"<< Dist<< " mm at speed "<<Speed <<"mm/s "<< endl;
	double Dist0 = 0;
	double dt = 200;
	while(Dist0 < Dist)
	{
		robot->lock();
		robot->enableMotors();
		if((Dist-Dist0) < 300)
		{
			robot->setVel((Dist-Dist0)/300*Speed+5);
		}
		else if(Dist0 <300 && !((Dist-Dist0)<300))
		{
			robot->setVel(Dist0/300*Speed + 5);
		}
		else
		{
			robot->setVel(Speed);
		}

		Dist0+=robot->getVel()*dt/1000;
		cout << "D=\t"<<Dist0 <<"S=\t"<<robot->getVel()<<endl;
		robot->unlock();
		ArUtil::sleep(dt);
	}
	robot->lock();
	robot->setVel(0);
	robot->unlock();
	return 1;
}

double spr=20,ag=360;
int Turn(ArRobot* robot,double Speedr,double Angle)
{
	cout << "Turn:"<< Angle << " deg at speed "<<Speedr <<"deg/s "<< endl;
	double Angle0 = 0;
	double dt = 200;
	double dirction=0;
	while(Angle0 < Angle)
	{
		robot->lock();
		robot->enableMotors();
		if((Angle-Angle0) < 45)
		{
			if(Speedr < 0)
				robot->setRotVel((Angle-Angle0)/45*Speedr - 2);
			else
				robot->setRotVel((Angle-Angle0)/45*Speedr + 2);
		}
		else if(Angle0 <30 && !((Angle-Angle0)<45))
		{
			if(Speedr < 0 )
				robot->setRotVel(Angle0/45*Speedr - 2);
			else
				robot->setRotVel(Angle0/45*Speedr + 2);
		}
		else
		{
			robot->setRotVel(Speedr);
		}
		if(robot->getRotVel()<0)
			Angle0+=-robot->getRotVel()*dt/1000;
		else
			Angle0+=robot->getRotVel()*dt/1000;
		cout << "A="<<Angle0 <<"\tSPEED-ROT=\t"<<robot->getRotVel()<<endl;
		robot->unlock();
		ArUtil::sleep(dt);
	}
	robot->lock();
	robot->setRotVel(0);
	robot->unlock();
	return 1;
}


int main(int argc, char **argv)
{
    int ret;
	string str;
  // the connection for Remote Host and Simulator
   ArTcpConnection con;
  // the connection through Serial Link to the robot
  //ArSerialConnection con;
  //ArGlobalFunctor updateCB(&update);

  ArRobot robot;

  // mandatory init
  Aria::init();

  // open the connection, if this fails exit
  if (( ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  // set the device connection on the robot
  robot.setDeviceConnection(&con);

  // try to connect, if we fail exit
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }
// add the sonar to the robot
  ArSonarDevice sonar;
  robot.addRangeDevice(&sonar);

  robot.comInt(ArCommands::SONAR, 1);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);
  
  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);
  //Go(&robot,sp,ds);
  Turn(&robot,spr,ag);
  robot.lock();
  robot.disconnect();
  robot.unlock();

  // now exit
  Aria::shutdown();
  return 0;

}

这里写图片描述

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值