Monday, 28 October 2019

Lego EV3 Guided Car Parking

Here we have implemented Guided EV3 Car Parking. This car is a line follower which is implemented using a light sensor. The car follows the line and reaches the "statues" which indicate that there is an existence of car parking slot in front of them. The slots may be empty or occupied. Car intercepts these statues using an Infra-red sensor. Once the car intercepts a statue, it tries to find if there is a car already parked in front of the statue. Car recognizes this with the help of Ultrasonic sensor. If the parking is already occupied, the car moves on, in search of an empty parking. Once it finds an empty parking, the car parks itself in that parking place.

As you must have noticed, the three statues are erected in the left of the road (the thick black line) and the car parking is on the right of the road. The three statues, correspond to three parking slots. To intercept the statues, the infra-red sensor is mounted on the back-left of the car. While the ultrasonic sensor is mounted on the back-right of the car, to detect whether the parking slot is empty or a car is already parked on the slot. Once an empty car parking slot is detected, the car takes a right turn to park itself. The gyro sensor is mounted on the front-right of the car and helps the car to turn in 90 degrees.  

NOTE: In order to understand the line follower mechanism, it would be good if you read the following article: EV3 Line Follower

Let me explain the program for guided car parking in more detail. The program is implemented in LeJOS programming language. We have a single class called CarkParking. The main method of the program begins with initializing the motors and the sensors. The four sensors, ultrasonic sensor, infrared sensor, color sensor and the gyro sensor are connected to ports S3, S1, S4, S2 respectively. They are initialized in Distance mode, Distance mode, Red mode and Angle mode respectively. The modes of all the sensors are almost self explanatory, except the Red mode of the color sensor. The Red mode returns the intensity of the reflected Red light from the black/white surface. On initializing all the sensors, the line follower program begins and we follow the following algorithm. 

  • Follow the line
    • Check whether the infrared sensor sample is less than 30cm. 
      • If yes, we know that there is a statue present near the car. Then we go ahead and check if ultrasonic sensor sample is greater than 50 cm. 
        • If yes, we know that the car parking is empty. Then we go ahead and check if gyro sensor sample is greater than -90 degrees
          • If yes, we keep taking a right turn until the gyro sensor sample reaches -90 degrees. 
          • If no, continue to follow the line       

Assumption: This program assumes that the automated car is necessarily searching for car parking slot and does not want to continue driving on the road without any need for parking. 




import lejos.hardware.Button;
import lejos.hardware.Sound;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.NXTMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.EV3GyroSensor;
import lejos.hardware.sensor.EV3IRSensor;
import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.hardware.sensor.SensorMode;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;
public class CarParking {
public static void main(String[] args) {
// TODO Auto-generated method stub
//Initialize color sensor
EV3ColorSensor colorSensor = new EV3ColorSensor(SensorPort.S4);
SensorMode mode = colorSensor.getRedMode();
//Initialize gyro sensor
EV3GyroSensor gSensor = new EV3GyroSensor(SensorPort.S2);
gSensor.reset();
//Initialize ultrasonic sensor
EV3UltrasonicSensor ultra = new EV3UltrasonicSensor(SensorPort.S3);
//Initialize IR sensor
EV3IRSensor irSensor = new EV3IRSensor(SensorPort.S1);
SampleProvider provider1 = ultra.getDistanceMode();
SampleProvider provider2 = gSensor.getAngleMode();
SampleProvider provider = irSensor.getDistanceMode();
//Get the White color sample
Sound.twoBeeps();
LCD.drawString("white", 0, 0);
Button.ESCAPE.waitForPressAndRelease();
float[] sampleWhite = new float[1];
mode.fetchSample(sampleWhite, 0);
LCD.drawInt(new Float(sampleWhite[0] * 100).intValue(), 2, 2);
//Get the Black color sample
Delay.msDelay(1000);
Sound.twoBeeps();
LCD.drawString("Black", 4, 4);
Button.ESCAPE.waitForPressAndRelease();
float[] sampleBlack = new float[1];
mode.fetchSample(sampleBlack, 0);
LCD.drawInt(new Float(sampleBlack[0] * 100).intValue(), 6, 6);
Delay.msDelay(3000);
//Set the default power and the multiplying factor for the line follower
int defaultPower = 30;
int multiplyingFactor = 50;
//Initialize motors
NXTMotor largeMotorB = new NXTMotor(MotorPort.B);
NXTMotor largeMotorC = new NXTMotor(MotorPort.C);
float[] color = new float[1];
float[] sample = new float[1];
float[] sample1 = new float[1];
float[] sample2 = new float[1];
while (!Button.ESCAPE.isPressed()) {
//Line Follower algorithm starts
float avgLight = (sampleBlack[0] + sampleWhite[0]) / 2;
mode.fetchSample(color, 0);
float cSpeed = defaultPower + multiplyingFactor
* (avgLight - color[0]) / (sampleWhite[0] - sampleBlack[0]);
largeMotorC.setPower(new Float(cSpeed).intValue());
largeMotorC.forward();
float bSpeed = defaultPower - multiplyingFactor
* (avgLight - color[0]) / (sampleWhite[0] - sampleBlack[0]);
largeMotorB.setPower(new Float(bSpeed).intValue());
largeMotorB.forward();
//Line Follower algorithm ends
//While following the line, fetch the IR sensor sample
provider.fetchSample(sample, 0);
LCD.drawInt(new Float(sample[0]).intValue(), 1, 1);
//Check if IR sensor sample is less than 30cm (which indicates the existance of statues)
if (sample[0] < 30) {
provider1.fetchSample(sample1, 0);
LCD.drawInt(new Float(sample1[0]).intValue(), 5, 5);
//Check if ultrasonic sensor samples are less than 50cm (which indicates an empty slot)
if (sample1[0] > 0.5) {
provider2.fetchSample(sample2, 0);
LCD.drawInt(new Float(sample2[0]).intValue(), 7, 7);
//Start parking the car by turning it, until the gyro sensor samples are greater than -90
while (sample2[0] > -90 && !Button.ESCAPE.isPressed()) {
largeMotorB.setPower(40);
largeMotorC.setPower(40);
largeMotorB.forward();
largeMotorC.backward();
provider2.fetchSample(sample2, 0);
LCD.drawInt(new Float(sample2[0]).intValue(), 7, 7);
}
//Once car turns 90 degrees for parking, drive more for 5 seconds and stop the car.
largeMotorB.setPower(40);
largeMotorC.setPower(40);
largeMotorB.forward();
largeMotorC.forward();
Delay.msDelay(5000);
break;
}
}
}
}
}
view raw CarParking.java hosted with ❤ by GitHub

Thursday, 17 October 2019

EV3 Stair Climber (Climbs Stairs of Size 15 cm)

I have assembled the Stair Climber robot as per the instruction manual given here. In this post, I am explaining the details about the robot which I understood. The front part of the robot is a complete "car". This car is attached with two Lego pins, to the rotating belt. A motor is driving the rotating belt to elevate the back wheels and middle wheels of the car. If the belt rotates anti-clockwise, middle wheels come up. If it rotates clockwise, the back wheels come up. The back wheels of the robot, are for keeping the robot stable when all 4 wheels of the car are up in the air. Front wheels are moving with high speed and they are climbing up due to thrust. There is a "gyro sensor" attached to the robot which made me know how much the robot is oriented in "space". It gives me the angle with which the robot has elevated from flat surface. When this angle goes above a certain threshold, I elevate the middle wheels. That's how the entire car goes up. Once the car reaches in a horizontal position on a stair, I bring up the last pair of wheels. To make things clear, there are two phenomena happening in this assembly: Driving and Lifting of the wheels. The following points are worth noting about the robot movements. 
  • The medium motor and one large motor are responsible for driving the back and front wheels respectively to move the robot forward
  • Another large motor is responsible for lifting the middle and back wheels 
  • The middle wheels are not driven, they are only lifted. They are present for proper balance of the car.
The A small video clip showing the performance of the robot is given below. Also, I am sharing the program for the robot movements. Let me explain the program in detail: The program begins with initializing all the three motors. The motor which drives the back wheels is the medium motor connected to port A. It is initialized with speed of 450 degrees per second. The motor which lifts the middle wheels and back wheels is a large regulated motor connected at port D. It is initialized with speed of 180 degrees per second. And lastly, the motor which drives the front wheels is the large regulated motor connected at port B. It is initialized with speed of 630 degrees per second. The front motor is intentionally set at a high speed to achieve a large thrust from the stair. The large thrust, help the front wheels move up on the stairs. The speed of the rest of the two motors is initialized based on observations about how fast/slow the motors should move. 

The touch sensor is connected at port S2 and initialized in Touch mode. This initialization helps in identifying whether the front button on the touch sensor is pressed. However, I am not making use of the touch sensor in this program. Ideally it could be used to stop the robot when it climbs all the stairs. I am programmatically stopping the robot after 2 stairs. The gyro sensor is connected at port S3. It is reset and then initialized in Angle mode. The angle mode returns the rotation angle in degrees since last reset. 

The main program begins with moving the front and the back motors forward with the function moveTheRobotForwardWithFrontAndBackWheels(). (NOTE: if you dig deep into the function, you will realize that if I rotate the motors backwards, the robot moves forward. It looks like there is some issue with my connections or the robot wheel alignment. However, apart from this issue, there is no cause of concern with the functioning of the robot further.) In the process of moving forward, the robot also starts collecting the Gyro sensor and the Touch sensor samples. If the Gyro sensor sample is less than -15 degrees (please remember, our robot is rotating counter-clockwise while climbing up the stairs, which will result into negative Gyro sensor angle.), two things happen: First, the speed of the lifting mechanism and the speed of the back wheels changes to 630 and -121 degrees per second respectively. (NOTE: the present name of the method is changeTheSpeedOfBackAndMiddleWheels(). Instead the name should be changeTheSpeedOfLiftingAndDrivingOfWheels(). The present name is causing a lot of confusion. However, if we rename the method and use it, the confusion will be gone!) I have kept the speed of the back wheels in negative, which will drive the back wheels backwards. This method results in front wheels moving forward, back wheels moving backwards and middle wheels getting lifted. This movement does not allow the robot to topple. The following little figure will make these movements clear. 



Please also note that the forward speed of the front wheels (630) is much more than the reverse speed  of the back wheels (-121). So in essence, the robot will move forward. However, because of the negative/backward speed, it will be protected from toppling from the stairs. Second, the middle wheels get pulled up until the gyro sample is negative. 

Once the gyro sample turns positive, the pull up of the middle wheels will stop and the flag singleStepDone will be set to true. There are certain steps to be performed once single step is done. They are: 
  • Make the robot move forward on the stair until the front and the middle wheels set properly on the stair
  • Back wheels which were on a negative speed and moving backwards as shown in the above figure, are now set to move forward with a positive speed of 540
  • The back wheels are lifted by moving the large motor clockwise. Please take a look at the two methods for pulling up the back wheels (method: startPullUpTheBackWheels()) and for pulling up the middle wheels (method: pullUpTheMiddleWheels()). You will find that they rotate the motors in opposite direction as explained in the first paragraph of the article. 
  When all the steps are done, the robot will stop by stopping all the three motors. 



import lejos.hardware.Button;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.motor.EV3MediumRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3GyroSensor;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;
public class EV3StairClimber {
private static final int DRIVE_STRAIGHT_ONCE_ON_STAIR = 800;
public static final float MARK = (float) 15.0;
public static final int NUMBER_OF_STEPS = 2;
public static final int PULL_UP = 5500; // for 16 cm
public static int stepsDone = 1;
static EV3MediumRegulatedMotor backWheelsDriver = initializeBackWheelsDriver();
static EV3LargeRegulatedMotor frontWheelsDriver = initializeFrontWheelsDriver();
static EV3LargeRegulatedMotor middleWheelsDriver = initializeMiddleWheelsDriver();
static EV3TouchSensor touchSensor = initializeTouchSensor();
static SampleProvider touchMode = getTouchMode();
static EV3GyroSensor gyroSensor = initializeGyroSensor();
static SampleProvider angleMode = getAngleMode();
public static void main(String[] args) {
// TODO Auto-generated method stub
LCD.drawString("EV3 Stair Climber", 0, 2);
Delay.msDelay(2000);
float[] sample = new float[1];
float[] touchSample = new float[1];
boolean singleStepDone = false;
while (Button.ESCAPE.isUp()) {
LCD.clear();
moveTheRobotForwardWithFrontAndBackWheels();
fetchAndDisplayGyroSample(angleMode, sample);
fetchAndDisplayTouchSample(touchMode, touchSample);
if (gyroSensorSampleTooNegative(sample)) {
changeTheSpeedOfBackAndMiddleWheels();
while (!gyroSensorSamplePositive(sample)
&& !touchSensorTouched(touchSample) && Button.ESCAPE.isUp()) {
pullUpTheMiddleWheels();
fetchAndDisplayGyroSample(angleMode, sample);
}
stopPullUp();
singleStepDone = true;
}
if (singleStepDone) {
moveForwardToFixTheFrontFourWheelsOnStair();
changeTheSpeedOfBackWheels();
pullUpTheBackWheels();
singleStepDone = false;
incrementStepCount();
displayNumberOfStepsClimbed();
if (allStepsAreClimbed()) {
//printMessage("All Steps Climbed");
break;
}
}
}
stopTheRobot();
}
private static boolean touchSensorTouched(float[] touchSample) {
return touchSample[0] == 1.0;
}
private static void fetchAndDisplayTouchSample(SampleProvider touchMode2,
float[] sample) {
// TODO Auto-generated method stub
touchMode.fetchSample(sample, 0);
LCD.drawInt(new Float(sample[0]).intValue(), 5, 5);
}
private static SampleProvider getTouchMode() {
// TODO Auto-generated method stub
return touchSensor.getTouchMode();
}
private static EV3TouchSensor initializeTouchSensor() {
// TODO Auto-generated method stub
EV3TouchSensor touchSensor = new EV3TouchSensor(SensorPort.S2);
return touchSensor;
}
private static void changeTheSpeedOfBackWheels() {
// TODO Auto-generated method stub
backWheelsDriver.setSpeed(540);
}
private static void moveForwardToFixTheFrontFourWheelsOnStair() {
// TODO Auto-generated method stub
backWheelsDriver.backward();
Delay.msDelay(DRIVE_STRAIGHT_ONCE_ON_STAIR);
}
private static void changeTheSpeedOfBackAndMiddleWheels() {
// TODO Auto-generated method stub
middleWheelsDriver.setSpeed(630);
backWheelsDriver.setSpeed(-121);
}
private static void printMessage(String message) {
LCD.clear();
LCD.drawString(message, 5, 5);
Delay.msDelay(3000);
}
private static void pullUpTheBackWheels() {
startPullUpTheBackWheels();
waitToPullUpBackWheels();
stopPullUp();
// middleWheelsDriver.setSpeed(540);
// middleWheelsDriver.rotateTo(2520);
}
private static void stopTheRobot() {
frontWheelsDriver.stop();
middleWheelsDriver.stop();
backWheelsDriver.stop();
}
private static void moveTheRobotForwardWithFrontAndBackWheels() {
moveTheBackWheelsForward();
moveTheFrontWheelsForward();
}
private static void stopPullUp() {
middleWheelsDriver.stop();
}
private static void waitToPullUpBackWheels() {
Delay.msDelay(PULL_UP);
}
private static int incrementStepCount() {
++stepsDone;
return stepsDone;
}
private static void displayNumberOfStepsClimbed() {
LCD.drawString("Step = " + stepsDone, 7, 7);
}
private static void startPullUpTheBackWheels() {
middleWheelsDriver.setSpeed(540);
middleWheelsDriver.forward();
}
private static boolean allStepsAreClimbed() {
return stepsDone > NUMBER_OF_STEPS;
}
private static EV3LargeRegulatedMotor initializeMiddleWheelsDriver() {
middleWheelsDriver = new EV3LargeRegulatedMotor(MotorPort.D);
middleWheelsDriver.setSpeed(180);
return middleWheelsDriver;
}
private static EV3LargeRegulatedMotor initializeFrontWheelsDriver() {
frontWheelsDriver = new EV3LargeRegulatedMotor(MotorPort.B);
frontWheelsDriver.setSpeed(630);
return frontWheelsDriver;
}
private static EV3MediumRegulatedMotor initializeBackWheelsDriver() {
backWheelsDriver = new EV3MediumRegulatedMotor(MotorPort.A);
backWheelsDriver.setSpeed(450);
return backWheelsDriver;
}
private static EV3GyroSensor initializeGyroSensor() {
gyroSensor = new EV3GyroSensor(SensorPort.S3);
gyroSensor.reset();
return gyroSensor;
}
private static SampleProvider getAngleMode() {
SampleProvider angleMode = gyroSensor.getAngleMode();
return angleMode;
}
private static void moveTheFrontWheelsForward() {
frontWheelsDriver.backward();
}
private static void moveTheBackWheelsForward() {
backWheelsDriver.backward();
}
private static void pullUpTheMiddleWheels() {
middleWheelsDriver.backward();
}
private static void fetchAndDisplayGyroSample(SampleProvider angleMode,
float[] sample) {
angleMode.fetchSample(sample, 0);
LCD.drawInt(new Float(sample[0]).intValue(), 5, 5);
}
private static boolean gyroSensorSamplePositive(float[] sample) {
return sample[0] >= 0.0;
}
private static boolean gyroSensorSampleTooNegative(float[] sample) {
return sample[0] < (-MARK);
}
}