• Video
• Description
• Code
• Photos
• References

 

Description

---

 

Code

DrummerBoy.u

load("dynamixel.u");
var Global.d = Dynamixel.new;

class DrummerBoy {

	load("drumset.u");
	
	/* Initialisation */

	function init() {
		var this.hitDistance = 0.5; // Force with which the drums will be hit
		var this.hitSpeed = 0.1; // Delay after drum has been hit
		var this.duration = 20; // Number of beats to play
		
		var this.drumTag = Tag.new;
		this.drumTag.freeze;
		
		// Initialise motors
		var this.head = Dynamixel.Device.new(d,4,Dynamixel.DeviceDesc.AX12);
		var this.rightWrist = Dynamixel.Device.new(d,1,Dynamixel.DeviceDesc.AX12);
		var this.rightShoulder = Dynamixel.Device.new(d,2,Dynamixel.DeviceDesc.AX12);
		var this.leftWrist = Dynamixel.Device.new(d,11,Dynamixel.DeviceDesc.AX12);
		var this.leftShoulder = Dynamixel.Device.new(d,12,Dynamixel.DeviceDesc.AX12);
		var this.torso = Dynamixel.Device.new(d,8,Dynamixel.DeviceDesc.AX12);
		var this.kick = Dynamixel.Device.new(d,10,Dynamixel.DeviceDesc.AX12);
		
		// Initialise center and extreme positions of robot limbs
		this.initPositions();
		
		// Reset positions
		this.setSpeed(1);
		this.reset();
		sleep(1s);
		this.setSpeed(0);
	};
	
	function initPositions() {
		
			// Set min and max positions as well as center positions for all joints
		
			var this.head.initPos = 0;
			var this.head.maxLeft = 1.5;
			var this.head.maxRight = -1.5;

			var this.rightWrist.initPos = 0;
			var this.rightWrist.maxUp = -1.5;
			var this.rightWrist.maxDown = 1.5;

			var this.rightShoulder.initPos = 1.5;
			var this.rightShoulder.maxRight = 0;
			var this.rightShoulder.maxLeft = 2.5;

			var this.leftWrist.initPos = 0;
			var this.leftWrist.maxUp = 1.5;
			var this.leftWrist.maxDown = -1.5;

			var this.leftShoulder.initPos = -1.5;
			var this.leftShoulder.maxRight = -2.5;
			var this.leftShoulder.maxLeft = 0;

			var this.torso.initPos = 0;
			var this.torso.maxRight = -1.5;
			var this.torso.maxLeft = 1.5;

			var this.kick.initPos = 0;
			var this.kick.maxUp = -1.5;
			var this.kick.maxDown = 1;
	};
	
	/* Main behaviour */
	
	function start() {
		reset();
		this.drumTag.freeze;
		
		// Create drumset object for left and right arm
		var this.leftDrumSet = DrumSet.new(this.leftShoulder,this.leftWrist,"left"),
		var this.rightDrumSet = DrumSet.new(this.rightShoulder,this.rightWrist,"right");
		
		echo("Found "+this.rightDrumSet.drums.size()+" drum(s) on the right");
		echo("Found "+this.leftDrumSet.drums.size()+" drum(s) on the left");
		
		// Start drumming if drums have been found
		
		if(this.rightDrumSet.drums.size()>0 || this.leftDrumSet.drums.size()>0) {
		
			this.drumTag.unfreeze;
			this.drumTag :while (1 == 1){
				var countdown = this.duration;
				every(2s) {
					this.drum();
					countdown--;
					if(countdown<=0) {
						this.drumTag.freeze;
						break;
					}
				};
			};
		};
	};
	
	function drum() {
		var i;
		var drumToHit;
		
		if(this.rightDrumSet.drums.size()>0) {
			
			// Pick a random drum and hit it
			i = random(this.rightDrumSet.drums.size());
			echo("Hitting drum "+i+" on the right");
			drumToHit = this.rightDrumSet.drums[i];
			
			// Lift wrist up to initial position
			this.rightWrist.targetPos = this.rightWrist.initPos;
			sleep(0.2s);
			
			// Turn shoulder to position of the drum
			this.rightShoulder.targetPos = drumToHit.getShoulderPos();
			sleep(0.2s);
			
			// Place wrist above drum
			this.rightWrist.targetPos = drumToHit.getWristPos()-this.hitDistance;
			sleep(0.2s);
			
			// Hit it
			this.hitRight();
		};
		if(this.leftDrumSet.drums.size()>0) {
			i = random(this.leftDrumSet.drums.size());
			echo("Hitting drum "+i+" on the left");
			drumToHit = this.leftDrumSet.drums[i];
			this.leftWrist.targetPos = this.leftWrist.initPos;
			sleep(0.2s);
			this.leftShoulder.targetPos = drumToHit.getShoulderPos();
			sleep(0.2s);
			this.leftWrist.targetPos = drumToHit.getWristPos()+this.hitDistance;
			sleep(0.2s);
			this.hitLeft();
		};
	};
	
	function stop() {
		this.drumTag.freeze;
	};
	
	/* Commands */
	
	function hitLeft() {
		// Execute hit motion with left wrist
		var curPos = this.leftWrist.position;
		this.leftWrist.targetPos = curPos-this.hitDistance;
		sleep(this.hitSpeed);
		this.leftWrist.targetPos = curPos;
	};
	
	function hitRight() {
		// Execute hit motion with right wrist
		var curPos = this.rightWrist.position;
		this.rightWrist.targetPos = curPos+this.hitDistance;
		sleep(this.hitSpeed);
		this.rightWrist.targetPos = curPos;
	};
	
	function hitKick() {
		// Execute hit motion with kick
		var curPos = this.kick.position;
		this.kick.targetPos = curPos+this.hitDistance;
		sleep(this.hitSpeed);
		this.kick.targetPos = curPos;
	};
	
	/* Utilities */
	
	function pos2Deg(var position, var left, var right) {
		// Convert motor position to degrees
		return (position-left)/(right-left)*(-90-90)+90;
	};
	
	function deg2Pos(var degree, var maxLeft, var maxRight, var leftDeg, var rightDeg) {
		// Convert degrees to motor position
		return (degree-rightDeg)/(leftDeg-rightDeg)*(maxRight-maxLeft)+maxLeft;
	};
	
	function reset() {
		// Initialise all limbs
		var delay = 0.1;
		this.rightShoulder.targetPos = 1.5;
		sleep(delay);
		this.rightWrist.targetPos = 0;
		sleep(delay);
		this.leftWrist.targetPos = 0;
		sleep(delay);
		this.leftShoulder.targetPos = -1.5;
		sleep(delay);
		this.torso.targetPos = 0;
		sleep(delay);
		this.kick.targetPos = 0;
		sleep(delay);
		this.head.targetPos = 0;
	};
	
	function setSpeed(var speed) {
		this.head.targetSpeed = speed;
		this.leftShoulder.targetSpeed = speed;
		this.leftWrist.targetSpeed = speed;
		this.rightShoulder.targetSpeed = speed;
		this.rightWrist.targetSpeed = speed;
		this.torso.targetSpeed = speed;
		this.kick.targetSpeed = speed;
	};
};

var drummer = DrummerBoy.new;

			

DrumSet.u

class DrumSet {
	load("drum.u");
	
	function init(shoulder,wrist,side) {
		var this.torqueThreshold = 80; // Sensitivity for drum detection

		// Initialise drum scan
		var this.drums = [];
		var drumSet = scan(shoulder,wrist,side);
		
		// Setup drum set
		if(drumSet.size()) {
			for(var i=0;i<drumSet.size();i++) {
				this.drums += [Drum.new(drumSet[i][0],drumSet[i][1])]
			};
		};
	};
	
	function scan(shoulder,wrist,side) {
		var step = 0.25; // Shoulder scan intervals
		var gap = 0.3; // Buffer space

		var init;
		var goal;
		var positions = [];
		var detects = [];
		
		if(side=="left") {
			// Start from left position, move to right
			init = shoulder.maxLeft;
			goal = shoulder.maxRight+gap;
			
			// Inverse step size for left shoulder
			step = -1*step;
			
			// Scan step by step from left to right
			for(var i = init;i>goal;i+=step) {
				shoulder.targetPos = i;
				positions += [i];
				// Detect drum at current position
				detects += [detect(wrist,side)];
				sleep(0.5s);
			};
		} else {
			init = shoulder.maxRight;
			goal = shoulder.maxLeft-gap;
			
			// Scan step by step from right to left
			for(var i = init;i<goal;i+=step) {
				shoulder.targetPos = i;
				positions += [i];
				// Detect drum at current position
				detects += [detect(wrist,side)];
				sleep(0.5s);
			};
		};
		// Return the evaluated scans
		return evaluateScans(positions,detects);
	};
	
	function detect(wrist,side) {
		var step = 0.1; // Steps to lower the wrist
		var ignore = 5; // The first few steps are ignored
		var gap = 1; // Leave some space before reaching maximum position
				
		// Don't change these variables
		var detected = false;
		var ok = true;
		var position = 0;
		var torque = 0;
		
		// Pick limb
		if(side=="left") {
			// If we deal with the left wrist, we have to inverse the direction
			step = -1*step; 
		};
		
		for(var i=0;i<60;i= i+1) {
			// Ignore first steps
			if(ignore>0) {
				ignore--;
			};
			torque = wrist.torque%1024; // prevent wraparound
			
			// If torque exceeds thresholds, drum has been detected
			if(torque>this.torqueThreshold && !ignore) {
				detected = true;
				position = wrist.position;
				break;
			};
			
			// If position is going to exceed maximum position, exit loop
			if(side=="left" && wrist.position+(step*gap)<wrist.maxDown) {
				ok = false;
			} else if (side!="left" && wrist.position+(step*gap)>wrist.maxDown) {
				ok = false;
			};
			if(ok) {
				wrist.targetPos = wrist.position+step;
			} else {
				break;
			};
			sleep(0.3s);
		};

		// reset position
		wrist.targetPos = wrist.initPos;
		
		// return position if drum has been detected
		if(detected) {
			return position;
		} else {
			return false;
		}
	};
	
	
	function evaluateScans(positions,detects) {
		// Array sizes of positions and detects have to match
		if(positions.size()!=detects.size()) {
			echo("Incompatible list sizes");
			return false;
		};
		var out = true;
		var inside = false;
		var drums = [];
		var drumSet = [];
		var p;
		for(var i=0;i<positions.size();i++) {
			// Mark the beginning, inside and outside positions of the drum
			if(detects[i] && out) {
				drums += ["b"];
				out = false;
				inside = true;
			}  else if (detects[i] && inside) {
				drums +=["i"];
			} else if (!detects[i]) {
				drums +=["o"];
				out = true;
				inside = false;
			};
		};
		// Construct the drum position from the measurements 
		for(var i=0;i<drums.size();i++) {
			p = 0;
			var j;
			var k;
			// If the beginning of a drum has been detected
			if(drums[i]=="b") {
				if(i<drums.size()-1) {
					// Look for the end of it
					for(j=i+1;j<drums.size();j++) {
						if(drums[j]=="o") {
							break;
						}
					};
					j--;
					// Sum up the positions that comprise the drum
					for(k=i;k<=j;k++) {
						p+=positions[k];
					};
				} else {
					j = i;
				};
				// And take the average of all the positions 
				// to get the center of the drum
				p = p/(j-i+1);
				// Add the shoulder and wrist position to the drum set
				drumSet += [[p,detects[i]]];
			}
		};
		return drumSet;	
	};
}
			

Drum.u

class Drum {
	function init(x,y) {
		var this.x = x;
		var this.y = y;
	};
	
	function getShoulderPos() {
		return this.x;
	};
	
	function getWristPos() {
		return this.y;
	};
};				
			

 

Photos

 

References

Credits