小车相关的

Posted 2022-12-08 Bwz_Learning

1.C相关的 

就是那serial1 改成serial2

还有一个就是serialEvent1 改成serialEvent2

dealy_time 调节速度的,越大越快

serial2表示新小车；

serial1表示旧小车

2.导航相关的

com.openrobot.app.navi.NagivatorChannelTask

public static NavigatorTask create(IRobot robot,IExecutor executor,NagivateDestination destination)
		if(destination == null)
			return null;
		NavigatorTask task = null;
		if(destination.navigatorMethod == null || destination.navigatorMethod.equals(""))
			task = new NagivatorGridTask(robot,executor,destination);
		else if(destination.navigatorMethod.equalsIgnoreCase("grid"))
			task = new NagivatorGridTask(robot,executor,destination);
		else if(destination.navigatorMethod.equalsIgnoreCase("channel"))
			task = new NagivatorChannelTask(robot,executor,destination);
		else
			//task = new NagivatorGridTask(robot,executor,destination);
			task = new NagivatorChannelTask(robot,executor,destination);
		
		task.init();
		return task;
	

3.关于Croass的加载

4.关于NagivatorChannelTask.java中的private Coordinate[] getRoadDirection(Road curRoad, Road nextRoad,boolean cur) 方法的修改

private Coordinate[] getRoadDirection(Road curRoad, Road nextRoad,boolean cur) 
		// TODO Auto-generated method stub
		Coordinate[] r1 = curRoad.getCenters().getCoordinates();
		Coordinate[] r2 = nextRoad.getCenters().getCoordinates();
		
		double mindis = -1;
		int r1Index = -1,r2Index = -1;
		//找到r1和r2距离最近的两个点
		for(int i=0;i<r1.length;i++)
			for(int j=0;j<r2.length;j++)				
				if(i!=0 && i != r1.length-1 && j!=0 && j != r2.length-1)continue;
				
				double dx = r1[i].x - r2[j].x;
				double dy = r1[i].y - r2[j].y;
				double dis = Math.sqrt(dx*dx+dy*dy);
				
				if(mindis == -1)
					mindis = dis;
					r1Index = i;
					r2Index = j;
				else if(mindis > dis)
					mindis = dis;
					r1Index = i;
					r2Index = j;
				
			
		
		
		
		List<Coordinate> results = new ArrayList<Coordinate>(Arrays.asList(cur?r1:r2));
		if(cur && r1Index == 0)		
			Collections.reverse(results);			
		else if(!cur && r2Index !=0)			
			Collections.reverse(results);						
		
		return results.toArray(new Coordinate[results.size()]);
		
	

1.假如当前道路R1的坐标序列是P1,P2,P3；下一条道路R2的坐标序列式P4,P5,P6
2.由于配置文件中记录的坐标点顺序是任意的，所以R1和R2两个道路相接的点有四种可能：P1和P4相接，P1和P6相接，P3和P4相接，P3和P6相接
getRoadDirection()函数的作用是先找到四种相接方式到底是哪一种：对于R1来说，如果P1是接头点，则返回P3,P2,P1，否则返回P1,P2,P3
这样返回的坐标序列能确保是行进方向一致
对于R2来说，如果P4是接头点，则返回P4,P5,P6，否则将坐标序列颠倒下 

5.关于X轴Y轴方向的确定

if(mainDirection == 0 || mainDirection == 1 || mainDirection == 2)//X为主方向 

6.坐标轴方向的确定

目前这块总是假设Road是平行坐标轴的，且车子总是在Road附近

private int getMainDirectoin()
		//将道路方向简单规划为与坐标轴平行的方向(只有室内，且障碍物比较规则才可能)
		//1 x轴正向，2 x轴反向，3 y轴正向，y 轴反向
		int mainDirection = 0;		
		if(curRoadAngle <= Math.PI/4 && curRoadAngle >= -1*Math.PI/4)//X正向
			mainDirection = 1;					
		else if(curRoadAngle > Math.PI/4 && curRoadAngle <= 3*Math.PI/4)//y轴正向
			mainDirection = 3;	
		else if(curRoadAngle > 3*Math.PI/4 && 3*Math.PI/4 <= Math.PI)//X反向
			mainDirection = 2;
		else if(curRoadAngle > 3*Math.PI/4 ||  curRoadAngle <= -1*3*Math.PI/4)//x轴反向
			mainDirection = 2;		
		else//y 轴反向
			mainDirection = 4;	
		
		return mainDirection;
	

7.是否到达目的点的判断

public boolean isArrive(Polygon destinationRange) 
		if(destinationRange == null)
			return false;
		StatePosition curPos = robot.getStateCollection().getValue(StateCollection.POSITION);
		//Point pt = GeometryUtil.createPoint(curPos.getCoord());
		//判断当前点和目的点的距离，距离小于10cm，就认为到达目的点了
		boolean isArriveDestination = false;
		//目的点的坐标点
		Coordinate[] coordinates = destinationRange.getCoordinates();
		for (int i = 0; i < coordinates.length; i++) 
			if( twoCoordinatesIsAlike(curPos.getCoord(),coordinates[i]) )
				isArriveDestination = true;
				break;
			
		
		return isArriveDestination;
	
	/**
	 * 判断两个点，是不是相似。
	 * 两个点的距离在15cm之内，就认为到达目的点了
	 * @param a
	 * @param b
	 * @return
	 */
	private boolean twoCoordinatesIsAlike(Coordinate a,Coordinate b)
		double juli = Math.sqrt( Math.pow((a.x - b.x), 2) + Math.pow((a.y - b.y), 2) );
		return juli > 25 ? false : true;
		
	

8.旋转之前，睡眠一会

//进行旋转
			if(rotateAction!=null && rotateAction.angle>=10 && rotateAction.direction!=WalkCode.STOP)			
				//旋转之前先睡眠一会
				Common.sleep(2000);
				rotateAction.execute();
				//旋转完成也睡眠一会
				Common.sleep(2000);
			

for(int i=0;i<actionQueue.size();i++)
				IAction action = actionQueue.get(i);
				RetCode retCode = null;
				//旋转的话，先睡眠一会
				if(action.getClass().isAssignableFrom(RotateAction.class))
					Common.sleep(2000);
					retCode = action.execute();
					Common.sleep(2000);
				else //不是旋转动作，直接执行
					retCode = action.execute();
				
				

9.通过道路的时候，前进一半的距离，这样可以进行角度的修正

//移动,这个应该就是避障逻辑的修改了（注意是前进一半的距离）

MoveAction moveAction = new MoveAction(robot,WalkCode.FORWARD,mainDistance);

前进一半距离的时候，可能会造成前进的距离越来越短。所以进行简单的判断

要是前进的距离小于60cm，就直接前进60cm。不要前进一半的距离了。

StatePosition curPosition = robot.getStateCollection().getValue(StateCollection.POSITION);//当前位置信息
			double curAngle = curPosition.getAngle(); //当前角度信息
		
			//将道路方向简单规划为与坐标轴平行的方向(只有室内，且障碍物比较规则才可能)
			//1 x轴正向，2 x轴反向，3 y轴正向，4 y轴反向 ，这个地方不是很理解
			int mainDirection = this.getMainDirectoin(); //得到当前小车的坐标轴方向
			RotateAction rotateAction = computeRotateAction(mainDirection,curAngle); //根据当前角度和mainDirection，确定需要旋转的角度
		
			//计算该方向上当前点到当前道路终点的距离		
			double mainDistance  = 0;
			if(mainDirection == 0 || mainDirection == 1 || mainDirection == 2)//X为主方向 
				mainDistance = Math.abs(curPosition.getCoord().x-curRoadPoints[curRoadPoints.length-1].x);
			else
				mainDistance = Math.abs(curPosition.getCoord().y-curRoadPoints[curRoadPoints.length-1].y);
			
			
			//尝试通过该条道路的条件
			//当前目标点的坐标距离道路最后一个坐标点的距离小于5cm，就认为通过这条路径了
			if(mainDistance <= 10) //小于5厘米认为已经到了
				return RetCode.COMPLETED;
			
			
			//进行旋转
			if(rotateAction!=null && rotateAction.angle>=10 && rotateAction.direction!=WalkCode.STOP)			
				//旋转之前先睡眠一会
				Common.sleep(2000);
				rotateAction.execute();
				//旋转完成也睡眠一会
				Common.sleep(2000);
			
			//移动,这个应该就是避障逻辑的修改了
			MoveAction moveAction = new MoveAction(robot,WalkCode.FORWARD,mainDistance/2);
			RetCode retCode = moveAction.execute();

10.旋转角度

旋转角度的实现，跟电量和地面有很大的关系。这个只能依靠通过，提前确定好参数，然后通过配置文件来写入到C端的代码里面。

提前确定，旋转角度的C参数。