Thomas' Robotics Portfolio

Studies


3D Environment Scanner

Posted on Nov 29, 2017

I wanted to see if it was practical to create a 3D map of the surrounding environment, like RADAR or LIDAR, using the ultrasonic sensor. It works, but there is too much data to process quickly and a scan at a very low resolution took around two minutes. For an application such as a maze, it would be better just to do a low-resolution scan at the level of the sensor. The scan could have also been improved by averaging several readings at each point rather than only a single reading.

Image Image

85 pieces were used during construction.

This robot was programed using Java on the LeJOS EV3 virtual machine.

Peudocode: Show/Hide

Initialize:
  Set Azimuth Acceleration to 3000°/s2
  Set Azimuth Speed to 360°/s
  Reset Azimuth Count
  Reset Altitude Count
  Set Azimuth Target to 180°
  Set Altitude Target to -60°
  Enable Azimuth Movement
  Enable Altitude Movement
  Create empty data array

Loop:
  If running and not finished:
    If Azimuth enabled:
	  Keep rotating to Azimuth Target
	  If target reached:
	    Disable Azimuth Movement
		Enable Reading
	Else:
	  Stop Azimuth
	  If not reading:
	    Set Azimuth Target to next step
		Enable Azimuth Movement
		If Target is too far:
		  Set finished
    If Altitude enabled:
	  Keep rotating to Altitude Target
	  If target reached:
	    Disable Altitude Movement
	  If calibrating:
	    Set Altitude Movement to Switch Status
    Else:
	  Stop Altitude
	  If calibrating:
	    Reverse Altitude until Switch triggered
		Stop Altitude
		Reverse Altitude by Switch offset
		Reset Altitude Count
		Done calibrating
		Set Altitude Target to -60°
	  Else:
	    If reading:
		  Set current data field to Sensor Distance
		  Set Altitude Target to next step
		  If Altitude Target is too far:
		    Disable Reading
		  Enable Altitude Movement
  Else If running:
    Return Azimuth to 0°
	Dump data to display
	Disable running
  Return running

Cleanup:
  Stop Azimuth
  Stop Altitude
				

Final Code: Show/Hide

package robot;

import lejos.hardware.port.*;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.hardware.sensor.EV3UltrasonicSensor;

import java.util.Date;

import lejos.hardware.motor.*;

public class Robot {
	
	private EV3LargeRegulatedMotor azimuthMotor = new EV3LargeRegulatedMotor(MotorPort.A);
	private EV3MediumRegulatedMotor altitudeMotor = new EV3MediumRegulatedMotor(MotorPort.D);
	private EV3TouchSensor altitudeSwitch = new EV3TouchSensor(SensorPort.S3);
	private EV3UltrasonicSensor distanceSensor = new EV3UltrasonicSensor(SensorPort.S2);
	
	private boolean canRun = true;
	
	private int kAzimuthResolution = 12; // Number of steps to turn 360 degrees
	private double kAzimuthRatio = 7.0/3.0; // deg*kAzimuthRatio = degrees (before ratio)
	private int kAltitudeResolution = 8; // Number of steps to turn 60 degrees
	private double kAltitudeRatio = 24; // deg*kAltitudeRatio = degrees (before ratio)  
	
	private int kAzimuthMin = -420;
	private int kAzimuthMax = 420;
	private int kAltitudeMin = 0;
	private int kAltitudeMax = 1440;
	private int kAltitudeSwitchOffset = 10;
	
	private int targetAzimuth = 0;
	private boolean azimuthMove = false;
	private boolean azimuthCalibration = false;
	
	private int targetAltitude = 0;
	private boolean altitudeMove = false;
	private boolean altitudeCalibration = false;
	private boolean reading = false;
	
	private long movingTime = 0;
	
	private int ptrAzimuth = 0;
	private int ptrAltitude = 0;
	
	private float[][] rawEnvironmentData;
	private boolean isFinished = false;
	
	public Robot() {
		// Initialization
		azimuthMotor.setAcceleration(3000);
		azimuthMotor.setSpeed(360);
		azimuthMotor.resetTachoCount();
		altitudeMotor.resetTachoCount();
		targetAzimuth = kAzimuthMax;
		targetAltitude = kAltitudeMax;
		azimuthMove = true;
		altitudeMove = true;
		movingTime = System.currentTimeMillis();
		rawEnvironmentData = new float[kAzimuthResolution+1][kAltitudeResolution+1];
	}
	
	public boolean Loop() {
		// Continuously runs as long as it returns true
		
		if (canRun && !isFinished) {
			if (azimuthMove) {
				azimuthMotor.rotateTo(targetAzimuth, true);
				if (azimuthMotor.getTachoCount() == targetAzimuth) {
					azimuthMove = false;
					reading = true;
				}
			} else {
				azimuthMotor.flt();
				if (!azimuthCalibration) {
					azimuthCalibration = true;
				} else {
					if (!reading) {
						ptrAzimuth++;
						if (ptrAzimuth <= kAzimuthResolution) {
							targetAzimuth = (int)((double)kAzimuthMax - (kAzimuthRatio*(double)ptrAzimuth*360.0/(double)kAzimuthResolution));
							System.out.println((double)ptrAzimuth*360.0/(double)kAzimuthResolution);
							azimuthMove = true;
						} else {
							ptrAzimuth = 0;
							isFinished = true;
						}
					}
				}
			}
			if (altitudeMove) {
				altitudeMotor.rotateTo(targetAltitude, true);
				if (altitudeMotor.getTachoCount() == targetAltitude) {
					altitudeMove = false;
				}
				if (!altitudeCalibration) {
					altitudeMove = ReadSwitch(altitudeSwitch);
				}
			} else {
				altitudeMotor.flt();
				if (!altitudeCalibration) {
					while (!ReadSwitch(altitudeSwitch)) {
						altitudeMotor.backward();
					}
					altitudeMotor.flt();
					altitudeMotor.rotate(-kAltitudeSwitchOffset);
					altitudeMotor.resetTachoCount();
					altitudeCalibration = true;
					targetAltitude = kAltitudeMax;
					altitudeMove = true;
				} else {
					if (reading) {
						// We can read a sequence
						rawEnvironmentData[ptrAzimuth][ptrAltitude] = ReadDistance(distanceSensor);
						ptrAltitude++;
						targetAltitude = (int)((double)kAltitudeMax - (kAltitudeRatio*(double)ptrAltitude*60.0/(double)kAltitudeResolution));
						if (ptrAltitude > kAltitudeResolution) {
							ptrAltitude = 0;
							targetAltitude = kAltitudeMax;
							reading = false;
						}
						System.out.println(targetAltitude);
						altitudeMove = true;
					}
				}
			}
		} else if (canRun) {
			azimuthMotor.rotateTo(0, true);
			// Print data
			for (int az=0;az<kAzimuthResolution;az++) {
				for (int al=0;al<kAltitudeResolution;al++) {
					System.out.print(rawEnvironmentData[az][al]);
					System.out.print(" ");
				}
				System.out.println();
			}
			canRun = false;
		}
		return canRun;
	}
	
	public void Finish() {
		// Cleanup
		azimuthMotor.flt();
		altitudeMotor.flt();
	}
	
	private boolean ReadSwitch(EV3TouchSensor sensor) {
		float[] sample = new float[] { 0.0f };
		sensor.getTouchMode().fetchSample(sample, 0);
		return sample[0] == 1.0f;
	}
	
	private float ReadDistance(EV3UltrasonicSensor sensor) {
		float[] sample = new float[] { 0.0f };
		sensor.getDistanceMode().fetchSample(sample, 0);
		return sample[0];
	}

}
				

Locomotion: "Segway"

Posted on Oct 13, 2017

This form of locomotion balances upright on a differential pair of wheels aligned along one axis, like a Segway. While its construction is simple, using only 32 pieces, the functionality and responsiveness of the software is crucial. I was unable to get the software working yet and will return to this study after researching the Inverted Pendulum physics problem. This form of locomotion could be practical in a simple environment requiring minimal physical interaction of the robot, such as a maze.


Locomotion: Worm

Posted on Sep 26, 2017

This is the first in a series of locomotion experiments. This simple worm can only move forward, can't turn, and uses only a single motor. While it is very simple, it is not practical since it is unable to move any additional mass.

Image

75 pieces were used during construction.

Image

Pseudocode: Show/Hide

Thread 1:
  Do forever {
    Rotate A CW 900° at 75% power
	Rotate A CCW 900° at 100% power
  }
Thread 2:
  Do forever {
    If Button2 is pressed {
	  End program
	}
  }
				

Projects


Rubik's Cube Solver

Posted on Dec 4, 2017

The Challenge:

Create a robot that can solve a Rubik's cube in 20 moves or less (optimal)

The goal for this robot was to build an efficient Rubik's cube solver with the limited parts in two kits plus an extension kit. This limited the possible number of "grippers" to hold and manipulate the cube to three, because another motor would be needed to move the colour sensor. I was going to avoid using the design of either the CubeStormer III or the MindCub3r. Unfortunately, there was not enough time in the course to get further than the basic concept or to finish the mathematics for optimizing solutions.


Spirograph

Posted on Nov 16, 2017

The Challenge:

Create a robot that can use a pen to draw a spirograph.

The goal for this robot was to draw a realistic spirograph, as if drawn by the toy. After several failed attempts to follow the spirograph's path smoothly, I decided to call the project finished since it was at least drawing a very basic (and jerky) spirograph. In one test, the gyroscope and colour sensor were accidentally swapped, resulting in some very interesting "robot art."

Image Image

85 pieces were used during construction.

This robot was programed using Java on the LeJOS EV3 virtual machine. I split basic operational code and project-specific code into two classes.

Peudocode: Show/Hide

Operational
===========
Start Thread 1
Robot Initialize
Do until stopFlag raised {
  Robot Loop
  If false then exit loop
}
Robot Finish
End program

Thread 1:
  Wait until ESCAPE button pressed
  Raise stopFlag

Robot
=====
Initialize:
  Start Thread 1
  Set spirograph parameters
  Raise canRun
  Start Thread 2
  
Loop:
  If canRun is raised {
    Set left (B) and right (A) to targetSpeeds
  }
  Return canRun

Finish:
  Stop left (B)
  Stop right (A)

Thread 1:
  Wait until colour is not the colour of the paper
  Lower canRun flag

Thread 2:
  Do until spirograph is finished {
    Get angleError
	If angleError > tolerance {
	  Set targetSpeeds to turn bot
	  Repeat loop;
	}
	Get distanceError
	If distanceError > tolerance {
	  Set targetSpeeds to drive forward
	  Repeat loop;
	}
	This waypoint was reached!
	Get next waypoint
  }
  Lower canRun flag
				

Final Code (Operational): Show/Hide

import lejos.hardware.Button;
import robot.Robot;

public class Spirograph {
	
	static volatile boolean stopFlag = false;
	
	public static void main(String[] args) {
		// Entry point
		
		// End program when escape is pressed
		stopThread.start();
		
		// Initialize robot
		Robot theRobot = new Robot();
		
		// Run Robot.loop() until escape is pressed or it returns false
		while (!stopFlag) {
			if (!theRobot.Loop()) {
				break;
			}
		}
		
		// Cleanup
		theRobot.Finish();
		
		// Exit
		System.exit(0);
	}

	private static Thread stopThread = new Thread() {
		public void run() {
			// End program when escape is pressed
			while ((Button.readButtons() & Button.ID_ESCAPE) == 0) {
			}
			stopFlag = true;
		}
	};

}
				

Final Code (Robot): Show/Hide

package robot;

import lejos.hardware.*;
import lejos.hardware.port.*;
import lejos.hardware.motor.*;
import lejos.hardware.sensor.*;

public class Robot {
	
	private EV3LargeRegulatedMotor left = new EV3LargeRegulatedMotor(MotorPort.B);
	private EV3LargeRegulatedMotor right = new EV3LargeRegulatedMotor(MotorPort.A);
	private EV3ColorSensor color = new EV3ColorSensor(SensorPort.S1);;
	private EV3GyroSensor gyro = new EV3GyroSensor(SensorPort.S4);;
	
	private boolean canRun = true;
	
	private double spiroParam_R = 0.0;
	private double spiroParam_r = 0.0;
	private double spiroParam_p = 0.0;
	
	private double spiroAngle = 0.0; // Radians
	private double spiroTargetAngle = 0.0; // Radians
	
	private static double WHEEL_DIAMETER = 0.28; // decimeters to start
	private static double ANGLE_STEP = 0.1;
	private static double POSITION_TOLERANCE = 0.1; // decimeters
	private static double ANGLE_TOLERANCE = 0.1;
	private static double FORWARD_VELOCITY = 0.5; // decimeters
	private static double DEGREES_TO_RADIANS = 2.0*Math.PI/360.0;
	
	private volatile double targetAngle = 0.0;
	private volatile double targetPosX = 0.0;
	private volatile double targetPosY = 0.0;
	
	private volatile double leftWheelSpeed = 0.0;
	private volatile double rightWheelSpeed = 0.0;
	
	private volatile double distanceToDrive = 0.0;
	private volatile double distanceDriven = 0.0;
	
	private volatile double posX = 0.0;
	private volatile double posY = 0.0;
	
	public Robot() {
		// Initialization
		
		// If the marker isn't over white paper, stop so that we don't mark up any floors
		colorThread.start();
		
		// Generate parameters
		// 4 <= R <= 10
		// 2 <= r < R
		// 0 < p <= r
		spiroParam_R = 3.0;
		spiroParam_r = 4.0;
		spiroParam_p = 1.0;
		
		// Calculate number of rotations needed
		spiroTargetAngle = 2.0*Math.PI*spiroParam_r/GCD(spiroParam_R, spiroParam_r);
		
		// Turn down the acceleration
		left.setAcceleration(3000);
		right.setAcceleration(3000);
		
		// Start counting angles
		//timeRef = System.currentTimeMillis();
		angleThread.start();
	}
	
	public boolean Loop() {
		// Continuously runs as long as it returns true
		
		if (canRun) {
			left.setSpeed((float)Speed(Math.abs(leftWheelSpeed)));
			right.setSpeed((float)Speed(Math.abs(rightWheelSpeed)));
			if (leftWheelSpeed >= 0) {
				left.forward();
			} else {
				left.backward();
			}
			if (rightWheelSpeed >= 0) {
				right.forward();
			} else {
				right.backward();
			}
		} else {
			
		}
		return canRun;
	}
	
	public void Finish() {
		// Cleanup
		
		left.stop();
		right.stop();
	}
	
	private double GetX() {
		double a = (spiroParam_R - spiroParam_r)*Math.cos(spiroAngle);
		double b = spiroParam_p*Math.cos(spiroAngle*(spiroParam_R - spiroParam_r)/spiroParam_r);
		System.out.println(spiroAngle + " -> " + (a+b));
		return a+b;
	}
	
	private double GetY() {
		double a = (spiroParam_R - spiroParam_r)*Math.sin(spiroAngle);
		double b = spiroParam_p*Math.sin(spiroAngle*(spiroParam_R - spiroParam_r)/spiroParam_r);
		return a-b;
	}
	
	private double Speed(double dmps) {
		// Speed in degrees per second
		return (dmps*360.0/(Math.PI*WHEEL_DIAMETER));
	}
	
	private int GCD(int a, int b) {
		return b == 0 ? a : GCD(b, a%b);
	}
	
	private double GCD(double a, double b) {
		a *= 1000.0;
		b *= 1000.0;
		return GCD((int)Math.round(a), (int)Math.round(b)) / 1000.0;
	}
	
	private Thread colorThread = new Thread() {
		public void run() {
			while (canRun) {
				// If the marker isn't over white paper, stop so that we don't mark up any floors
				if (color.getColorID() != 5) { // RGBYOWK = 0;1;2;3;5;6;7 respectively
					//canRun = false;
				}
			}
		}
	};
	
	private double GetCurrentAngle() {
		// Read gyro sensor in radians
		float[] sample = { 0.0f };
		gyro.getAngleMode().fetchSample(sample, 0);
		return ((double)sample[0] % 360) * DEGREES_TO_RADIANS;
	}
	
	private Thread angleThread = new Thread() {
		public void run() {
			while (canRun && spiroAngle < spiroTargetAngle) {

				// Get the right angle
				double angleError = (targetAngle - GetCurrentAngle()) / Math.PI;
				
				if (Math.abs(angleError) > 1) {
					angleError = -2.0*Math.signum(angleError) + angleError;
				}

				//System.out.println(angleError);
				
				if (Math.abs(angleError) > ANGLE_TOLERANCE) {
					// Turn at some speed in that direction
					leftWheelSpeed = -angleError * 5.0;
					rightWheelSpeed = angleError * 5.0;
					left.resetTachoCount();
					// Can't move forward until pointing in the right direction!
					continue;
				}
				
				if (distanceToDrive - distanceDriven > POSITION_TOLERANCE) {
					// Keep going
					leftWheelSpeed = FORWARD_VELOCITY;
					rightWheelSpeed = FORWARD_VELOCITY;
					distanceDriven += left.getTachoCount()*Math.PI*WHEEL_DIAMETER/360.0;
					left.resetTachoCount();
				} else {
					// Reached target!
					leftWheelSpeed = 0;
					rightWheelSpeed = 0;
					// Increment step and calculate new angle and distance
					spiroAngle += ANGLE_STEP;
					posX = targetPosX;
					posY = targetPosY;
					targetPosX = GetX();
					targetPosY = GetY();
					targetAngle = Math.atan2(targetPosY - posY, targetPosX - posX);
					left.resetTachoCount();
					distanceDriven = 0.0;
					distanceToDrive = Math.sqrt(Math.pow(targetPosY - posY, 2) + (Math.pow(targetPosX - posX, 2)));
					//System.out.println(distanceToDrive);
				}
			}
			canRun = false;
		}
	};

}
				

Sensabot

Posted on Oct 18, 2017

The Challenge:

Create a robot which drives straight forward, stopping and raising and lowering its arm directly over three lines, then returns to its strting position.

Since the bot only needs to drive straight, there is only one drive motor connected directly to each wheel. An additional motor with rack & pinion gears controls the arm. A colour sensor detects when the bot reaches a line, and the motor's built-in encoder is used to return to the starting position. The "jerky" motion when accelerating is due to the center of mass positioned over the front axle rather than in the center of the vehicle.

Image

78 pieces were used during construction.

This robot was programmed using Java on the LeJOS EV3 virtual machine

Pseudocode: Show/Hide

Thread 1:
  Do until a button is pressed {
    Check [1] color and compare to previous line state
	Check rising edge
  }
  End program

Thread 2:
  Initialize drive motor
  Repeat (3) {
    MoveToNextPoint
	DoInspect
  }
  ReturnToHome
  End program

MoveToNextPoint:
  Rotate B forward/CCW
  Wait for rising edge
  Stop B
  Cancel rising edge

DoInspect:
  Rotate A CW 340°
  Rotate A CCW 340°
  Set line state
  Cancel rising edge

ReturnToHome:
  Rotate B backward/CW
  Wait for [B] encoder <= 0
  Stop B
				

Final Code: Show/Hide

import lejos.hardware.*;
import lejos.hardware.port.*;
import lejos.hardware.motor.*;
import lejos.hardware.sensor.*;

public class Sensabot {
	
	private static volatile boolean line = false;
	private static volatile boolean rise = false;
	private static EV3ColorSensor sensor = new EV3ColorSensor(SensorPort.S1);;
	private static EV3LargeRegulatedMotor driveMotor = new EV3LargeRegulatedMotor(MotorPort.B);
	private static EV3MediumRegulatedMotor armMotor = new EV3MediumRegulatedMotor(MotorPort.A);
	
	private static Thread sensorThread = new Thread() {
		public void run() {
			while (Button.readButtons() == 0) {
				if (sensor.getColorID() == 1) { // Green
					rise = !line || rise;
					line = true;
				} else {
					line = false;
				}
			}
			System.exit(0);
		}
	};

	public static void main(String[] args) {
		
		sensorThread.start();
		
		driveMotor.resetTachoCount();
		driveMotor.setAcceleration(3000);
		
		for (int i=0;i<3;i++) {
			MoveToNextPoint();
			DoInspect();
		}
		ReturnToHome();
		
		System.exit(0);
		
	}
	
	private static void DoInspect() {
		armMotor.rotate(340);
		armMotor.rotate(-340);
		line = true;
		rise = false;
	}
	
	private static void MoveToNextPoint() {
		driveMotor.forward();
		while (!rise) { }
		driveMotor.flt();
		rise = false;
	}
	
	private static void ReturnToHome() {
		driveMotor.backward();
		while (driveMotor.getTachoCount() > 0) { }
		driveMotor.flt();
	}
}
				
Last edited on November 7, 2017