Search algorithm for the pathing

Back to: PRE2015 2 Groep1

In order for the swarm to search an targeted area for objects, an search algorithm is needed. This search algorithm will determine the pathing of the robots, the choices made by the robots will be based on sensory input, primarily from the Ultrasound Sensor. Since the current basic model for the NXT Robots does not have an rotary actuator attached to the Ultrasound Sensor, the robots will only be able to sense a cone infront of them. The current setup for this searching algorithm is given below, where the following basics are set up:

• Sensing the distance from object infront of the robot while driving straight. When the distance gets below a certain preset threshold, the robot will enter another state which still needs to be programmed.
• Sensing the distance from object infront of the robot while turning left or right.
• Sensing when it it pushing an object, and if needed send out a signal for assistance of the swarm.

package com.mydomain;

import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.util.Delay;
import lejos.nxt.SensorPort;
//import lejos.nxt.TouchSensor;
import lejos.nxt.*;


public class HelloWorld {
    /**
     * @param args
     */
	public static int  speed = 1000;
	public static int state = 1;
    public static void main(String[] args) {
    	
    	
    	// New local variables for sensor usage
    	UltrasonicSensor sonic     =   new UltrasonicSensor(SensorPort.S2);
    	TouchSensor touchleft      =   new TouchSensor(SensorPort.S1);
    	TouchSensor touchright     =   new TouchSensor(SensorPort.S4);
    	
    	// Standard Starting Protocol
        System.out.println("Hello World!");
        Button.waitForAnyPress();
        
        
        //Search State
     for (int t = 0; t < 100000000; t++) {
        switch ( state ) {
        
        	case 1: //searching straight
        		for (int i = 0; i <2000; i++) {
        
        			while (sonic.getDistance() > 40) {
        			
        				Motor.A.backward();
        				Motor.A.setSpeed(speed);
        				Motor.C.backward();
        				Motor.C.setSpeed(speed);
        			}
        		}
        	
        		if (sonic.getDistance() < 40) {
        			state = 3; 
        		}
        		else {
        			state = 2;
        		}
        		
        	
        break;
        
        	case 2: //turning right
        		for (int i = 0; i <500; i++) {
                
        			while (sonic.getDistance() > 40) {
        		
        				Motor.A.backward();
        				Motor.A.setSpeed(speed);
        				Motor.C.forward();
        				Motor.C.setSpeed(speed);
        			}
        		}
        		
        		if (sonic.getDistance() < 40) {
            		state = 3; 
            	}
            	else {
            		state = 1;
            	}
        	
        
        //Found State
        case 3: // pushing
		for (int i = 0; i<500; i++) {

			while (touch.getTouchRight() > (threshold touch) and (touch.getTouchLeft() > (threshold touch){
        	
        			Motor.A.backward();
        			Motor.A.setSpeed(speed);
        			Motor.C.backward();
        			Motor.C.setSpeed(speed);
        			Delay.msDelay(3000);
	        		t = 100000000;
			}
		}
		if (touch.getTouchRight() < (threshold touch) and (touch.getTouchLeft() < (threshold touch){
		state = 1;
	}
        break;
        }
   
    }
}
}