lineformation.java

利用JAVA编写的群体机器人局部通讯完成一定得队形控制

/*
 * LineFormation.java
 */
 
package Domains.LineFormation;

import	EDU.gatech.cc.is.util.Vec2;
import	EDU.gatech.cc.is.abstractrobot.*;
import	EDU.gatech.cc.is.clay.*;
import  java.io.*;

import java.awt.*;
import java.net.*;
import java.awt.event.*;
import javax.swing.*;



/**
 * Swarm robots LineFormation control(2007/01/28).
 * @author Yongming Yang &yxh
 * @version $Revision: 1.0 $
 */

public class LineFormation extends ControlSystemMFN150
	{
	public final static boolean DEBUG = true;
	public static boolean isRecorded = false;
	
	private NodeVec2	turret_configuration;
	private NodeVec2	steering_configuration;
	private NodeInt 	state_monitor;
	private i_FSA_ba 	STATE_MACHINE;
	int run_i;
	boolean change = false;
	//SimulationCanvas si = new SimulationCanvas();
	//Vec2  robot_p = abstract_robot.getPosition(-1);  
	
	/**
	 * Configure the control system using Clay.
	 */
	public void configure()
		{
		//======
		// Set some initial hardware configurations.
		//======
		abstract_robot.setObstacleMaxRange(3.0); // don't consider things further away
	 	abstract_robot.setBaseSpeed(0.8*abstract_robot.MAX_TRANSLATION);	
		
		//======
		// perceptual schemas
		//======
		
		
		//--- robot's global position
		NodeVec2
		PS_GLOBAL_POS = new v_GlobalPosition_r(abstract_robot);

		//--- obstacles
		NodeVec2Array // the sonar readings
		PS_OBS = new va_Obstacles_r(abstract_robot);

		//--- homebase 
		NodeVec2      // the place to deliver
		//PS_HOMEBASE0_GLOBAL = new v_FixedPoint_(30.0,-8.0);
		PS_HOMEBASE0_GLOBAL = new v_FixedPoint_(30.0,0.0);
		
	
		NodeVec2      // make it egocentric  自我中心
		PS_HOMEBASE0 = new v_GlobalToEgo_rv(abstract_robot,
				PS_HOMEBASE0_GLOBAL);

		//======
		// Perceptual Features
		//======
		// close to homebase 
		NodeBoolean
		PF_CLOSE_TO_HOMEBASE0 = new b_Close_vv(0.4, PS_GLOBAL_POS,
			PS_HOMEBASE0_GLOBAL);


		//======
		// motor schemas
		//======
		// avoid obstacles
		NodeVec2
		MS_AVOID_OBSTACLES = new v_Avoid_va(1.5,
			abstract_robot.RADIUS + 0.1,
			PS_OBS);
			
		// swirl obstacles wrt homebase0
		NodeVec2
		MS_SWIRL_OBSTACLES_HOMEBASE0 = new v_Swirl_vav(2.0,
			abstract_robot.RADIUS + 0.22,
			PS_OBS,
			PS_HOMEBASE0);

		// go home 0
		NodeVec2
		MS_MOVE_TO_HOMEBASE0 = new v_LinearAttraction_v(0.4,0.0,PS_HOMEBASE0);	

		// noise vector
		NodeVec2
		MS_NOISE_VECTOR = new v_Noise_(5,seed);

		// swirl obstacles wrt noise
		NodeVec2
		MS_SWIRL_OBSTACLES_NOISE = new v_Swirl_vav(2.0,
			abstract_robot.RADIUS + 0.1,
			PS_OBS,
			MS_NOISE_VECTOR);
			
		// maintain line_formation  0-8
		NodeVec2
		MS_LINE_FORMATION = new v_LineFormation_r(0,2.5,1,0.0,abstract_robot);
		
		// maintain line_formation0  8-0
		NodeVec2
		MS_LINE_FORMATION0 = new v_LineFormation0_r(0,2.5,1.0,0.0,abstract_robot);
		
	
	    
		//======
		// AS_WANDER
		//======
		v_StaticWeightedSum_va 
		AS_WANDER = new v_StaticWeightedSum_va();

		AS_WANDER.weights[0]  = 1.0;
		AS_WANDER.embedded[0] = MS_AVOID_OBSTACLES;

		AS_WANDER.weights[1]  = 1.0;
		AS_WANDER.embedded[1] = MS_NOISE_VECTOR;

		AS_WANDER.weights[2]  = 1.0;
		AS_WANDER.embedded[2] = MS_SWIRL_OBSTACLES_NOISE;
	

		//======
		// AS_DELIVER0
		//======
		v_StaticWeightedSum_va
		AS_DELIVER0 = new v_StaticWeightedSum_va();

		AS_DELIVER0.weights[0]  = 1.5;
		AS_DELIVER0.embedded[0] = MS_AVOID_OBSTACLES;

		AS_DELIVER0.weights[1]  = 0.8;
		AS_DELIVER0.embedded[1] = MS_MOVE_TO_HOMEBASE0;

		AS_DELIVER0.weights[2]  = 1.5;
		AS_DELIVER0.embedded[2] = MS_SWIRL_OBSTACLES_HOMEBASE0;

		AS_DELIVER0.weights[3]  = 0.1;
		AS_DELIVER0.embedded[3] = MS_NOISE_VECTOR;
		
		AS_DELIVER0.weights[4]  = 1.5;//1.8
		AS_DELIVER0.embedded[4] = MS_LINE_FORMATION;
		
		AS_DELIVER0.weights[5]  = 1.5;//1.8
		AS_DELIVER0.embedded[5] = MS_LINE_FORMATION0;
		

		//======
		// STATE_MACHINE
		//======
		STATE_MACHINE = new i_FSA_ba();

		STATE_MACHINE.state = 1;
		
		// STATE 0 WANDER
		//STATE_MACHINE.triggers[0][0]  = PF_TARGET0_VISIBLE;
		//STATE_MACHINE.follow_on[0][0] = 1; // transition to ACQUIRE0
	
		// STATE 1 DELIVER0
		STATE_MACHINE.triggers[1][0]  =PF_CLOSE_TO_HOMEBASE0;
		STATE_MACHINE.follow_on[1][0] =0 ;// transition to deliver
		
	
		state_monitor = STATE_MACHINE;


		//======
		// STEERING
		//======
		v_Select_vai
		STEERING = new v_Select_vai((NodeInt)STATE_MACHINE);

		STEERING.embedded[0] = AS_WANDER;	
		STEERING.embedded[1] = AS_DELIVER0;
	
		//======
		// TURRET
		//======
		v_Select_vai
		TURRET = new v_Select_vai((NodeInt)STATE_MACHINE);

		TURRET.embedded[0] = AS_WANDER;	
		TURRET.embedded[1] = AS_DELIVER0;


		turret_configuration = TURRET;
		steering_configuration = STEERING;	
		}
		
	/**
	 * Called every timestep to allow the control system to
	 * run.
	 */
	public int takeStep()
		{
		Vec2	result;
		double	dresult;
		long	curr_time = abstract_robot.getTime();
		Vec2	p;
		

		int robot_id = abstract_robot.getID(-1);

		// STEER
		result = steering_configuration.Value(curr_time);
		//robot_v [run_i][robot_id] = result.r;
		//robot_v [run_i] = result.r;
		//run_i++;
		//System.out.println(abstract_robot.getID()+" "+result.r);
		//System.out.println(run_i);
		//System.out.println("robot_v["+run_i+"]["+robot_id+"]="+result.r);
		
		/*try
		{
			/* ------return useful data------*/
			/*PrintWriter out0 = new PrintWriter(new FileWriter("E:\\yxh\\TBtest\\speed0.txt",true));
			PrintWriter out1 = new PrintWriter(new FileWriter("E:\\yxh\\TBtest\\speed1.txt",true));
			PrintWriter out2 = new PrintWriter(new FileWriter("E:\\yxh\\TBtest\\speed2.txt",true));
			PrintWriter out3 = new PrintWriter(new FileWriter("E:\\yxh\\TBtest\\speed3.txt",true));
			PrintWriter out4 = new PrintWriter(new FileWriter("E:\\yxh\\TBtest\\speed4.txt",true));
			PrintWriter out5 = new PrintWriter(new FileWriter("E:\\yxh\\TBtest\\speed5.txt",true));
			PrintWriter out6 = new PrintWriter(new FileWriter("E:\\yxh\\TBtest\\speed6.txt",true));
			PrintWriter out7 = new PrintWriter(new FileWriter("E:\\yxh\\TBtest\\speed7.txt",true));
			PrintWriter out8 = new PrintWriter(new FileWriter("E:\\yxh\\TBtest\\speed8.txt",true));
			
 			if ((curr_time%1000)==0)
 			switch(robot_id)
 			{
 				//case 0:
 				//out0.println(robot_p.x+"  "+robot_p.y);
 				//out0.close();
				//break;
				case 0:
				out0.println(result.r);
				out0.close();
				break;	
				case 1:
 				out1.println(result.r);				
				out1.close();
				break;	
				case 2:
 				out2.println(result.r);				
				out2.close();
				break;	
				case 3:
 				out3.println(result.r);				
				out3.close();
				break;	
				case 4:
 				out4.println(result.r);			
				out4.close();
				break;	
				case 5:
 				out5.println(result.r);
				out5.close();
				break;	
				case 6:
 				out6.println(result.r);				
				out6.close();
				break;	
				case 7:
 				out7.println(result.r);
				out7.close();
				break;	
				case 8:
 				out8.println(result.r);
				out8.close();
				break;	
 			}
 			
			
		}
		catch(Exception e){		}
		/*------finishing returning------*/
		
		
		abstract_robot.setSteerHeading(curr_time, result.t);
		abstract_robot.setSpeed(curr_time, result.r);

		// TURRET
		result = turret_configuration.Value(curr_time);
		abstract_robot.setTurretHeading(curr_time, result.t);	

		// STATE DISPLAY
		int state = STATE_MACHINE.Value(curr_time);
		if (state == 0)
			abstract_robot.setDisplayString("wander");
		else if (state == 1)
			abstract_robot.setDisplayString("deliver");	
	
				
		/*if ( !isRecorded && robot_p.x >= 29.6 ) // near target
		{		   
			/******* Recording finishing time ********/
		/* 	try
		    {
		       	PrintWriter out = new PrintWriter(new FileWriter("E:\\yxh\\TBtest\\timefile.txt",true));
		    	out.println("Robot "+abstract_robot.getID()
		    				+" records. The finishing time: "
		    				+(float)abstract_robot.getTime()/1000);  
		    	out.println("--------------------------------------------");		    			    	
		    	out.close();
		    	isRecorded =true;
		    	
		    	System.out.println("The finish time is recorded !");
		    	
		    }
		    catch(Exception e)
		    {
		    	System.out.println("File Open Error!");
		    }		    		
			/**************Recording end***************/		
		//}
		return(CSSTAT_OK);
		}
	}