Saturday, December 27, 2008

MINDSTORMS CATERPILLAR

I enhanced the code a little bit that the cat can move without seriously hitting any objects. It is steered by the ultrasonic-, touch- and light sensor. I wrote a Java class to let the cat find a black spot by avoiding barriers. It’s kind of an ad hoc code, I’ll optimise it during the next days and will let it look a bit more organised.

After reorganising the code I think it’s a good basis to add more intelligence to it, maybe something for remembering the route and optimising the path.

But for now it does it’s job:



Java class:

package tospinout;

import lejos.nxt.*;

/**
* 2008-12-27 author 2spinout
*/

public class Cat
{
static TouchSensor ts = new TouchSensor(SensorPort.S3); //initialize Touchsensor
static UltrasonicSensor us = new UltrasonicSensor(SensorPort.S4); //initialize Touchsensor
static LightSensor ls = new LightSensor(SensorPort.S2); //initialze Lightsensor

//drive train
public static void main (String[] aArg)
throws Exception
{
// PortA = Motor.A = ultrasonic movement motor
// PortB = Motor.B = left driving motor
// PortC = Motor.C = right driving motor
// Port1 = SoundSensor
// Port2 = LightSensor
// Port3 = TouchSensor
// Port4 = UltrasonicSensor

LCD.drawString("2spinout",3,4);
Thread.sleep(2000);
ls.setFloodlight(true);

for(int COUNT = 0 ; COUNT < 20 ; COUNT += 1)
{
int stop = 0;
int light = 100;

Motor.A.setSpeed(400);
Motor.B.setSpeed(400);//720=2RPM
Motor.C.setSpeed(400);
Motor.B.forward();
Motor.C.forward();
Thread.sleep(1000);

do {
LCD.drawString("--> forward",3,4);
for(int count2 = -90 ; count2 < 91 ; count2 +=45)

{
light = ls.readValue();

if ((us.getDistance() < 45)|| (Motor.B.getActualSpeed() <390) || (Motor.C.getActualSpeed() < 390)||(light < 40)||(ts.isPressed()==true))

{
Motor.B.stop(); //stop driving engines
Motor.C.stop();
stop = 1;
}
Motor.A.rotateTo(count2);
}

Motor.A.rotateTo(0);
if (light<40)
{
break;
}

} while(stop != 1);

if (light<40)
{
break;
}
Thread.sleep (1000);


//turn back
Motor.B.setSpeed(400);//720=2RPM
Motor.C.setSpeed(400);
Motor.B.backward();
Motor.C.backward();
LCD.drawString("<-- backward",3,4);
Thread.sleep (1000);
Motor.B.stop(); //stop driving engines
Motor.C.stop();
Thread.sleep (1000);

//find best way / best distance
int left = 0;
int right = 0;

//check left
Motor.A.rotateTo(-90);
left = us.getDistance();
Thread.sleep(500);

//check right
Motor.A.rotateTo(90);
right = us.getDistance();
Thread.sleep(500);

Motor.A.rotateTo(0);
Thread.sleep(500);

if (left>right)
{
//turn left
Motor.B.setSpeed(400);//720=2RPM
Motor.C.setSpeed(400);
Motor.B.backward();
Motor.C.forward();
LCD.drawString("-^ turn left",3,4);
Thread.sleep (1200);
Motor.B.stop(); //stop driving engines
Motor.C.stop();
Thread.sleep(1000);
}

else
{
//turn right
Motor.B.setSpeed(400);//720=2RPM
Motor.C.setSpeed(400);
Motor.B.forward();
Motor.C.backward();
LCD.drawString("-v turn right",3,4);
Thread.sleep (1200);
Motor.B.stop(); //stop driving engines
Motor.C.stop();
Thread.sleep (1000);
}

}

ls.setFloodlight(false);

}
}

No comments: