<repo_name>Roboticsotago/robotics_repo<file_sep>/Projects/2017/SoccerBots/runeverything.sh
#!/bin/bash

if [[ $(hostname) == "Boris" ]]; then
	sleep 7
	pd_patch=goalie_control_refactored.pd
	echo Goalie
	xdotool mousemove --sync 0 0
	sleep 3
	python -u blob_tracking_cv2.py | python boris_python2.py
	xmessage yo 
else
	echo Attacker
	sleep 7
	pd_patch=attacker_main.pd
	urxvt -name Pd -title Pd -e sh -c "/usr/bin/pd -nomidi -alsa -noadc -r 8000 -send \"; pd dsp 1\" motor-control.pd $pd_patch sensor_input.pd" &
	sleep 10
	urxvt -hold -name read_sensors -title read_sensors -e sh -c "./read_sensors.tcl | pdsend 7000" &
	sleep 5
	xdotool mousemove --sync 0 0
	sleep 1
	urxvt -hold -name blob_tracking -title blob_tracking -e sh -c "python2.7 blob_tracking_cv2.py | pdsend 7001" &
	xmessage yo
fi
<file_sep>/Projects/2016/MIDIBots/arduino/pwm_examples/pwm_7.ino
// MIDI note tests for Mechatronics, CME 2016-03-21
// 1. Loop sanity check.  Watch for off-by-one errors!
// 2. Translate note frequency formula into C syntax and check (MIDI note 69 should give 440 Hz). 
// 3. Correct integer division in formula!
// 4. Derive OCR "top" value from frequency, and print for checking.
// 5. Correct type of "top" to get the full available range.
// 6. Move the formulae into separate functions, and check that it still works!
// 7. Add stuff for generating the PWM output.  Tweak range of notes for musical range.


// General settings:

const int prescale = 8;
const int serial_speed = 9600;

// On the Mega, we have timer1 attached to pins D11 and D12, D12 being the primary one.
// On "ordinary" Arduinos, it's on pins 9 and 10.
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
const int OUTPUT_PIN     = 12;
const int OUTPUT_PIN_AUX = 11;
#else
const int OUTPUT_PIN     = 10;
const int OUTPUT_PIN_AUX = 9;
#endif

#define TIMER_CLK_STOP		0x00	///< Timer Stopped
#define TIMER_CLK_DIV1		0x01	///< Timer clocked at F_CPU
#define TIMER_CLK_DIV8		0x02	///< Timer clocked at F_CPU/8
#define TIMER_CLK_DIV64		0x03	///< Timer clocked at F_CPU/64
#define TIMER_CLK_DIV256	0x04	///< Timer clocked at F_CPU/256
#define TIMER_CLK_DIV1024	0x05	///< Timer clocked at F_CPU/1024

// This defines which bits within the TCCRnB register determine the prescale factor:
#define TIMER_PRESCALE_MASK	0xF8        // 0B11111000

// Map desired prescale divider to register bits:
byte timer_prescale_bits(int prescale) {
	if (prescale == 1)
		return 0x01;
	else if (prescale == 8)
		return 0x02;
	else if (prescale == 64)
		return 0x03;
	else if (prescale == 256)
		return 0x04;
	else if (prescale == 1024)
		return 0x05;
	else
		return 0x00;	// Error?!
}


float frequency(int note) {
	return 440 * pow(2, (note - 69) / 12.0);
}

unsigned int top(float frequency) {
	return round(F_CPU / (prescale * frequency) - 1);
}


void pwm(float frequency, float duty_cycle) {
	TCNT1 = 0;        // Reset timer counter
	
	unsigned int wrap_limit = top(frequency);
	OCR1A = wrap_limit;
	OCR1B = int(wrap_limit * duty_cycle);

	// TCCR1A's timer mode should already be set, so just use TCCR1B to start PWM (is that sufficient?):
	TCCR1A = _BV(COM1A0) | _BV(COM1B1) | _BV(WGM11) | _BV(WGM10);
	TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11);	
	TCCR1B = (TCCR1B & TIMER_PRESCALE_MASK) | timer_prescale_bits(prescale);
}
	
void setup() {
	pinMode(OUTPUT_PIN, OUTPUT);
	pinMode(OUTPUT_PIN_AUX, OUTPUT);
	digitalWrite(OUTPUT_PIN, LOW);
	digitalWrite(OUTPUT_PIN_AUX, LOW);
	delay(8000);  // safety delay for serial I/O
	Serial.begin(serial_speed);
}

void loop() {
	int prescale = 8;
	for (int note = 33; note <= 96; note++) {
		float f = frequency(note);
		unsigned int t = top(f);
		Serial.print(note);
		Serial.print(": f=");
		Serial.print(f);
		Serial.print(", top=");
		Serial.println(t);
		pwm(frequency(note), 0.3);
		delay(150);
	}
}
<file_sep>/Projects/2016/MIDIBots/arduino/fan_test/ano/src/sketch.ino
../../fan_test.ino<file_sep>/Projects/2016/MIDIBots/Airflow requirements.txt
Notes on airflow for the woodwind instruments for the MusicBots project 2016

--

We did some testing of Chris's Aerobed fan.  This is rated at 12 V ~6 A (10 A fuse), and is designed to be run off a car cigar lighter 
or eight 1.5 V batteries.

V-A testing off the bench supply:

V:	A:	Comment:
0	0
0.3	0.5
0.6	0.4	turn-off point
1.0	0.44	turn-on point
1.5	0.5
2.0	0.59
2.5	0.71
3.0	0.83
3.5	0.96
4.0	1.12
4.5	1.30
5.0	1.48
6.0	1.9
7.0	2.4
7.5	2.73
8.0	2.99	Power supply current limit

The fan/motor is quite noisy, even at low voltages.  I think the noise is mostly from the motor, not the fan impeller blades, so we 
might be able to replace it with a quieter brushless fan.  Alternatively, doing the 3D design and fabrication of an impeller will be 
good practice for the hovercraft project later in the year..!

For playing the recorder in tune, more pressure and airflow is required for higher notes.  Here is the voltage required to achieve the 
correct pitch (frequency) for some test notes:

D	1.9 V
E	2.1 V
G	2.5 V
A	2.7 V
B	3.1 V
C	3.2 V
D	3.4 V

--

Required air pressure for acoustic woodwind instruments:

oboe: 20..60 mm Hg = 2.8..7.7 kPa
Rossing: reed woodwind: 3-5 kPa
flute: ~1 kPa, 20..60 m/s air jet velocity

Fuks and Sundberg (1996):
	Clarinet: 25-60 cm H2O -> 2.5-6.0 kPa
	Saxophone: 15-80 cm H2O -> 1.5-8.0 kPa
	Oboe: 35-100 cm H2O -> 3.5-10.0 kPa
	Bassoon: 10-90 cm H2O -> 1.0-9.0 kPa

Fletcher:
	Flute: 0.2-2.5 kPa

--

A centrifugal blower is probably a better type of fan for high pressure (axial fans can produce a lot of airflow, but are limited in 
pressure).

Centrifugal fan/impeller blades can be straight, or curved backwards or forwards.  Curved-forward centrifugal blades probably have the 
best characteristics:
 - high pressure
 - low noise
 - small airflow

They do require more mechanical power to run them, however.

--

Some candidiate fans (or we could make our own):

Sunon 12V:
	1.5 A @ 12 V
	5400 RPM
	44.2 CPM = 75 m^3/hour
	3.39 in. H2O = 0.84 kPa
	Probably not enough pressure

Sunon 3.5 A 12 V blower, PF97331BX-B00U-A99
	97 mm x 95 mm x 33 mm
	6800 RPM
	93 m^3/hour
	1.3 kPa
	Sufficient pressure for a recorder or flute, but probably not a reed instrument.
	Digi-Key has, but only in min. 30x orders
	
Delta-Electronics BCB1012UH-F00
	1.0 kPa
	9300 RPM
	1.00 m^3/min
	3 wire leads

Delta-Electronics BCB0812UHN-TP09
	0.915 kPa
	2.3 A
	PWM control and tach! (4-wire)
	http://www.digikey.com/product-detail/en/delta-electronics/BCB0812UHN-TP09/603-1213-ND/2034820
	Could use with plain Arduino (no shield required) - with pressure sensor for control feedback
		Have ordered one of these from Digi-Key

--

Fan speed control/pressure regulation

We will probably need some kind of air reservoir to help maintain steady pressure and airflow while mutliple woodwind instruments are 
turning on and off.  The regulation of air pressure could be done mechanically, using a weight or a spring mechanism, but we also have 
the opportunity to use an active control system by monitoring the pressure in the reservoir electronically, and using PWM to vary the 
fan motor speed.  It might be necessary to use a PID (proportional-integral-derivative) control system to maintain the pressure 
accurately enough.

Note how airflow and pressure are analogous to electrical current and voltage!  The wind reservoir is like a capacitor.  A narrow tube 
is like a resistor (it will reduce the current and the pressure coming out).  Acoustic impedance (analogous to electrical impedance) is 
an important characteristic of air-column instruments such as brass and woodwind.

We will also want to be able to regulate the pressure/flow at the instrument in order to get acceptable intonation. We have tested an 
arrangement using a servo motor squeezing on a thin air tube made from heatshrink tubing.  The Hitec servos seem to be strong enough for 
this application.  If this can cut off the air supply completely, then we would not require a separate solenoid valve.  An alternative 
to the squeezed tube would be a butterfly valve: a disc-shaped valve rotating inside a cylindrical pipe.  This would be a good match for 
a servo motor, and would probably be easier to actuate as the airflow is roughly balanced on both sides of the valve (having to work 
against the airflow would require more force).
<file_sep>/Projects/2016/MIDIBots/arduino/libraries/MIDIBot/MIDIBot.h
/*

Arduino library for our MIDIBot shield circuit board, Robocup 2016
Team _underscore_, Information Science Mechatronics, University of Otago

To use this library, #include it, and instantiate a la "MIDIBot drumBot;" (note: no trailing parentheses - that would make it a function declaration!).

You will also need to define a note_on() and note_off() implementation for the specific robot. These will be called from MIDIBot::process_MIDI, passing the MIDI note number and velocity as arguments.

Your sketch will also need to implement a self_test() function, which will be called from MIDIBot::process_MIDI if the Self-Test button is pressed.

Because you can't use delay() or Serial.begin() in the constructor, this class defines a separate MIDIBot::begin() method which reads the MIDI channel from the DIP switches, flashes the MIDI channel number, and starts serial communication at MIDI baud rate (after a 5-second pause to avoid reprogramming problems).

The following is a basic example:
--
#include <MIDIBot.h>
MIDIBot thisMIDIBot;

void note_on(int note, int velocity) {
	digitalWrite(LED_PIN, HIGH);
	analogWrite(MOSFET_PWM_PIN, 64);
}

void note_off(int note, int velocity) {
	digitalWrite(LED_PIN, LOW);
	analogWrite(MOSFET_PWM_PIN, 0);
}

void self_test() {
	thisMIDIBot.test_MIDI_channel();
	// Bot-specific tests...
}

void setup() {
	thisMIDIBot.test_MIDI_channel();	// Indicate MIDI channel at startup
}

void loop() {
	thisMIDIBot.process_MIDI();
}
--
*/


// NOTE: Due to apparent problems with calling delay() inside a constructor, you can't flash the MIDI channel number on the LED automatically at startup (though you can read the MIDI channel DIP switches).  Therefore, call yourMIDIBot.test_MIDI_channel() it your setup() function if you want this to happen.

// TODO:
// [ ] Convert to camelCase throughout for function names?  It's recommended in the Arduino API style guide.  Also member variables?
// [ ] Rename _i to something more meaningful, e.g. _dataByteIndex or _MIDIBufferIndex
// [ ] #define constants?  That seems to be the norm with .h files, and they'd only be defined if the header were included, and might save on processor registers?

#ifndef MIDIBot_included
#define MIDIBot_included

#include "Arduino.h"


// Define pin mappings for the MIDIBot shield:
// For a library, should these be global symbols or defined within the class?
// Also, how do we make the class a singleton?
// Should these be consts or #defines?
const int
	MIDI_1x_PIN = 2,
	MIDI_2x_PIN = 3,
	MIDI_4x_PIN = 4,	/* USBDroid has pulldown resistor on D4! */
	MIDI_8x_PIN = 5,
	MOSFET_PWM_PIN = 6,
	SERVO_1_PIN = 9,	/* USBDroid has pulldown resistor on D9! */
	SERVO_2_PIN = 10,
	SERVO_3_PIN = 11,	/* USBDroid has pulldown resistor on D11! */
	MOSFET_2_PIN = 12,
	MOSFET_3_PIN = A1,	/* not A3 as on the silkscreen! */
	MOSFET_4_PIN = A2,	/* not A4 as on the silkscreen! */
	LED_PIN = 13,		/* USBDroid has pulldown resistor on D13! */
	LATENCY_ADJUST_PIN = A0,
	SELF_TEST_PIN = A5,
	SERIAL_SPEED = 31250
	
;

// Empty prototypes for additional functions that the sketch using this library should define (for bot-specific functionality):
// (Do these need to be declared "extern" as well?)
void note_on(int note, int velocity);
void note_off(int note, int velocity);
void self_test(); // Called from process_MIDI() when the self-test button is pressed


class MIDIBot {
	public:
		MIDIBot();	// Constructor method.  Sets up the MIDIBot Shield's pins, sets the MIDI channel ID from the DIP switches, and starts Serial input for MIDI compatibility.
		void begin();	// Start serial comms
		void clearData();	// Zeroes out the buffer for received MIDI data
		void flash(int on_time, int off_time);	// Flash the onboard LED with specified on/off duration in milliseconds
		void flash_number(int n);	// Flash the LED in a pattern to indicate the value of <n>
		void read_MIDI_channel();	// Read MIDI DIP switch settings and store in _MIDI_channel
		void process_MIDI();	// Main MIDI-processing "loop"
		// Generic self-test functions:
		void test_blink();	// Blink the onboard LED once
		void test_button();	// Light the onboard LED if the self-test button is pressed
		void test_MIDI_channel();	// Read and flash-display the MIDI channel number
	private:
		int _MIDI_channel;	// 1..16, 10=drums
		
		// MIDI messages have up to 2 data bytes, which we store in an array:
		// TODO: perhaps name the index more sensibly/explicitly:
		int _dataByte[2], _statusByte, _i;
		
		// For monophonic instruments, it's useful to keep track of the note currently being played, so that any note-off messages for other notes can be ignored.
		int _current_note_number;
};

#endif
<file_sep>/Projects/2017/SoccerBots/arduino/buzzer/pokemon.h
//short part of Pokemon Red/Blue/Yellow opening theme

const int pokemonTempo = 200;
const int pokemonArrayLength = 37;
float pokemon[pokemonArrayLength][2] = {{69,0.5},{69,0.5},{74,1},{69,0.5},{69,0.5},{75,1},
	{69,0.5},{69,0.5},{74,1},{69,0.5},{69,0.5},{73,1},
	{69,0.5},{69,0.5},{74,1},{69,0.5},{69,0.5},{79,1},
	{81,2},{69,2},
	{77,2},{65,2},
	{69,0.5},{69,0.5},{74,1},{69,0.5},{69,0.5},{75,1},
	{69,0.5},{69,0.5},{74,1},{69,0.5},{69,0.5},{79,1},
	{78,4},
	{74,1},{0,3}};
<file_sep>/Projects/2017/SoccerBots/calibrate_vision_cv2.py
#!/usr/bin/env python2.7
#Calibration of the camera to do when lighting conditions change
import sys
def debug(msg):
	sys.stderr.write(str(msg) + "\n")

from time import sleep
execfile("camera_setup.py")
debug("calibrating gain...")
sleep(2)
camera.exposure_mode = 'off'
debug("camera.analog_gain = " + str(camera.analog_gain))

# Now fix the white balace:
debug("Hold a white card in front of the camera now!")
sleep(5)
g = camera.awb_gains
debug("Thanks; camera.awb_gains = " + str(g))
camera.awb_mode = 'off'
camera.awb_gains = g
calibration_frames = 240

def calibrate_target_colour():
	debug("Calibrating target colour")
	debug("Put the target colour in the capture frame")
	camera.start_preview(alpha=200)
	sleep(3)
	i=1
	h_avg=0
	s_avg=0
	v_avg=0
	h_min=180
	h_max=0
	s_min=255
	s_max=0
	v_min=255
	v_max=0
	
	for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
		# grab the raw NumPy array representing the image, then initialize the timestamp
		# and occupied/unoccupied text
		#print("copy array")
		image = frame.array
		
		# Crop the central region of interest and convert to HSV colour space:
		xres = capture_res[0]
		yres = capture_res[1]
		# TODO: make a function for these?
		Y1 = int(round(yres * (yfrac + 1) / 2.0))
		Y0 = int(round(yres * (1 - yfrac) / 2.0))
		X0 = int(round(xres * (1 - xfrac) / 2.0))
		X1 = int(round(xres * (xfrac + 1) / 2.0))
		hsv = cv2.cvtColor(image[Y0:Y1,X0:X1], cv2.COLOR_BGR2HSV)
		
		# Draw sampling region on the frame to assist with aiming:
		cv2.rectangle(image,(X0,Y0),(X1,Y1),(0,255,0),2)
		cv2.imshow("Frame",image)
		key = cv2.waitKey(16) & 0xFF

		# Compute the average colour for the sampling region:
		average_colour = [hsv[:, :, c].mean() for c in range(hsv.shape[-1])]
		#average_colour = hsv[capture_res[0]/2,capture_res[1]/2]
		debug(str(average_colour) + str(i))
		rawCapture.truncate(0)
		if  i==1:
			h_avg = average_colour[0]
			s_avg = average_colour[1]
			v_avg = average_colour[2]
		else:
			h_avg = (i-1)/float(i)*h_avg + average_colour[0]/float(i)
			s_avg = (i-1)/float(i)*s_avg + average_colour[1]/float(i)
			v_avg = (i-1)/float(i)*v_avg + average_colour[2]/float(i)
			
		current_h_min = hsv[:,:,0].min()
		current_h_max = hsv[:,:,0].max()
		current_s_min = hsv[:,:,1].min()
		current_s_max = hsv[:,:,1].max()
		current_v_min = hsv[:,:,2].min()
		current_v_max = hsv[:,:,2].max()
		if current_h_min < h_min: h_min = current_h_min
		if current_h_max > h_max: h_max = current_h_max
		if current_s_min < s_min: s_min = current_s_min
		if current_s_max > s_max: s_max = current_s_max
		if current_v_min < v_min: v_min = current_v_min
		if current_v_max > v_max: v_max = current_v_max
		
		i = i + 1
		
		if i >= calibration_frames:
			average_colour[0] = h_avg
			average_colour[1] = s_avg
			average_colour[2] = v_avg
			debug(average_colour)
			return [h_min,s_min,v_min], [h_max,s_max,v_max]
			break;
		
		
calibrated_colour = calibrate_target_colour()	
			
calibration_file_name = "camera_calibration_state_cv2.py"

def file_output(file_name, white_balance, target_colour_min_max):
	filex = open(file_name, "w")
	filex.write("calibrated_white_balance = " + str(white_balance) + "\n")
	filex.write("calibrated_goal_colour = " + str(target_colour_min_max) + "\n")
	filex.close()
	
file_output(calibration_file_name, g, calibrated_colour)
<file_sep>/Projects/2016/MIDIBots/arduino/midi_test/ano/src/sketch.ino
../../midi_test.ino<file_sep>/Projects/2016/MIDIBots/arduino/pwm_synth_test/ano/src/sketch.ino
../../pwm_synth_test.ino<file_sep>/Documentation/Arduino/memory-and-EEPROM/build.sh
#!/bin/sh

BASENAME=memory-and-EEPROM

pandoc -s --latex-engine=xelatex --variable=geometry:a4paper --include-in-header=fontoptions.tex --include-in-header=fullrule.tex $BASENAME.md -o $BASENAME.pdf
<file_sep>/Projects/2016/MIDIBots/arduino/uke_test/ano/src/sketch.ino
../../uke_test.ino<file_sep>/Projects/2016/MIDIBots/arduino/midi_drum/midi_drum.ino
// An Arduino program for triggering a servo-controlled drum-stick in response to MIDI note messages


// General settings:
const float DOWN = 0;
const float UP = 0.5;
 
const int prescale = 8;
const int SERVO_FREQUENCY = 50; // Hz (1/(20 ms))
const int channel = 0;


// MIDI messages have up to 2 data bytes, which we store in an array:
int dataByte[2], statusByte = 0, i = 0;

void clearData(){
  dataByte[0] = 0;
  dataByte[1] = 0;
  i = 0;
}

// Even though this is only monophonic, it's still useful to keep track of the note currently being played, so that any note-off messages for other notes can be ignored.
int current_note_number = 0;

// On the Mega, we have timer1 attached to pins D11 and D12, D12 being the primary one.
// On "ordinary" Arduinos, it's on pins 9 and 10.
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
const int OUTPUT_PIN     = 12;
const int OUTPUT_PIN_AUX = 11;
#else
const int OUTPUT_PIN     = 10;
const int OUTPUT_PIN_AUX = 9;
#endif

// Some PWM control definitions:
#define TIMER_CLK_STOP		0x00	///< Timer Stopped
#define TIMER_CLK_DIV1		0x01	///< Timer clocked at F_CPU
#define TIMER_CLK_DIV8		0x02	///< Timer clocked at F_CPU/8
#define TIMER_CLK_DIV64		0x03	///< Timer clocked at F_CPU/64
#define TIMER_CLK_DIV256	0x04	///< Timer clocked at F_CPU/256
#define TIMER_CLK_DIV1024	0x05	///< Timer clocked at F_CPU/1024

// This defines which bits within the TCCRnB register determine the prescale factor:
#define TIMER_PRESCALE_MASK	0xF8        // 0B11111000

// Map desired prescale divider to register bits:
byte timer_prescale_bits(int prescale) {
	if (prescale == 1)
		return 0x01;
	else if (prescale == 8)
		return 0x02;
	else if (prescale == 64)
		return 0x03;
	else if (prescale == 256)
		return 0x04;
	else if (prescale == 1024)
		return 0x05;
	else
		return 0x00;	// Error?!
}


float frequency(int note) {
	return 440 * pow(2, (note - 69) / 12.0);
}

unsigned int top(float frequency) {
	return round(F_CPU / (prescale * frequency) - 1);
}


void pwm(float frequency, float duty_cycle) {
	TCNT1 = 0;        // Reset timer counter
	
	unsigned int wrap_limit = top(frequency);
	OCR1A = wrap_limit;
	OCR1B = int(wrap_limit * duty_cycle);

	// TCCR1A's timer mode should already be set, so just use TCCR1B to start PWM (is that sufficient?):
	TCCR1A = _BV(COM1A0) | _BV(COM1B1) | _BV(WGM11) | _BV(WGM10);
	TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11);	
	TCCR1B = (TCCR1B & TIMER_PRESCALE_MASK) | timer_prescale_bits(prescale);
}

void pwm_off() {
    return;
	TCCR1B = (TCCR1B & TIMER_PRESCALE_MASK) | TIMER_CLK_STOP;
	TCNT1 = 0;
	digitalWrite(OUTPUT_PIN, LOW); // This seems to be necessary to silence it properly (sometimes gets stuck at 5 V otherwise!)
}
	
void setup() {
	pinMode(OUTPUT_PIN, OUTPUT);
	pinMode(OUTPUT_PIN_AUX, OUTPUT);
	digitalWrite(OUTPUT_PIN, LOW);
	digitalWrite(OUTPUT_PIN_AUX, LOW);
	clearData();
        pwm(50, calcDutyCicle(UP));
	delay(2000);  // safety delay for serial I/O
	Serial.begin(31250);
        
}

float calcDutyCicle(float in){
      if(in < 0)
              in = 0;
      if(in > 1)
              in = 1;
      return (in + 1)/20.0;
}

void drum(){
        int spd = 100; // ms; 80 ms is OK up to about 220 BPM
        pwm(SERVO_FREQUENCY, calcDutyCicle(DOWN));
        delay(spd);
        pwm(SERVO_FREQUENCY, calcDutyCicle(UP));
        delay(spd);
}

void loop  () {
	if (Serial.available() > 0) {
		int data = Serial.read();
		if (data > 127) {
			// It's a status byte. Store it for future reference.
			statusByte = data;
			clearData();  
		} else {
			// It's a data byte.
			dataByte[i] = data;
			if (statusByte == (0x90 | channel) && i == 1) {
				// Note-on message received
				if (dataByte[1] == 0 && dataByte[0] == current_note_number) {
					// Stop note playing
					pwm_off();
				} else {
					// Start note playing
					current_note_number = dataByte[0];
//					pwm(frequency(current_note_number), dataByte[1] / 127.0 / 2); // TODO: map velocity to PWM duty
				        drum();
                                }
			} else if (statusByte == (0x80 | channel) && i == 1 && dataByte[0] == current_note_number) {
				// Note-off message received
				// Stop note playing
				pwm_off();
			}
			i++;
			// TODO: error detection if i goes beyond the array size.
		}
	}
}

<file_sep>/Projects/2017/SoccerBots/arduino/magnetometer_test/magnetometer_test.ino
/*
  Magnetometer Test
  Reads data from the LIS3MDL sensor and calculate a compass heading and can
  return the angle to a specified heading
  The y-axis points in the forwards direction of the robot

TODO:
- implement correction for magmetic north
*/

/*
The sensor outputs provided by the library are the raw 16-bit values
obtained by concatenating the 8-bit high and low magnetometer data registers.
They can be converted to units of gauss using the
conversion factors specified in the datasheet for your particular
device and full scale setting (gain).

Example: An LIS3MDL gives a magnetometer X axis reading of 1292 with its
default full scale setting of +/- 4 gauss. The GN specification
in the LIS3MDL datasheet (page 8) states a conversion factor of 6842
LSB/gauss (where LSB means least significant bit) at this FS setting, so the raw
reading of 1292 corresponds to 1292 / 6842 = 0.1888 gauss.
*/

#include <Wire.h>
#include <LIS3MDL.h>
#include <math.h>

char report[80];
int cal_x, cal_y;
float reference_heading;

LIS3MDL mag;
LIS3MDL::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32768, -32768, -32768};

// calibrate the magnetometer, the magnetometer must be moved through its full axis of rotation while calibrating
void calibrateSensor(){
  for (int i = 0; i < 200; i += 1) {
    mag.read();

    // minimum values
    running_min.x = min(running_min.x, mag.m.x);
    running_min.y = min(running_min.y, mag.m.y);
    running_min.z = min(running_min.z, mag.m.z);

    // maximum values
    running_max.x = max(running_max.x, mag.m.x);
    running_max.y = max(running_max.y, mag.m.y);
    running_max.z = max(running_max.z, mag.m.z);

    // print limits
    snprintf(report, sizeof(report), "min: {%+6d, %+6d, %+6d}   max: {%+6d, %+6d, %+6d}",
      running_min.x, running_min.y, running_min.z,
      running_max.x, running_max.y, running_max.z);
   // Serial.println(report);

    delay(50);
  }

  // calculate calibrated origin
  cal_x = (running_max.x + running_min.x) / 2;
  cal_y = (running_max.y + running_min.y) / 2;
}

// read raw data from magnetometer and adjust from calibration
void read() {
  mag.read();

  // adjust raw data for calibration
  mag.m.x -= cal_x;
  mag.m.y -= cal_y;

  // print adjusted values
  snprintf(report, sizeof(report), "M: %6d, %6d, %6d",
    mag.m.x, mag.m.y, mag.m.z);
  //Serial.println(report);
}

// returns the compass heading from magnetometer, range [0, 360]
float getCompassHeading() {
    read();
    float heading = calcAngle(mag.m.x, mag.m.y) * -1; // compass heading is angle clockwise from North, hence the * -1
    if (heading < 0) {
        return heading + 360.0;
    } else {
        return heading;
    }
}

// calculate the angle of point (x, y)
float calcAngle(int x, int y) {
  return degrees(atan2(x, y));
}

// stores the compass angle that is the desired direction
float saveHeading() {
  reference_heading = getCompassHeading();
}

// returns the angle that the robot is off the reference_heading, range [-180, 180]
float getRelativeAngle(float my_heading) {
  float angle_diff = (my_heading - reference_heading) * -1.0;

  Serial.print("Actual heading: "); Serial.print(my_heading);
  Serial.print(" Angle difference: "); Serial.println(angle_diff);

  if (angle_diff < -180.0) {
    return angle_diff + 360.0;
  } else if (angle_diff > 180.0) {
    return angle_diff - 360.0;
  } else {
    return angle_diff;
  }
}

// checks to see if facing target dirrection
int isTargetGoal(float my_heading) {
  if ((my_heading < 90.0) && (my_heading > -90.0)) {
    return 1;
  } else {
    return 0;
  }
}

// functions to run at start for testing
void startFunctionsTest() {
  // calibrate the sensor
  Serial.println("Calibration started");
  calibrateSensor();
  Serial.println("Calibration finished");

  // store the current direction after short delay
  Serial.print("Saving direction in: ");
  for (int n = 4; n > 0; n -= 1) {
    Serial.print(n);
    delay(1000);
  }
  Serial.println();
  saveHeading();
  Serial.print("Saved heading is: "); Serial.println(reference_heading);
  delay(2000);
}

void loopTest() {
  Serial.print("Angle to desired heading is: ");
  Serial.println(isTargetGoal(getRelativeAngle(getCompassHeading())));
}

void setup() {
  Serial.begin(9600);
  Serial.println("Magnetometer Test");
  Wire.begin();

  if (!mag.init()) {
    Serial.println("Failed to detect and initialize magnetometer!");
    while (1);
  }
  mag.enableDefault();

  startFunctionsTest();
}

void loop() {
  loopTest();
  delay(1000);
}
<file_sep>/Projects/2017/SoccerBots/trigonometry/Makefile
atan2: atan2.c
	gcc -std=c99 -o atan2 atan2.c -lm
	./atan2

.PHONY: clean
clean:
	rm atan2
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_ukebot/firmware_ukebot.ino
// UkeBot code for the 2016 MIDIBots
// Team _underscore_, Information Science Mechatronics, University of Otago

/*
TODO:
[ ] Consider refactoring to use Servo.writeMicroseconds() for more accurate control over positioning. Maybe add a wrapper function that accepts target angle as a float.
[ ] Calibrate slide positions to correspond to musical intervals
[ ] Add note-on messages for slide positioning
[ ] Add note-on messages for plucking indiviual strings (think of it as having 5 positions which can be reached: top, between first two strings, between middle strings, between last two strings, and bottom).  Could be tricky to get the timing right, as the time to pluck will depend on the current strumming position.  Perhaps just pick a single value that generally works OK.
[ ] Refactor to use arrays (2-dimensional?) for slide and strum (note, position pairs).
*/

// Define pin mappings for the MIDIBot shield:
#include <Servo.h> 
#include <MIDIBot.h>
MIDIBot UkeBot;

Servo strum;
Servo slide;

int pos = 0;    // variable to store the servo position

// Deprecated:
const int UP_NOTE = 50; // Middle C
const int DOWN_NOTE = 52; // D

// Servo angles for strumming...

const int STRUM_MIN = 92; 
const int STRUM_MAX = 105;

const int
	STRUM_0 = 92,	// OK
	STRUM_1 = 93,	//
	STRUM_2 = 95,	// 
	STRUM_3 = 97,	// 
	STRUM_4 = 98,	//
	STRUM_5 = 100,	// 
	STRUM_6 = 101,	// OK
	STRUM_7 = 103,	// Maybe 102 - sometimes hits the next string
	STRUM_8 = 104,	// 
	STRUM_9 = 105;	// OK

// ..and the slide.
// For slide max/min, we probably want min to be at the nut end, and max at the soundhole end.
const int SLIDE_MIN = 29;
const int SLIDE_MAX = 83;

const int
	SLIDE_0 = 29,
	SLIDE_1 = 37,
	SLIDE_2 = 45,
	SLIDE_3 = 52,
	SLIDE_4 = 59,
	SLIDE_5 = 65,
	SLIDE_6 = 71,
	SLIDE_7 = 77,
	SLIDE_8 = 83;

// MIDI note numbers for strumming...
const int
	STRUM_0_NOTE = 0,
	STRUM_1_NOTE = 1,
	STRUM_2_NOTE = 2,
	STRUM_3_NOTE = 3,
	STRUM_4_NOTE = 4,
	STRUM_5_NOTE = 5,
	STRUM_6_NOTE = 6,
	STRUM_7_NOTE = 7,
	STRUM_8_NOTE = 8,
	STRUM_9_NOTE = 9;

// ...and picking:
const int
	SLIDE_0_NOTE = 12,
	SLIDE_1_NOTE = 13,
	SLIDE_2_NOTE = 14,
	SLIDE_3_NOTE = 15,
	SLIDE_4_NOTE = 16,
	SLIDE_5_NOTE = 17,
	SLIDE_6_NOTE = 18,
	SLIDE_7_NOTE = 19,
	SLIDE_8_NOTE = 20;

// TODO: constants (or an array?) of servo positions that correspond to musical notes.

// Keep track of last slide position set, in case we end up getting really fancy with the logic and trying to optimise the timings or something.
int slide_position = SLIDE_MIN;

// Unused?
const int STRUM_DELAY = 100;

const int SLIDE_DELAY = 300;	// Worst-case slide motion time (HOME..MIN). Unconfirmed!


int i;

void move_slide(int pos) {
	// Clip motion to valid range:
	if (pos < SLIDE_MIN) {pos = SLIDE_MIN;}
	if (pos > SLIDE_MAX) {pos = SLIDE_MAX;}
	// Move it:
	slide.write(pos);
}

// TODO: similar to move_slide() but for picking, maybe?

void note_on(int note, int velocity) {
	switch (note) {
		case STRUM_0_NOTE:	strum.write(STRUM_0); break;
		case STRUM_1_NOTE:	strum.write(STRUM_1); break;
		case STRUM_2_NOTE:	strum.write(STRUM_2); break;
		case STRUM_3_NOTE:	strum.write(STRUM_3); break;
		case STRUM_4_NOTE:	strum.write(STRUM_4); break;
		case STRUM_5_NOTE:	strum.write(STRUM_5); break;
		case STRUM_6_NOTE:	strum.write(STRUM_6); break;
		case STRUM_7_NOTE:	strum.write(STRUM_7); break;
		case STRUM_8_NOTE:	strum.write(STRUM_8); break;
		case STRUM_9_NOTE:	strum.write(STRUM_9); break;

		case SLIDE_0_NOTE:	move_slide(SLIDE_0); break;
		case SLIDE_1_NOTE:	move_slide(SLIDE_1); break;
		case SLIDE_2_NOTE:	move_slide(SLIDE_2); break;
		case SLIDE_3_NOTE:	move_slide(SLIDE_3); break;
		case SLIDE_4_NOTE:	move_slide(SLIDE_4); break;
		case SLIDE_5_NOTE:	move_slide(SLIDE_5); break;
		case SLIDE_6_NOTE:	move_slide(SLIDE_6); break;
		case SLIDE_7_NOTE:	move_slide(SLIDE_7); break;
		case SLIDE_8_NOTE:	move_slide(SLIDE_8); break;		
	}
}

void note_off(int note, int velocity) {}


void setup()
{
	UkeBot.begin();
	
	// Servo setup:
	strum.attach(SERVO_3_PIN);
	slide.attach(SERVO_2_PIN);
	
	// Set initial position
	strum.write(STRUM_MIN);
	slide.write(SLIDE_MIN);
}

void self_test_picking() {
	strum.write(STRUM_1); delay(500);
	strum.write(STRUM_0); delay(500);
	strum.write(STRUM_3); delay(500);
	strum.write(STRUM_2); delay(500);
	strum.write(STRUM_5); delay(500);
	strum.write(STRUM_4); delay(500);
	strum.write(STRUM_7); delay(500);
	strum.write(STRUM_6); delay(500);
	strum.write(STRUM_9); delay(500);
	strum.write(STRUM_8); delay(500);

	strum.write(STRUM_8); delay(500);
	strum.write(STRUM_9); delay(500);
	strum.write(STRUM_6); delay(500);
	strum.write(STRUM_7); delay(500);
	strum.write(STRUM_4); delay(500);
	strum.write(STRUM_5); delay(500);
	strum.write(STRUM_2); delay(500);
	strum.write(STRUM_3); delay(500);
	strum.write(STRUM_0); delay(500);
	strum.write(STRUM_1); delay(500);
	
	strum.write(STRUM_0); delay(500);	// Return home
}

void self_test() {
	// TODO: test each semitone, not just min/max slide position. This would be easier if we used an array for the slide positions.
	digitalWrite(LED_PIN, HIGH);
	move_slide(SLIDE_MIN); delay(SLIDE_DELAY);
	self_test_picking();
	move_slide(SLIDE_MAX); delay(SLIDE_DELAY);
	self_test_picking();
	move_slide(SLIDE_MIN); delay(SLIDE_DELAY);
	digitalWrite(LED_PIN, LOW);
}

void loop()
{
	UkeBot.process_MIDI();
}
<file_sep>/Projects/2017/RescueBot/arduino/motor_test/motor_test.ino
//#include <line_follower.h>
//#include <robot.h>
//#include <seeed.h>

// Simple demonstration LED blinker for the DSpace Robot board
// The onboard LEDs are on digital pins 6-10.
//  D6: LED1, red
//  D7: LED2, orange
//  D8: LED3, yellow
//  D9: LED4, green
// D10: LED5, blue

// Note that we have generally modified our boards to allow PWM control of both H-bridge channels.  On the stock boards, pins 4 and 5 are enable, but pin 4 has no PWM support.  Also 6,7 motor A, 9,10 motor B.

// Left motor is on pin 0, right motor is on pin 1.
// RobotMotor class provides Init(), Stop(), Brake(), Forwards(), Backwards().
// RobotDrive is for higher-level control.  Doesn't have a turn-on-the spot behaviour.  We probably want PWM control over speed as well, so we might have to devise our own library for this.
// Might not even have to be hardcoded for the DSpace Robot board - perhaps any dual H-bridge configuration.

// Change pin 4 to 11 for our modified DSpace boards (PWM on fwd)

#define MOTOR_R_ENABLE 11
#define MOTOR_L_ENABLE 5

#define MOTOR_R_1_PIN 6
#define MOTOR_R_2_PIN 7

#define MOTOR_L_1_PIN 9
#define MOTOR_L_2_PIN 10

#define LEFT_MOTOR 0
#define RIGHT_MOTOR 1

const int MOTOR_L_DUTY=180;
const int MOTOR_R_DUTY=150;
//#include <line_follower.h>
//#include <robot.h>
//#include <seeed.h>

//static const int LED_PIN = 10;

void setup() {
	pinMode(MOTOR_L_ENABLE, OUTPUT); digitalWrite(MOTOR_L_ENABLE, LOW);
	pinMode(MOTOR_R_ENABLE, OUTPUT); digitalWrite(MOTOR_R_ENABLE, LOW);

	pinMode(MOTOR_L_1_PIN, OUTPUT); digitalWrite(MOTOR_L_1_PIN, LOW);
	pinMode(MOTOR_L_2_PIN, OUTPUT); digitalWrite(MOTOR_L_2_PIN, HIGH);

	pinMode(MOTOR_R_1_PIN, OUTPUT); digitalWrite(MOTOR_R_1_PIN, LOW);
	pinMode(MOTOR_R_2_PIN, OUTPUT); digitalWrite(MOTOR_R_2_PIN, HIGH);
}

// Low-level functions for driving the L and R motors independently...


void L_Fwd() {
	digitalWrite(MOTOR_L_1_PIN, LOW);
	digitalWrite(MOTOR_L_2_PIN, HIGH);
	analogWrite(MOTOR_L_ENABLE, 120);
//	digitalWrite(MOTOR_L_ENABLE, HIGH);
}

void L_Rev() {
	digitalWrite(MOTOR_L_1_PIN, HIGH);
	digitalWrite(MOTOR_L_2_PIN, LOW);
	analogWrite(MOTOR_L_ENABLE, MOTOR_L_DUTY);
//	digitalWrite(MOTOR_L_ENABLE, HIGH);
}

void L_Stop() {
	digitalWrite(MOTOR_L_1_PIN, LOW);
	digitalWrite(MOTOR_L_2_PIN, HIGH);
	analogWrite(MOTOR_L_ENABLE, 0);
//	digitalWrite(MOTOR_L_ENABLE, LOW);
}

void L_Brake() {
	digitalWrite(MOTOR_L_1_PIN, HIGH);
	digitalWrite(MOTOR_L_2_PIN, HIGH);
	analogWrite(MOTOR_L_ENABLE, MOTOR_L_DUTY);
//	digitalWrite(MOTOR_L_ENABLE, HIGH);
}


void R_Fwd() {
	digitalWrite(MOTOR_R_1_PIN, LOW);
	digitalWrite(MOTOR_R_2_PIN, HIGH);
	analogWrite(MOTOR_R_ENABLE, MOTOR_L_DUTY);
//	digitalWrite(MOTOR_R_ENABLE, HIGH);
}

void R_Rev() {
	digitalWrite(MOTOR_R_1_PIN, HIGH);
	digitalWrite(MOTOR_R_2_PIN, LOW);
	analogWrite(MOTOR_R_ENABLE, MOTOR_R_DUTY);
//	digitalWrite(MOTOR_R_ENABLE, HIGH);
}

void R_Stop() {
	digitalWrite(MOTOR_R_1_PIN, LOW);
	digitalWrite(MOTOR_R_2_PIN, HIGH);
	analogWrite(MOTOR_R_ENABLE, 0);
//	digitalWrite(MOTOR_R_ENABLE, LOW);
}

void R_Brake() {
	digitalWrite(MOTOR_R_1_PIN, HIGH);
	digitalWrite(MOTOR_R_2_PIN, HIGH);
	analogWrite(MOTOR_R_ENABLE, MOTOR_L_DUTY);
//	digitalWrite(MOTOR_R_ENABLE, HIGH);
}

// High-level functions for driving both motors at once:

void Fwd() {
	L_Fwd(); R_Fwd();
}

void Rev() {
	L_Rev(); R_Rev();
}

void Stop() {
	L_Stop(); R_Stop();
}

void Brake() {
	L_Brake(); R_Brake();
}

void veerL() {
	L_Stop(); R_Fwd();
}

void veerR() {
	L_Fwd(); R_Stop();
}

void spinL() {
	L_Rev(); R_Fwd();
}

void spinR() {
	L_Fwd(); R_Rev();
}

void low_level_test() {
	L_Fwd(); delay(2500); L_Stop(); delay(2500);
	L_Rev(); delay(2500); L_Stop(); delay(2500);
	R_Fwd(); delay(2500); L_Stop(); delay(2500);
	R_Rev(); delay(2500); L_Stop(); delay(2500);
}

void high_level_test() {
	Fwd(); delay(2500); Stop(); delay(2500);
	Rev(); delay(2500); Stop(); delay(2500);
	veerL(); delay(2500); Stop(); delay(2500);
	veerR(); delay(2500); Stop(); delay(2500);
	spinL(); delay(2500); Stop(); delay(2500);
	spinR(); delay(2500); Stop(); delay(2500);
	delay(5000);
}

void loop() {low_level_test(); high_level_test();}

void loop__() {
	L_Fwd();
	R_Fwd();
	delay(1500);
	L_Stop();
	R_Stop();
	delay(1000);
	L_Rev();
	R_Rev();
	delay(1500);
	L_Stop();
	R_Stop();
	delay(1000);
}

void loop_() {
//	digitalWrite(MOTOR_L_ENABLE, HIGH);
	analogWrite(MOTOR_L_ENABLE, 180);
	delay(2000);
	digitalWrite(MOTOR_L_ENABLE, LOW);
	delay(2000);
//	digitalWrite(MOTOR_R_ENABLE, HIGH);
	analogWrite(MOTOR_R_ENABLE, 150);
	delay(2000);
	digitalWrite(MOTOR_R_ENABLE, LOW);
	delay(2000);
}
<file_sep>/Projects/2017/SoccerBots/arduino/buzzer/fnaf.h
//Freddy's Jingle from Five Nights at Freddy's games/small section of Carmen Overture by <NAME>

const int fnafTempo = 110;
const int fnafArrayLength = 37;
float fnaf[fnafArrayLength][2] = {{72,1},{74,0.75},{72,0.25},{69,1},{69,1},
	{69,0.75},{67,0.25},{69,0.75},{70,0.25},{69,2},
	{70,1},{67,0.75},{72,0.25},{69,2},
	{65,1},{62,0.75},{67,0.25},{60,2},
	{62,2.5},{70,0.5},{69,0.5},{67,0.5},
	{69,0.5},{67,0.5},{69,0.5},{70,0.5},{69,2},
	{64,1},{69,1},{69,1},{68,0.75},{71,0.25},
	{76,4}};
<file_sep>/Projects/2017/SoccerBots/arduino/sensors_boris/magnetometer.h
/*
  Magnetometer
  Reads data from the LIS3MDL sensor and calculate a compass heading, aswell as
  return the angle to a specified heading
  The y-axis points in the forwards direction of the robot for a correct heading

TODO:
- implement correction for magmetic north
*/

/*
The sensor outputs provided by the library are the raw 16-bit values
obtained by concatenating the 8-bit high and low magnetometer data registers.
They can be converted to units of gauss using the
conversion factors specified in the datasheet for your particular
device and full scale setting (gain).

Example: An LIS3MDL gives a magnetometer X axis reading of 1292 with its
default full scale setting of +/- 4 gauss. The GN specification
in the LIS3MDL datasheet (page 8) states a conversion factor of 6842
LSB/gauss (where LSB means least significant bit) at this FS setting, so the raw
reading of 1292 corresponds to 1292 / 6842 = 0.1888 gauss.
*/

#include <Wire.h>
#include <LIS3MDL.h>
#include <math.h>

char report[80];
int cal_x, cal_y;
float target_heading;
float magnetometer_getCompassHeading();

LIS3MDL mag;
LIS3MDL::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32768, -32768, -32768};

float magnetometer_saveHeading() { // stores the compass angle that is the desired direction
	target_heading = magnetometer_getCompassHeading();
	EEPROM_write_int(EEPROM_TARGET_HEADING, target_heading);
	beep();
}

void magnetometer_reset() { // reset the running max and min values for each axis to allow a complete calibration
	running_min.x =  32767;
	running_min.y =  32767;
	running_min.z =  32767;
	running_max.x = -32768;
	running_max.y = -32768;
	running_max.z = -32768;
}

void magnetometer_calculateOrigin(int min_x, int min_y, int min_z, int max_x, int max_y, int max_z) {
	cal_x = (max_x + min_x) / 2;
	cal_y = (max_y + min_y) / 2;
}

void magnetometer_saveCalibration() { // stores the new calibrated origin in the EEPROM
	EEPROM_write_int(EEPROM_MIN_X, running_min.x);
	EEPROM_write_int(EEPROM_MIN_Y, running_min.y);
	EEPROM_write_int(EEPROM_MIN_Z, running_min.z);
	EEPROM_write_int(EEPROM_MAX_X, running_max.x);
	EEPROM_write_int(EEPROM_MAX_Y, running_max.y);
	EEPROM_write_int(EEPROM_MAX_Z, running_max.z);
	
	beep();
	delay(50);
	beep();
	delay(250);
}

void magnetometer_restoreCalibration() { // restores the calibrated origin stored in the EEPROM
	magnetometer_calculateOrigin(EEPROM_read_int(EEPROM_MIN_X), EEPROM_read_int(EEPROM_MIN_Y), EEPROM_read_int(EEPROM_MIN_Z),
					EEPROM_read_int(EEPROM_MAX_X), EEPROM_read_int(EEPROM_MAX_Y), EEPROM_read_int(EEPROM_MAX_Z));
	
        //playTune(nokia, nokiaTempo, nokiaArrayLength); FOR BORIS
        playTune(mk_shortened, mk_shortened_tempo, mk_shortened_array_length); //FOR SHUTTER
        //playTune(sherlock, sherlockTempo, sherlockArrayLength);
	/*beep();
	delay(50);
	beep();
	delay(250);*/
}

void magnetometer_beepUntilHeadingSaved() { // beeps the buzzer untill the save heading button is pressed
	while (digitalRead(SAVE_HEADING_BUTTON_PIN)) {
		beep();
		delay(200);
	}
	noTone(BUZZER);
	magnetometer_saveHeading();
}

void magnetometer_calibrateMagnetometer() { // calibrate the magnetometer, the magnetometer must be moved through its full axis of rotation while calibrating
	DEBUG("starting magnetometer calibration");
	magnetometer_reset();
	while(!digitalRead(CALIBRATION_MODE_SWITCH_PIN)){
		mag.read();

		// minimum values
		running_min.x = min(running_min.x, mag.m.x);
		running_min.y = min(running_min.y, mag.m.y);
		running_min.z = min(running_min.z, mag.m.z);

		// maximum values
		running_max.x = max(running_max.x, mag.m.x);
		running_max.y = max(running_max.y, mag.m.y);
		running_max.z = max(running_max.z, mag.m.z);

		// print limits
		// snprintf(report, sizeof(report), "min: {%+6d, %+6d, %+6d}   max: {%+6d, %+6d, %+6d}",
		//   running_min.x, running_min.y, running_min.z,
		//   running_max.x, running_max.y, running_max.z);
		// DEBUG(report);
	}

	magnetometer_saveCalibration();
	magnetometer_restoreCalibration(); // also calculates the origin
	magnetometer_beepUntilHeadingSaved(); // prompt for save heading
	DEBUG("ending magnetometer calibration");
}

void magnetometer_read() { // read raw data from magnetometer and adjust from calibration
	mag.read();

	// adjust raw data for calibration
	mag.m.x -= cal_x;
	mag.m.y -= cal_y;

	// print adjusted values
	// snprintf(report, sizeof(report), "M: %6d, %6d, %6d", mag.m.x, mag.m.y, mag.m.z);
	// DEBUG(report);
}

float magnetometer_calcAngle(int x, int y) { // calculate the angle of point (x, y)
	return degrees(atan2(x, y));
}

float magnetometer_getCompassHeading() { // returns the compass heading from magnetometer, range [0, 360]
	magnetometer_read();
	float heading = magnetometer_calcAngle(mag.m.x, mag.m.y) * -1; // compass heading is angle clockwise from North, hence the * -1
	if (heading < 0) {
		return heading + 360.0;
	} else {
		return heading;
	}
}

float magnetometer_getRelativeAngle(float actual_heading) { // returns the angle that the robot is off the target heading, range [-180, 180]
	float angle_diff = (actual_heading - target_heading) * -1.0;

	DEBUG_NOEOL("Actual heading: "); DEBUG_NOEOL(actual_heading);
	DEBUG_NOEOL(" Angle difference: "); DEBUG(angle_diff);

	if (angle_diff < -180.0) {
		return angle_diff + 360.0;
	} else if (angle_diff > 180.0) {
		return angle_diff - 360.0;
	} else {
		return angle_diff;
	}
}

int magnetometer_isTargetDirection(float actual_heading) { // checks to see if facing in the general target dirrection
	if ((actual_heading < 70.0) && (actual_heading > -70.0)) {
		return 1;
	} else {
		return 0;
	}
}

float magnetometer_getAngleToTarget() { // returns the angle needed to turn towards the target heading
	return magnetometer_getRelativeAngle(magnetometer_getCompassHeading());
}

void magnetometerTestLoop() {
	DEBUG_NOEOL("Angle to desired heading is: ");
	DEBUG(magnetometer_isTargetDirection(magnetometer_getAngleToTarget()));
}

void magnetometerSetup() {
	// Serial.begin(9600);
	// DEBUG("Magnetometer Test");
	Wire.begin();
	if (!mag.init()) {
		DEBUG("Failed to detect and initialize magnetometer!");
		while (1);
	}
	mag.enableDefault();
	
	magnetometer_restoreCalibration();
	target_heading = EEPROM_read_int(EEPROM_TARGET_HEADING);
}
<file_sep>/Documentation/Computer Vision/notes.txt
Computer Vision for Info Sci Mechatronics club

There are a variety of image processing and analysis methods that could be useful for projects such as our soccer and rescue robots for 2017.  I'll record a plain list of things here, and explore/explain in separate files...

 - Object recognition (ball, goal, rescue can, features of the playing field, other robots)

 - Lens distortion removal (image warping), for easier recognition of straight (and parallel, perpendicular) lines.

 - Stereoscopic imaging: identifying depth (distance) by comparing two images taken from slightly different vantage points (left and right)
	It might be possible, given the limited features and locations in the soccer field, to use a single camera and infer position and orientation just from 2D analysis of the perspective.


General Image Processing/Computer Vision

http://www.cs.dartmouth.edu/farid/downloads/tutorials/fip.pdf
<file_sep>/Documentation/Arduino/memory-and-EEPROM/memory-and-EEPROM.md
% Arduino EEPROM (and other memory types)
% <NAME> \<mailto:<EMAIL>\>
% 2017-06-29

## Arduino/ATmega/AVR?!

Some preliminary notes about the relationship between Arduino, the ATmega chip family, and the AVR architecture:

* **AVR** is the processor architecture.  This is an abstract specification that defines the instruction set, memory model, etc.  AVR is an 8-bit architecture, meaning that processor registers are 8 bits in length.

* **ATmega328p, ATmega2560, etc.** are specific types of microcontroller chip made by Atmel, which implement the AVR architecture.

* **Arduino** is a family of hobbyist-friendly circuit boards that integrate an ATmega chip, USB interface, power management, and a set of header pins having a standard layout.

The AVR architecture uses the Harvard memory model (named after the US university), which has separate memory and access buses for data and program code.  This is in contrast to the Von Neumann architecture, which has a combined memory for data and code with one memory access bus.


## SI vs. binary multiplier prefixes

In the SI system, prefixes such as kilo, micro, and so on are defined in terms of powers of 10 (kilo = 1000 = `10^3` = `1e3`).  However, computer memories are usually arranged in powers of 2, so there is a family of related binary multiplier prefixes that are close to the powers of 10:

----

	kilobinary = kibi = ki = 2^10 = 1024
	megabinary = mebi = Mi = 2^20 = 1048576
	gigabinary = gibi = Gi = 2^30 = 1073741824
	terabinary = tebi = Ti = 2^40 = 1099511627776
	
----

(Note that Frink does not recognise the "kilobinary" and "ki" forms.)


## Arduino memory types, sizes, and characteristics

There are several types of memory built into the microcontroller chips used on the Arduino:

### Registers:

These are the internal working memory of the processor.  To perform operations on variables, their values must be loaded into processor registers.  The result of operations are also stored in registers prior to being copied back out to RAM.

General-purpose registers are very fast, but there are very few of them (32 in the AVR architecture), and their names are fixed (`R0`..`R31`).  Each register is 8 bits in length.

See also: <https://en.wikipedia.org/wiki/Atmel_AVR_instruction_set#Processor_registers>

When programming in assembly language, you address and manipulate registers directly and have to manage their use yourself.  When using compiled Arduino C/C++ code, you can name variables how you like and the compiler will take care of mapping them to processor registers.

### Static RAM (SRAM):

This is the working memory of your program. Values of program variables are stored here (when not being worked on by the processor), along with runtime data structures such as the stack and the heap.

The ATmega328 has 2 kiB of SRAM, and the ATmega2560 has 8 kiB.  This is a tiny amount compared to a desktop PC, which likely has over a million times as much RAM!
	
### Flash program memory:

This is where program code is stored.  When you upload a sketch, the compiled and linked binary code is written to the chip's flash memory.  The program flash memory is non-volatile, so it will remain intact when the chip is powered off.  When the chip powers up, it loads and runs whatever program is stored in flash.

The flash memory capacity is 32 kiB on the ATmega328, and 256 kiB on the ATmega2560.

### EEPROM:

This stands for Electrically Eraseable Programmable Read-Only Memory.  This memory provides a non-volatile storage that your programs can use.  It is a little like a computer's hard drive, only very basic: instead of files and folders, you are limited to reading and writing one byte at a time.  EEPROM capacity is also extremely small: the Atmega chips used on Arduino boards provide between 1 and 4 kiB of EEPROM storage.

EEPROM cells have a limited lifespan: around 100,000 write cycles before they wear out.

Arduino-based 3D printers typically use the EEPROM to store machine-specific settings such as the X, Y and Z dimensions, axis homing directions, acceleration settings, etc.  A climate-control microcontroller might use the EEPROM for storing set-points for temperature and humidity, or information about what times of the day to run the climate control system.


### Conserving SRAM

The SRAM is very limited on the basic Arduino chips, and you can easily exhaust it by storing lots of strings in your program (e.g. for debugging messages).  Your program will likely behave very strangely if it runs out of SRAM (there is no memory checking)!  A handy way to save space is to declare string constants as `PROGMEM`.  This will cause them to be stored only in program flash memory, not in SRAM.  For example:

___
```c
#include <avr/pgmspace.h>
const char GREETING[] PROGMEM = "Welcome to Awesome Firmware v1.2\
(c) 2017 The Visionaries";
// ...
	Serial.println(GREETING);
// ...
```
___

(Hmm, I tested this and it doesn't seem to work...also, the `#include` doesn't seem to be necessary.)

See also: <https://www.arduino.cc/en/Reference/PROGMEM>

Using larger data types than necessary is also a waste of memory.  If you know that a variable will never hold a value outside the range of 0..255, use a `byte` rather than an `int`.

When logging data, consider streaming the data to a larger host computer via serial or Internet, rather than storing it in SRAM.



## Using the EEPROM

In practice, the EEPROM behaves a lot like a fixed-size non-volatile array of bytes.  Since the EEPROM capacity varies from chip to chip, it is useful to be able to find out how much is there (especially if you want to use a lot of it).  The `EEPROM.length()` function returns the number of bytes in the EEPROM.

(Hmm, only `EEPROM.length()` doesn't seem to be available in the Arduino version I'm testing on.  Apparently this got dropped when the EEPROM library was updated, and was later re-introduced.)

You can also use the precompiler constant `E2END` to find out the last available address on the EEPROM for the selected chip.  This value is determined an compile-time rather than run-time, and relies on choosing the correct board type in the Arduino IDE.  The size of the EEPROM would be `E2END+1`.

The Arduino API provides the following low-level functions for reading/writing EEPROM data (one byte at a time; the address values are byte-level):

_____________________________________________
```c
void EEPROM.write(int address, byte value)
byte EEPROM.read(int address)
```
_____________________________________________

It also provides these higher-level functions, that allow you to read/write larger/more complex data:

___
```c
EEPROM.put(address, value)
EEPROM.get(address, value)
```
___

However, not all Arduino versions seem to support this.  If reading/writing int values (signed or unsigned---but watch for overflow!), you could use the following:

___
```c

void EEPROM_write_int(int address, int value) {
	EEPROM.write(address, highByte(value));
	EEPROM.write(address + sizeof(byte), lowByte(value));
}

int EEPROM_read_int(int address) {
	byte highByte = EEPROM.read(address);
	byte  lowByte = EEPROM.read(address + sizeof(byte));
	return word(highByte, lowByte);
}

// You could combine the two reads and the return into a one-liner:
int EEPROM_read_int(int address) {
	return word(EEPROM.read(address), EEPROM.read(address + sizeof(byte)));
}
```
___

Values of type `int` occupy two bytes, so you will need to leave room in your addressing scheme for both bytes.  There is nothing stopping you from overwriting part of an `int` in EEPROM, so be careful!  You should sketch out your EEPROM addressing scheme before implementing and using it, and document it in your program.

Since EEPROM supports only a limited number of write cycles, you should try to avoid unnecessary writes.  To help you with this, the Arduino API provides the `EEPROM.update()` function, which only writes if the data would be changing (it does a read first to see if the write is necessary):

___
```c
EEPROM.update(int address, byte value)
```
___

(This also is absent from the Arduino version I'm testing with!)

You can check the EEPROM contents for data corruption using the CRC function.  This won't let you recover or correct the data corruption---only detect it.  There is example code here:

<https://www.arduino.cc/en/Tutorial/EEPROMCrc>


## Further reading/reference:

 - <https://www.arduino.cc/en/Tutorial/EEPROM>
 - <https://www.arduino.cc/en/Reference/EEPROM>
<file_sep>/Projects/2016/MIDIBots/arduino/digital_pin_test/ano/src/sketch.ino
../../digital_pin_test.ino<file_sep>/Projects/2017/SoccerBots/arduino/sensors/nokia.h
//default ringtone on Nokia phones

const int nokiaTempo = 194;
const int nokiaArrayLength = 13;
float nokia[nokiaArrayLength][2] = {{88,0.5},{86,0.5},{78,1},{80,1},
	{85,0.5},{83,0.5},{74,1},{76,1},
	{83,0.5},{81,0.5},{73,1},{76,1},
	{81,4}};
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_template/ano/src/sketch.ino
../../firmware_template.ino<file_sep>/Projects/2017/SoccerBots/python-opencv-picamera-test.py
#!/usr/bin/env python2

# Demo/test of the Python OpenCV bindings (wot I built from source) with the picamera (Python-MMAL) module.
# Based on http://www.pyimagesearch.com/2015/03/30/accessing-the-raspberry-pi-camera-with-opencv-and-python/

# <NAME>'s excellent picamera library:
from picamera.array import PiRGBArray
from picamera import PiCamera

# The OpenCV library:
import cv2

from time import sleep,time
import sys
import numpy


# Set up camera and a NumPy array wrapper suitable for transferring the image data into OpenCV

target_framerate = 20
# 0.08 s frame capture with 50 Hz target


# Had originally thought 1640 x 922 (scaled down to 820 x 461) would be suitable
# However: "PiCameraResolutionRounded: frame size rounded up from 820x461 to 832x464"
# Perhaps 1640 x 1232 capture, scaled down by 4 to 410 x 308?
# Nope: "PiCameraResolutionRounded: frame size rounded up from 410x308 to 416x320"
# 640 x 360 for 16:9?
# Argh: "PiCameraResolutionRounded: frame size rounded up from 640x360 to 640x368" (360 is not divisible by 16)
capture_res = (1640,922)
#capture_res = (3280,2464)
#capture_res = (1640,1232)
capture_res = (640,480)
capture_res = (320,240)
capture_res = (128,96)
# Nope, even then: PiCameraResolutionRounded: frame size rounded up from 3280x2464 to 3296x2464
# Hmm, but at full res, we can't capture faster than 15 Hz.
# Argh, 1640 is not divisible by 16 either!: "PiCameraResolutionRounded: frame size rounded up from 1640x1232 to 1664x1232"
# So maybe we have to capture at full res (3280x2464)? At least those are div by 16...
#resized_res = (capture_res[0]/4, capture_res[1]/4)
#resized_res = (640,480)
#resized_res = (1640,1232)
# But we need a resolution where x and y are both divisible by 16 that has a usefully scaled down version that's also divisible by 16.
# I think given that the capture resolutions are so limited, don't worry about those or the integer scaling part.
# But we will need the resized resolution to be the correct aspect ratio and divisble by 16 in both axes.
# Ah, here are some:
# 16:9 modes:
resized_res = (768,432)
#resized_res = (512,288)
resized_res = (256,144)
resized_res = (640,480)
resized_res = (320,240)
resized_res = (128,96)
# 4:3 modes:
#resized_res = (512,384)
#camera = PiCamera()

#camera = PiCamera(sensor_mode=4, resolution=capture_res, framerate=target_framerate)
camera = PiCamera(resolution=capture_res, framerate=target_framerate)
camera.iso = 400
camera.shutter_speed = 20000
#camera.shutter_speed = 15000
print("calibrating gain...")
sleep(2)
camera.exposure_mode = 'off'
print("camera.analog_gain = ",camera.analog_gain)

# Now fix the white balace:
print("Hold a white card in front of the camera now!")
sleep(5)
g = camera.awb_gains
print("Thanks; camera.awb_gains = ",g)
camera.awb_mode = 'off'
camera.awb_gains = g

# That's it for the camera setup.

#camera.start_preview(alpha=127)
#sleep(60)

# array wrapper:
rawCapture = PiRGBArray(camera)
# If resizing, you likely have to tell the array how big to be:
#rawCapture = PiRGBArray(camera, size=resized_res)

sleep(0.1)

def capture_still():
	# The camera provides different ports for still images and video, specified via use_video_port. Specify False for better quality still images, or True when streaming video.
	camera.capture(rawCapture, format="bgr", use_video_port=False, resize=resized_res)
	image = rawCapture.array
	cv2.imshow("Image", image)
	# Image doesn't display until this:
	cv2.waitKey(0)

	# Oh, wait, do we have to clear it before continuing?!
	rawCapture.truncate(0)
	# Yes, that was it!

#capture_still()

#exit()

then = 0
now = 0
end_time = 0

# For colour matching testing:
# Helper for converting a decimal <r,g,b> triple into an integer [b,g,r] array:
def bgr(r,g,b):
		return [int(round(b*255)), int(round(g*255)), int(round(r*255))]

blue_lo = bgr(0.1, 0.24, 0.4)
blue_hi = bgr(0.2, 0.4, 0.6)
lower = numpy.array(blue_lo, dtype="uint8")
upper = numpy.array(blue_hi, dtype="uint8")

# What about HSV, which worked pretty well for the original SimpleCV attempt?
# OpenCV docs: For HSV, Hue range is [0,179], Saturation range is [0,255] and Value range is [0,255]
# Goal blue H was about 102/180, S=190/255, V=50/255 (or 50/100?)
goal_hsv = (102, 190, 100)
goal_hsv_tolerance = (12, 65, 100)
# Um, does bad stuff happen if the range results in a min below 0 or a max above 255?
# Should the range be applied additively or multiplicatively?
def lower(hsv, hsv_tolerance):
	return [float(hsv[0])-hsv_tolerance[0], float(hsv[1])-hsv_tolerance[1], float(hsv[2])-hsv_tolerance[2]]
def upper(hsv, hsv_tolerance):
	return [float(hsv[0])+hsv_tolerance[0], float(hsv[1])+hsv_tolerance[1], float(hsv[2])+hsv_tolerance[2]]

goal_hsv_lo = numpy.array(lower(goal_hsv, goal_hsv_tolerance), dtype="uint8")
goal_hsv_hi = numpy.array(upper(goal_hsv, goal_hsv_tolerance), dtype="uint8")

# capture frames from the camera
# CME: can we specify resize with use_video_port=True? Seems so
# Hmm, why is the video capture noticeably dimmer than the still?
# And why is the frame rate so jittery? Hmm, it seems to be slow to capture the next frame every second frame. Setting the capture frame rate low makes it more obvious.
# Hmm, it's not just every second frame that's pausing...and how long the pause is when capturing the new frame depends on the timing of the loop!
# Sometimes there's a delay of over 2 s while restarting the loop!
# Ah-ha: looks like it's the resizing that causes the jitter.
# Note that when use_video_port=False, reading each image takes about 0.3 s!
print("Press 'q' to quit...")
# , resize=resized_res
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
#while True:
	#camera.capture_sequence(rawCapture, format="bgr", use_video_port=True, resize=resized_res)
	#print(time() - end_time)
	#print("start loop")
	now = time()
	elapsed_time = now  - then
	sys.stderr.write('Frame rate: ' + str(round(1.0/elapsed_time,1)) + ' Hz\n')

	# grab the raw NumPy array representing the image, then initialize the timestamp
	# and occupied/unoccupied text
	#print("copy array")
	image = frame.array
	#image = rawCapture.array
 
	# show the frame
	#print("show raw frame")
	#cv2.imshow("Frame", image)
	#print(" wait >")
	#key = cv2.waitKey(16) & 0xFF
	# if the `q` key was pressed, break from the loop
	#if key == ord("q"):
	#	break
	
	# Let's try some basic colour matching:
	# Should we convert to HSV first?
	# Hmm, it seems that the chroma resolution is quite poor at certain camera settings (low res)...or maybe it's just the lower res exaggerating it.
	#print("BGR -> HSV...")
	hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
	# Remember, it's (B,G,R) otherwise!
	mask = cv2.inRange(hsv, goal_hsv_lo, goal_hsv_hi)
	#output = cv2.bitwise_and(image, image, mask=mask)

	# show the matching area:
	#print("show masked frame")
	cv2.imshow("Frame", mask)
	#print(" wait >")
	key = cv2.waitKey(16) & 0xFF
	# if the `q` key was pressed, break from the loop
	if key == ord("q"):
		break
	
	# clear the stream in preparation for the next frame
	#print("truncating...")
	rawCapture.truncate(0)
 
	then = now
	
	#print("end loop")
	#end_time = time()
<file_sep>/Projects/2016/MIDIBots/arduino/midi_input_test/ano/src/sketch.ino
../../midi_input_test.ino<file_sep>/Projects/2017/SoccerBots/stopeverything.sh
#!/bin/sh
killall tclsh
killall pd
killall python
<file_sep>/Projects/2016/MIDIBots/arduino/midi_input_test/midi_input_test.ino


void setup() {
	//  Set up serial port for MIDI baud rate (31.25 kbit/s):
	Serial.begin(31250);
	
	// We might want to use the LED to indicate activity (though don't use delay() unnecessarily (e.g. blink())).
	pinMode(LED_BUILTIN, OUTPUT);
	digitalWrite(LED_BUILTIN, LOW);

  flash(); flash(); flash();
}

void loop() {
  int readByte;
  if (Serial.available() > 0) {
    readByte = Serial.read();
    if (readByte == 0x90)
      flash();
  }
}

void ledOn() {digitalWrite(LED_BUILTIN, HIGH);}

void ledOff() {digitalWrite(LED_BUILTIN, LOW);}

void flash() {
    ledOn();
    delay(50);
    ledOff();
}
<file_sep>/Projects/2016/MIDIBots/arduino/drum_roll/ano/src/sketch.ino
../../drum_roll.ino<file_sep>/Projects/2016/MIDIBots/arduino/midi_test/midi_test.ino
  // MIDI output testing, CME 2016


// Tempo:
const int del = 100;
//const int del = 200;
//const int del = 60;

//const int switchPin = 2;
//int switchState = 0;
const int ledPin = 13;

// Arrays for the current and previous state of all the key switches:
// Notably, we need to keep the TX pin free for MIDI transmission.
// NOTE: on the Arduino Mega, the big double-row of pins starts has a pair of 5V pins, then digital pins 22 and 23.  The last digital pin is 53, and then there are two grounds at the end.
const int numKeys = 56;
const int firstKeyPin = 2;
// NOTE: awkward mismatch of pin numbers and array indexes, since we're not necessarily starting at 0!  It's a bit wasteful of memory, but let's just make the array bigger, and ignore any we aren't using, and use the same numbering for the array elements as for the pin numbers for the input switches.
int currSwitchState[numKeys + firstKeyPin];
int prevSwitchState[numKeys + firstKeyPin];

void setup() {
	//  Set up serial port for MIDI baud rate (31.25 kbit/s):
	Serial.begin(31250);
//	Serial.begin(9600);
//	Serial.println("setup()...");
	
//	pinMode(switchPin, INPUT); digitalWrite(switchPin, HIGH);	// Microswitch input, needs internal pull-up resistor.
	pinMode(ledPin, OUTPUT); digitalWrite(ledPin, LOW);

	// Set up input pins and initialise switch state arrays:
	for (int pin = firstKeyPin; pin <= firstKeyPin + numKeys - 1; pin++) {
	//	Serial.println(pin);
		pinMode(pin, INPUT);
		digitalWrite(pin, HIGH);
		currSwitchState[pin] = 0;
		prevSwitchState[pin] = 0;
	}

}

void loop() {
	readSwitches();
//	switchTest(); delay(2);
//	glissando();
//	playRandomNote();
//	discoBeat();
//	hecticBeat();
//	playArpeggios();
}
void glissando() {
	// play notes from F#-0 (0x1E) to F#-5 (0x5A):
	for (int note = 0; note < 128; note ++) {
		// Note on channel 1 (0x90), some note value (note), middle velocity (0x45):
		playNote(1, note, 100);
		delay(50);
		// Note on channel 1 (0x90), same note value (note), silent velocity (0x00):
		playNote(1, note, 0x00);
	//	delay(100);
	}
}

// This function from example Arduino code:
//  plays a MIDI note.  Doesn't check to see that
//  cmd is greater than 127, or that data values are  less than 127:
void noteOn(int cmd, int pitch, int velocity) {
	Serial.write(cmd);
	Serial.write(pitch);
	Serial.write(velocity);
}

// IMO, a more sensible way would be to construct the first byte based on the channel number, and not require the 0x9 to be passed at all:
// NOTE: channel numbers here are the logical user-friendly channel numbers (1..16), not the internal values used to represent them.
void playNote(int channel, int pitch, int velocity) {
	// Convert channel to a 4-bit value:
	if (channel > 16 || channel < 1) channel = 0;
	Serial.write(0x90 | channel);
	Serial.write(pitch);
	Serial.write(velocity);
}

void playRandomNote() {
	int pitch = random(30, 110);
	int velocity = random(40, 120);
	int duration = random(20, 100);
	int pause = random(0, 100);
	noteOn(0x90, pitch, velocity);
	delay(duration);
	noteOn(0x90, pitch, 0); // off
//	delay(pause);
}

void playArpeggios() {
    int octave = random(0,4) + 4;
    int arpDegree = random(0,3);
    int note = octave * 12 + scaleDegreeToSemitone(arpeggioDegreeToScaleDegree(arpDegree));
    playNote(1, note, 100);
    delay(70);
    playNote(1, note, 0);
}


int scaleDegreeToSemitone(int scaleDegree) {
	if (scaleDegree == 0) return 0;
	else if (scaleDegree == 1) return 2;
	else if (scaleDegree == 2) return 4;
	else if (scaleDegree == 3) return 5;
	else if (scaleDegree == 4) return 7;
	else if (scaleDegree == 5) return 9;
	else if (scaleDegree == 6) return 11;
	else return 0;
}

int arpeggioDegreeToScaleDegree(int arpeggioDegree) {
	if (arpeggioDegree == 0) return 0;
	else if (arpeggioDegree == 1) return 3;
	else if (arpeggioDegree == 2) return 5;
	else return 0;
}


void discoBeat() {
	int velocity = random(64, 128);
	playNote(10, 36, velocity); delay(del); playNote(10, 36, 0);
	playNote(10, 42, velocity); delay(del); playNote(10, 42, 0);
	playNote(10, 38, velocity); delay(del); playNote(10, 38, 0);
	playNote(10, 42, velocity); delay(del); playNote(10, 42, 0);
}

void hecticBeat() {
	int velocity = random(48, 128);
	playNote(10, 36, velocity); delay(del); playNote(10, 36, 0);
	playNote(10, 41, velocity); delay(del); playNote(10, 41, 0);
	playNote(10, 38, velocity); delay(del); playNote(10, 38, 0);
	playNote(10, 41, velocity); delay(del); playNote(10, 41, 0);
}

// Trigger a MIDI note based on an attached switch.  We'll use polling for this (so it can be scaled up to a polyphonic keyboard controller), and we'll also need to remember the state of the switch(es) for triggering note on/off.
/*
void switchTest() {
	if (!digitalRead(switchPin) != switchState) {
		switchState = !switchState;
		playNote(1, 69, switchState * 100);
	}
	digitalWrite(ledPin, switchState);
}
*/

void blink() {
	digitalWrite(ledPin, HIGH);
	delay(50);
	digitalWrite(ledPin, LOW);
}

// Read the state of the input key switches and trigger note on/off MIDI events as necessary.
void readSwitches() {
	for (int pin = firstKeyPin; pin <= firstKeyPin + numKeys - 1; pin++) {
		currSwitchState[pin] = !digitalRead(pin);	// Active-low; negate here.
	//	digitalWrite(ledPin, currSwitchState[pin]);
		if (currSwitchState[pin] != prevSwitchState[pin]) {
		//	blink();
		//	playNote(1, 60 + pin, 60);
			playNote(1, 36 + pin, currSwitchState[pin] * 100);	// Note on/off depending on whether the key is pressed.
		}
		prevSwitchState[pin] = currSwitchState[pin];	// Store the state for next time.
	}
	delay(1); // Hold off just a little do reduce spurious note on/off due to switch contact bouncing.
}

<file_sep>/Projects/2016/MIDIBots/arduino/pwm_examples/pwm_3.ino
// MIDI note tests for Mechatronics, CME 2016-03-21
// 1. Loop sanity check.  Watch for off-by-one errors!
// 2. Translate note frequency formula into C syntax and check (MIDI note 69 should give 440 Hz). 
// 3. Correct integer division in formula!

void setup() {
	delay(8000);  // safety delay for serial I/O
	Serial.begin(9600);
}

void loop() {
	for (int n = 0; n < 128; n++) {
		float f = 440 * pow(2, (n - 69) / 12.0);
		Serial.print(n);
		Serial.print(": ");
		Serial.println(f);
	}
	delay(5000);
}
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_synthbot.old/firmware_synthbot.ino
// SynthBot code for the 2016 MIDIBots
// Team _underscore_, Information Science Mechatronics, University of Otago
// NOTE: there's a limit to how low a frequency this can play, given the PWM settings.  Note frequencies will jump around below this limit.


// Define pin mappings for the MIDIBot shield:
const int
	MIDI_1x_PIN = 2,
	MIDI_2x_PIN = 3,
	MIDI_4x_PIN = 4,
	MIDI_8x_PIN = 5,
	MOSFET_PWM_PIN = 6,
	SERVO_1_PIN = 9,
	SERVO_2_PIN = 10,
	SERVO_3_PIN = 11,
	MOSFET_2_PIN = 12,
	MOSFET_3_PIN = A1, /* not A3 as on the silkscreen! */
	MOSFET_4_PIN = A2, /* not A4 as on the silkscreen! */
	LED_PIN = 13,
	LATENCY_ADJUST_PIN = A0,
	SELF_TEST_PIN = A5
;

int
	MIDI_channel = 0;	// 1..16, 10=drums

// MIDI messages have up to 2 data bytes, which we store in an array:
int dataByte[2], statusByte = 0, i = 0;

void clearData(){
  dataByte[0] = 0;
  dataByte[1] = 0;
  i = 0;
}


// Even though this is only monophonic, it's still useful to keep track of the note currently being played, so that any note-off messages for other notes can be ignored.
int current_note_number = 0;


// Tricky low-level code from Chris for programming the PWM output for precise frequencies...

// On the Mega, we have timer1 attached to pins D11 and D12, D12 being the primary one.
// On "ordinary" Arduinos, it's on pins 9 and 10.  On the MIDIBot shield, pin 10 is Servo 2 (middle pin).
// Ideally we'd use the PWM MOSFET output on D6, but that uses Timer 0 and doesn't support frequency-accurate PWM, as far as I can tell.
// This will need an external circuit anyway to limit the current and block DC, so it's not a huge hassle to add a MOSFET circuit (with resistors and protection diode) to this as well.  It can be powered from one of the aux 12 V headers.
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
const int OUTPUT_PIN     = 12;
const int OUTPUT_PIN_AUX = 11;
#else
const int OUTPUT_PIN     = 10;
const int OUTPUT_PIN_AUX = 9;
#endif


// General settings:
const int prescale = 8;

// Some PWM control definitions:
#define TIMER_CLK_STOP		0x00	///< Timer Stopped
#define TIMER_CLK_DIV1		0x01	///< Timer clocked at F_CPU
#define TIMER_CLK_DIV8		0x02	///< Timer clocked at F_CPU/8
#define TIMER_CLK_DIV64		0x03	///< Timer clocked at F_CPU/64
#define TIMER_CLK_DIV256	0x04	///< Timer clocked at F_CPU/256
#define TIMER_CLK_DIV1024	0x05	///< Timer clocked at F_CPU/1024

// This defines which bits within the TCCRnB register determine the prescale factor:
#define TIMER_PRESCALE_MASK	0xF8        // 0B11111000

// Map desired prescale divider to register bits:
byte timer_prescale_bits(int prescale) {
	if (prescale == 1)
		return 0x01;
	else if (prescale == 8)
		return 0x02;
	else if (prescale == 64)
		return 0x03;
	else if (prescale == 256)
		return 0x04;
	else if (prescale == 1024)
		return 0x05;
	else
		return 0x00;	// Error?!
}

// Map MIDI note numbers to frequency in Hz:
float frequency(int note) {
	return 440 * pow(2.0, (note - 69) / 12.0);
}

// Function for computing the PWM wrap limit ("top" value) based on the desired frequency (and the CPU clock speed and prescaler setting):
unsigned int top(float frequency) {
	return round(F_CPU / (prescale * frequency) - 1);
}


void pwm(float frequency, float duty_cycle) {
	TCNT1 = 0;        // Reset timer counter
//	pwm_off(); // Maybe necessary to avoid stuck at 5 V condition? Nope, not enough...
	
	unsigned int wrap_limit = top(frequency);
	OCR1A = wrap_limit;
	OCR1B = int(wrap_limit * duty_cycle);

	// TCCR1A's timer mode should already be set, so just use TCCR1B to start PWM (is that sufficient?):
	TCCR1A = _BV(COM1A0) | _BV(COM1B1) | _BV(WGM11) | _BV(WGM10);
	TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11);	
	TCCR1B = (TCCR1B & TIMER_PRESCALE_MASK) | timer_prescale_bits(prescale);
}

void pwm_off() {
	TCCR1B = (TCCR1B & TIMER_PRESCALE_MASK) | TIMER_CLK_STOP;
	TCNT1 = 0;
	digitalWrite(OUTPUT_PIN, LOW); // This seems to be necessary to silence it properly (sometimes gets stuck at 5 V otherwise!)
}
	

void flash(int on_time, int off_time) {
	digitalWrite(LED_PIN, HIGH);
	delay(on_time);
	digitalWrite(LED_PIN, LOW);
	delay(off_time);	
}

// Routine for displaying a number by blinking the LED (since hardware serial will be taken already by the MIDI comms):
void flash_number(int n) {
	delay(500);
	// Hundreds:
	for (int i = 0; i < (n % 1000 / 100); i++) {
		flash(800, 800);
	}
	delay(500);
	// Tens:
	for (int i = 0; i < (n % 100 / 10); i++) {
		flash(400, 400);
	}
	delay(500);
	// Fastest flashing for the ones:
	for (int i = 0; i < (n % 10); i++) {
		flash(200, 200);
	}
	delay(500);
}


// Read MIDI channel DIP switches and store the result:
// NOTE: Freetronics USBDroid has a pull-down resistor on pin D4, which makes that pin unusable! It acts as if D4 is always grounded, i.e. the switch is on.
// Multiply the 4x bit by 4 on a normal Arduino, or 0 to force it off.
void read_MIDI_channel() {
	MIDI_channel =
		!digitalRead(MIDI_8x_PIN) * 8 +
		!digitalRead(MIDI_4x_PIN) * 4 +
		!digitalRead(MIDI_2x_PIN) * 2 +
		!digitalRead(MIDI_1x_PIN);
}

void self_test() {
	// Robot-specific self-test routine goes here
//	flash(50, 50); flash(50, 50); flash(50, 50); flash(50, 50);
	read_MIDI_channel();
	flash_number(MIDI_channel);
	pwm(frequency(40), 0.1); delay(250);
	pwm(frequency(52), 0.1); delay(250);
	pwm(frequency(64), 0.1); delay(250);
	pwm(frequency(76), 0.1); delay(250);
	pwm_off();
}

void setup()
{
	// Set up pin modes for the MIDIBot Shield (input, internal pull-up resistor enabled):
	pinMode(MIDI_1x_PIN, INPUT_PULLUP);
	pinMode(MIDI_2x_PIN, INPUT_PULLUP);
	pinMode(MIDI_4x_PIN, INPUT_PULLUP);
	pinMode(MIDI_8x_PIN, INPUT_PULLUP);
	
	// Ditto for self-test button:
	pinMode(SELF_TEST_PIN, INPUT_PULLUP);
	
	// TODO: output pins (servos, MOSFETs, LED)
	pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, LOW);
	pinMode(MOSFET_PWM_PIN, OUTPUT); analogWrite(MOSFET_PWM_PIN, 0);
	pinMode(MOSFET_2_PIN, OUTPUT); digitalWrite(MOSFET_2_PIN, LOW);
	pinMode(MOSFET_3_PIN, OUTPUT); digitalWrite(MOSFET_3_PIN, LOW);
	pinMode(MOSFET_4_PIN, OUTPUT); digitalWrite(MOSFET_4_PIN, LOW);
	pinMode(SERVO_1_PIN, OUTPUT); digitalWrite(SERVO_1_PIN, LOW);
	pinMode(SERVO_2_PIN, OUTPUT); digitalWrite(SERVO_2_PIN, LOW);
	pinMode(SERVO_3_PIN, OUTPUT); digitalWrite(SERVO_3_PIN, LOW);
	
	// Initialise MIDI channel number according to DIP switch settings:
	read_MIDI_channel();
	flash_number(MIDI_channel);
	
	// Set up MIDI communication:
	Serial.begin(31250);
	clearData();
	
	// Attach interrupt for self-test button and function:
//	attachInterrupt(digitalPinToInterrupt(SELF_TEST_PIN), self_test, FALLING);
	// Only available on certain pins?  Just poll in main loop instead.
	
	// Flash to indicate startup/ready:
//	flash(50, 200); flash(50, 400); flash(50, 200); flash(400, 0);
	// or not, since we're flashing the MIDI channel ID number anyway. ;)
	
	// Run self-test at startup as well?
//	self_test();
	
	pwm_off();
}

void loop()
{
	if (!digitalRead(SELF_TEST_PIN)) {
		self_test();
	}
	process_MIDI();
//	test_blink();
//	test_button();
//	test_flash_number();
//	test_MIDI_channel();
//	test_MOSFETs();
//	test_MOSFETs_cycle();
//	test_PWM();
}

void process_MIDI() {
	if (Serial.available() > 0) {
		int data = Serial.read();
		if (data > 127) {
			// It's a status byte. Store it for future reference.
			statusByte = data;
			clearData();  
		} else {
			// It's a data byte.
			dataByte[i] = data;
			if (statusByte == (0x90 | MIDI_channel) && i == 1) {
				// Note-on message received
				if (dataByte[1] == 0 && dataByte[0] == current_note_number) {
					// Stop note playing
					pwm_off();
					digitalWrite(LED_PIN, LOW);
					analogWrite(MOSFET_PWM_PIN, 0);
				} else {
					// Start note playing
					current_note_number = dataByte[0];
					pwm(frequency(current_note_number), dataByte[1] / 127.0 / 2); // TODO: map velocity to PWM duty
					// Plus illumination:
					digitalWrite(LED_PIN, HIGH);
					analogWrite(MOSFET_PWM_PIN, 64);
				}
			} else if (statusByte == (0x80 | MIDI_channel) && i == 1 && dataByte[0] == current_note_number) {
				// Note-off message received
				// TODO: also respond to note-on with vel=0 as note-off
				// Stop note playing
				pwm_off();
				digitalWrite(LED_PIN, LOW);
				analogWrite(MOSFET_PWM_PIN, 0);
			}
			i++;
			// TODO: error detection if i goes beyond the array size.
		}
	}
}

void test_blink() {
	digitalWrite(LED_PIN, HIGH);
	delay(500);
	digitalWrite(LED_PIN, LOW);
	delay(500);
}

void test_button() {
	digitalWrite(LED_PIN, !digitalRead(SELF_TEST_PIN));
}

void test_flash_number() {
	if (!digitalRead(SELF_TEST_PIN)) {
		flash_number(3);
		delay(2000);
		flash_number(16);
		delay(2000);
		flash_number(235);
		delay(2000);
		flash_number(127);
		delay(2000);
	}
}

void test_MIDI_channel() {
	if (!digitalRead(SELF_TEST_PIN)) {
		read_MIDI_channel();
		Serial.println(MIDI_channel);
		flash_number(MIDI_channel);
	}
}

void test_MOSFETs() {
	if (!digitalRead(SELF_TEST_PIN)) {
		analogWrite(MOSFET_PWM_PIN, 64);
		digitalWrite(MOSFET_2_PIN, HIGH);
		digitalWrite(MOSFET_3_PIN, HIGH);
		digitalWrite(MOSFET_4_PIN, HIGH);
	} else {
		analogWrite(MOSFET_PWM_PIN, 0);
		digitalWrite(MOSFET_2_PIN, LOW);
		digitalWrite(MOSFET_3_PIN, LOW);
		digitalWrite(MOSFET_4_PIN, LOW);
	}
}

void test_MOSFETs_cycle() {
	digitalWrite(MOSFET_PWM_PIN, HIGH);
	delay(250);
	digitalWrite(MOSFET_PWM_PIN, LOW);
	
	digitalWrite(MOSFET_2_PIN, HIGH);
	delay(250);
	digitalWrite(MOSFET_2_PIN, LOW);
	
	digitalWrite(MOSFET_3_PIN, HIGH);
	delay(250);
	digitalWrite(MOSFET_3_PIN, LOW);

	digitalWrite(MOSFET_4_PIN, HIGH);
	delay(250);
	digitalWrite(MOSFET_4_PIN, LOW);
}

void test_PWM() {
	analogWrite(MOSFET_PWM_PIN, 64);
	delay(250);
	analogWrite(MOSFET_PWM_PIN, 0);
	delay(500);
}
<file_sep>/Projects/2017/SoccerBots/testpipe2pd.py
import time
message = 0
def send2pd(message):
	print(str(message) + ";" )

while True: 
	message += 1
	send2pd(message)
<file_sep>/Projects/2017/SoccerBots/rpicam-setup.sh
#!/bin/sh
#config raspberry pi camera
v4l2-ctl -c power_line_frequency=1 -c iso_sensitivity_auto=0 -c auto_exposure=1 -c exposure_time_absolute=100 -c  white_balance_auto_preset=3 -c iso_sensitivity=4 -c scene_mode=0
<file_sep>/Projects/2016/MIDIBots/kicad/TODO.txt
[ ] Find a footprint for the Molex 8981 power connector with larger through-holes
[ ] Reposition MIDI IN connector in case it interferes with the IOREF and spare header pin above it (present on newer Arduino models).

[ ] Add some more net labels to assist with assigning track styles on PCB.
[ ] Filter and bypass caps as part of each MOSFET circuit? Think DC motor, solenoid, lamp, etc.

[ ] Check for conflict with big USB-B on some Arduino boards
[ ] Remove mounting holes in board?
[ ] Decorative logo?  Maybe for rev. 2!  (Ideas: musical note logo, smiley face)

--
[Y] Move LAT_ADJ label so it won't be covered by the PC900V IC socket
[Y] Move the servo headers to the left a little
[Y] Label SW2 as CH or MIDI_CH
[Y] Corrected layout orientation of input diode D1
[Y] Moved some part reference labels to be visible
--
[Y] Added Current_Loop track style
[Y] Assigned some more track styles and ran a few additional tracks
--
[Y] Remap pins for MOSFET 3 and 4 to make PCB routing easier
[Y] Break out remaining Arduino pins (there's only about 2!) and put header with a GND in the bottom left corner.
[Y] Fixed unexpected mess with use of old DIP socket footprints
[Y] Various footprints remapped
--
[Y] Flip resistors around MOSFET circuits and reroute tracks
[Y] Add 12V aux track
[Y] Route VCC tracks/zones
[Y] Fixed some unconnected track segments
[Y] Moved some reference labels
[Y] Finalise outline and add ground plane!
--
[Y] Fix weird grounding layout on the decoupling cap on the PC900V.
--
[Y] Remove shield silkscreening to allow more legible text.
[Y] Remove stray "P3" from back silkscreening.
--
[Y] Remove 10 cm x 10 cm alignment markers (in case they will be unwanted)
--
[Y] Move entire board to be nearer the origin (in case that's important for keeping within the size limit)? Or set origin, if that's possible. Paul says the origin should be at the bottom-left of the board.  There is also a "Use auxiliary axis as origin" option when plotting to Gerber - relevant?  Seems so.  Oh, don't forget to do the same with the drill settings!
[Y] Add copyright information and tidy up notice
--
[Y] Tidy up +12V trace supplying the Arduino VIN.
--
[Y] Move the MOSFET output flyback diodes to the other side of the connector and LED, for slightly tidier layout.
--
[Y] Make MOSFET connector labels more consistent.
--
[Y] Swap power and signal lines on the servo connectors!
<file_sep>/Projects/2017/SoccerBots/arduino/dspace_servo_motor_test/dspace_servo_motor_test.ino
//New file location at motor_control.
// Testing the IR reflectance sensors with the DSpace Robot board
// Working towards a line-following robot for Robocup 2017

// Current config (on the 2013 Dalek test-bed) uses pin 2 on each of sensor blocks SENSOR0, SENSOR1, SENSOR2, and SENSOR3.
// These correspond to A0..A3.
// A4 and A5 are also available

// Will need light/dark thresholds for each sensor for calibration...
// ...or just one for all, if they read consistently enough


#define BAUD_RATE 115200

// TODO: this was true of the Dalek, but what about the SoccerBot?:
// Right Dalek motor is considerably more powerful, so we need to be able to regulate each separately...
// Don't go too high with these on the Dalek as it will draw too much current and cause resets!
#include <Servo.h> //incldued for kicker
Servo Kicker;
const int SERVO_PIN = 13; //Servo 2, Pin 2 SDK.
const int KICKER_MIN = 100;
const int KICKER_MAX = 60; //these will need testing.
const int KICKER_DELAY = 1000;
const int MOTOR_L_DUTY=128;
const int MOTOR_R_DUTY=128;
const int DIR_MASK 		= 0b00100000;
const int MOTOR_MASK 	= 0b01000000;
const int SPEED_MASK 	= 0b00011111;
const int MESSAGE_TYPE_MASK = 0b10000000;
const int KICKER_MASK = 0b00000001;

#define MOTOR_R_ENABLE 5
#define MOTOR_L_ENABLE 11

#define MOTOR_R_1_PIN 10
#define MOTOR_R_2_PIN 9

#define MOTOR_L_1_PIN 7
#define MOTOR_L_2_PIN 6

// Unused?
#define LEFT_MOTOR 0
#define RIGHT_MOTOR 1

#define R_LED 11
#define O_LED 2
#define Y_LED 12
#define G_LED 13
#define BUZZER 3
#define L_BUTTON 19
#define R_BUTTON 8

#define DEBUGGING 0
/*
#define debug(message) \
	do { if (DEBUGGING) Serial.println(message); } while (0)
*/
#ifdef DEBUGGING
	#define DEBUG(x) Serial.println (x)
#else
	#define DEBUG(x)
#endif


const int MOTOR_TOGGLE_SWITCH = 18; //physical pin 2 on sensor block 4.

int motors_enabled = 0;

void setup() {
	Kicker.attach(SERVO_PIN);
	Kicker.write(KICKER_MIN);
	pinMode(MOTOR_TOGGLE_SWITCH, INPUT); digitalWrite(MOTOR_TOGGLE_SWITCH, 1);
	pinMode(R_LED, OUTPUT); digitalWrite(R_LED, LOW);
	pinMode(O_LED, OUTPUT); digitalWrite(O_LED, LOW);
	pinMode(Y_LED, OUTPUT); digitalWrite(Y_LED, LOW);
	pinMode(G_LED, OUTPUT); digitalWrite(G_LED, LOW);
	pinMode(BUZZER, OUTPUT);
	pinMode(L_BUTTON, INPUT); digitalWrite(L_BUTTON, HIGH); // or INPUT_PULLUP on newer Arduino
	pinMode(R_BUTTON, INPUT); digitalWrite(R_BUTTON, HIGH);	// NOTE: hardware problem with Sensor 0 Pin 1 on the Dalek board?  It's stuck at only about 1.7 V when pulled high.  Oh, hardwired onboard LED!  Have now removed resistor R4 to open that circuit. :)

	pinMode(MOTOR_L_ENABLE, OUTPUT); digitalWrite(MOTOR_L_ENABLE, LOW);
	pinMode(MOTOR_R_ENABLE, OUTPUT); digitalWrite(MOTOR_R_ENABLE, LOW);

	pinMode(MOTOR_L_1_PIN, OUTPUT); digitalWrite(MOTOR_L_1_PIN, LOW);
	pinMode(MOTOR_L_2_PIN, OUTPUT); digitalWrite(MOTOR_L_2_PIN, HIGH);

	pinMode(MOTOR_R_1_PIN, OUTPUT); digitalWrite(MOTOR_R_1_PIN, LOW);
	pinMode(MOTOR_R_2_PIN, OUTPUT); digitalWrite(MOTOR_R_2_PIN, HIGH);

	Serial.begin(BAUD_RATE);

	#ifdef DEBUGGING
	Serial.println("\n\nDspace Motor Controller - Info Sci Mechatronics v0.01");
	#endif
}

// TODO: we don't have a buzzer on the SoccerBot, so this can go:

	// Around 2100-2500 Hz sounds good on the piezo buzzer we have, with 2100 Hz being quite loud (near resonant frequency).
/*	digitalWrite(G_LED, HIGH); tone(BUZZER, 2100, 100); delay(200);
	digitalWrite(G_LED, LOW);  tone(BUZZER, 2200, 100); delay(200);
	digitalWrite(G_LED, HIGH); tone(BUZZER, 2300, 100); delay(200);
	digitalWrite(G_LED, LOW);  tone(BUZZER, 2400, 100); delay(200);
	digitalWrite(G_LED, HIGH); tone(BUZZER, 2500, 100); delay(200);
	delay(200); digitalWrite(G_LED, LOW);
}

// Audible click for debugging
// WARNING: using click() as defined below makes the right motor not work at all.  Shared pins, maybe?  Perhaps it needs a delay() of at least the duration of the click?

void click() {
	tone(BUZZER, 2100, 2);
}


void beep_bad() {
	tone(BUZZER, 2500, 100); delay(200);
	tone(BUZZER, 2100, 200); delay(200);
}

void beep_good() {
	tone(BUZZER, 2100, 100); delay(100);
	tone(BUZZER, 2500, 100); delay(100);
}
*/
// Low-level functions for driving the L and R motors independently...

void L_Fwd() {
	if (!motors_enabled) return;
	digitalWrite(MOTOR_L_1_PIN, LOW);
	digitalWrite(MOTOR_L_2_PIN, HIGH);
	analogWrite(MOTOR_L_ENABLE, MOTOR_L_DUTY);
//	digitalWrite(MOTOR_L_ENABLE, HIGH);
}

void L_Rev() {
	if (!motors_enabled) return;
	digitalWrite(MOTOR_L_1_PIN, HIGH);
	digitalWrite(MOTOR_L_2_PIN, LOW);
	analogWrite(MOTOR_L_ENABLE, MOTOR_L_DUTY);
//	digitalWrite(MOTOR_L_ENABLE, HIGH);
}

void L_Stop() {
	digitalWrite(MOTOR_L_1_PIN, LOW);
	digitalWrite(MOTOR_L_2_PIN, HIGH);
	analogWrite(MOTOR_L_ENABLE, 0);
//	digitalWrite(MOTOR_L_ENABLE, LOW);
}

void L_Brake() {
	if (!motors_enabled) return;
	digitalWrite(MOTOR_L_1_PIN, HIGH);
	digitalWrite(MOTOR_L_2_PIN, HIGH);
	analogWrite(MOTOR_L_ENABLE, MOTOR_L_DUTY);
//	digitalWrite(MOTOR_L_ENABLE, HIGH);
}


void R_Fwd() {
	if (!motors_enabled) return;
	digitalWrite(MOTOR_R_1_PIN, LOW);
	digitalWrite(MOTOR_R_2_PIN, HIGH);
	analogWrite(MOTOR_R_ENABLE, MOTOR_R_DUTY);
//	digitalWrite(MOTOR_R_ENABLE, HIGH);
}

void R_Rev() {
	if (!motors_enabled) return;
	digitalWrite(MOTOR_R_1_PIN, HIGH);
	digitalWrite(MOTOR_R_2_PIN, LOW);
	analogWrite(MOTOR_R_ENABLE, MOTOR_R_DUTY);
//	digitalWrite(MOTOR_R_ENABLE, HIGH);
}

void R_Stop() {
	digitalWrite(MOTOR_R_1_PIN, LOW);
	digitalWrite(MOTOR_R_2_PIN, HIGH);
	analogWrite(MOTOR_R_ENABLE, 0);
//	digitalWrite(MOTOR_R_ENABLE, LOW);
}

void R_Brake() {
	if (!motors_enabled) return;
	digitalWrite(MOTOR_R_1_PIN, HIGH);
	digitalWrite(MOTOR_R_2_PIN, HIGH);
	analogWrite(MOTOR_R_ENABLE, MOTOR_R_DUTY);
//	digitalWrite(MOTOR_R_ENABLE, HIGH);
}


void L_Drive(float speed){
	if (!motors_enabled) return;
	if (speed < 0){
		digitalWrite(MOTOR_L_1_PIN, HIGH);
		digitalWrite(MOTOR_L_2_PIN, LOW);
	}else{
		digitalWrite(MOTOR_L_1_PIN, LOW);
		digitalWrite(MOTOR_L_2_PIN, HIGH);
	}

	analogWrite(MOTOR_L_ENABLE, (int) round(speed * MOTOR_L_DUTY));
}


void R_Drive(float speed){
	if (!motors_enabled) return;
	if (speed < 0){
		digitalWrite(MOTOR_R_1_PIN, HIGH);
		digitalWrite(MOTOR_R_2_PIN, LOW);
	}else{
		digitalWrite(MOTOR_R_1_PIN, LOW);
		digitalWrite(MOTOR_R_2_PIN, HIGH);
	}

	analogWrite(MOTOR_R_ENABLE, (int) round(speed * MOTOR_R_DUTY));
}


// High-level functions for driving both motors at once:

void Veer(float left_speed, float right_speed){
	L_Drive(left_speed); R_Drive(right_speed);

}

void Fwd() {
	L_Fwd(); R_Fwd();
}

void Rev() {
	L_Rev(); R_Rev();
}

void Stop() {
	L_Stop(); R_Stop();
}

void Brake() {
	L_Brake(); R_Brake();
}

void veerL() {
	L_Stop(); R_Fwd();
}

void veerR() {
	L_Fwd(); R_Stop();
}

void spinL() {
	L_Rev(); R_Fwd();
}

void spinR() {
	L_Fwd(); R_Rev();
}

void L_Spd(int speed, bool dir) {
	if (!motors_enabled) {speed = 0;}
	digitalWrite(MOTOR_L_1_PIN, dir);
	digitalWrite(MOTOR_L_2_PIN, !dir);
	analogWrite(MOTOR_L_ENABLE, speed);
}


void R_Spd(int speed, bool dir) {
	if (!motors_enabled) {speed = 0;}
	digitalWrite(MOTOR_R_1_PIN, dir);
	digitalWrite(MOTOR_R_2_PIN, !dir);
	analogWrite(MOTOR_R_ENABLE, speed);
}

void kick(){
	Kicker.write(KICKER_MAX);
	delay(KICKER_DELAY);
	Kicker.write(KICKER_MIN);
	delay(KICKER_DELAY);

}

void kicker_move(int direction) {
	Kicker.write(direction? KICKER_MAX: KICKER_MIN);
}

/*
void calc_track(int move)
{
	if (backtrack[move][0] > 0)
	{
		backtrack[move][0]++;
	}
	else
	{
		for (int i = 0; i < 5; i++)
		{
			for (int j = 9; j > 0; j--)
			{
				backtrack[i][j] = backtrack[i][j-1];
			}
		}
		for (int i = 0; i < 5; i++)
		{
			backtrack[i][0] = 0;
		}
		backtrack[move][0] = 1;
	}
}


void retrace()
{
	for (int i = 0; i < 10; i++)
	{
		if (backtrack[i][0] > 0)
		{
			//spin right, so spin left
			for (int j = 0; j < backtrack[i][0]; j++)
				spinL();
		}
		if (backtrack[i][1] > 0)
		{
			//veer right, so veer left
			for (int j = 0; j < backtrack[i][1]; j++)
				Veer(0.7,1);
		}
		if (backtrack[i][2] > 0)
		{
			//forward, so move back
			for (int j = 0; j < backtrack[i][2]; j++)
				Rev();
		}
		if (backtrack[i][3] > 0)
		{
			//veer left, so veer right
			for (int j = 0; j < backtrack[i][3]; j++)
				Veer(1,0.7);
		}
		if (backtrack[i][4] > 0)
		{
			//spin left, so spin right
			for (int j = 0; j < backtrack[i][4]; j++)
				spinR();
		}
	}

}
*/
// For detecting the line, we'll have a function to read an analog value for each sensor and compare with a light/dark threshold.




/*
void control() {
//	digitalWrite(Y_LED, LOW);

	l_line = isBlack(L_PIN);
	m_line = isBlack(M_PIN);
	r_line = isBlack(R_PIN);

	debug_line_sensors_with_LEDs();

	// TODO: could shift and combine the boolean values for more readable code
	// e.g. switch bits ... 0b000 -> lost ... 0b010 -> fwd ...

	if (!l_line && !m_line && !r_line) {
		DEBUG("Lost!");
		// Would be nice to light red LED if lost.
	//	digitalWrite(Y_LED, HIGH);
	//	beep_bad();
	//	Fwd();
		Rev();
	//	Veer(-0.9,-0.7);	// TODO: figure out why the left motor doesn't work here.
	//	retrace();
	}
	if (!l_line && !m_line &&  r_line) {
		DEBUG("spin right");
		spinR();
	//	calc_track(0);
	}
	if (!l_line &&  m_line && !r_line) {
		DEBUG("fwd");
	//	beep_good();
		Fwd();
	//	calc_track(2);
	}
	if (!l_line &&  m_line &&  r_line) {
		DEBUG("veer right");
		Veer(1.0,0.8);
	//	calc_track(1);
		}
	if ( l_line && !m_line && !r_line) {
		DEBUG("spin left");
		spinL();
	//	calc_track(4);
	}
	if ( l_line && !m_line &&  r_line) {
		DEBUG("?!");
	//	digitalWrite(Y_LED, HIGH);
		Stop();
	//	calc_track(1);
	}
	if ( l_line &&  m_line && !r_line) {
		DEBUG("veer left");
		Veer(0.8,1.0);
	//	calc_track(3);
	}
	if ( l_line &&  m_line &&  r_line) {
		DEBUG("perpendicular?!");
	//	digitalWrite(Y_LED, HIGH);
	//	Fwd();
		Veer(1.0,0.6);
	//	calc_track(2);
	}
}


void toggleLED(){
	led_state = !led_state;
	digitalWrite(G_LED, led_state);
}
*/

/*void motor_control(){
	if(Serial.available() > 0){
		int data = Serial.read();
		if((data&MOTOR_MASK)>>7 == 1){
			R_Spd((data&SPEED_MASK)<<2, (data&DIR_MASK)>>6);
			Serial.println("R forward");
			Serial.println((data&SPEED_MASK)<<2);
		}
		else{
			L_Spd((data&SPEED_MASK)<<2, (data&DIR_MASK)>>6);
			Serial.println("L forward");
			Serial.println((data&SPEED_MASK)<<2);
		}

	}
}

void test_loop() {
	//TODO: Add servo control code.
	if (!digitalRead(MOTOR_TOGGLE_SWITCH)) {motors_enabled = 1;}
	if (digitalRead(MOTOR_TOGGLE_SWITCH)) {motors_enabled = 0;}
	if (!motors_enabled) {Stop();}else(){motor_control();}

	//delay(CYCLE_TIME);

}
 */

void motor_control(){
	if (Serial.available() > 0) {
		int data = Serial.read();
		if ((data&MESSAGE_TYPE_MASK)>>7==0){
			kicker_move(data&KICKER_MASK);
        }else{
			if ((data&MOTOR_MASK)>>6) {
				R_Spd((data&SPEED_MASK)<<3, (data&DIR_MASK)>>5);
				Serial.println("R forward (DIR, Speed): ");
				Serial.println((data&DIR_MASK)>>5);
				Serial.println((data&SPEED_MASK)<<3);
			} else {
				L_Spd((data&SPEED_MASK)<<3, (data&DIR_MASK)>>5);
				Serial.println("L forward (DIR, Speed): ");
				Serial.println((data&DIR_MASK)>>5);
				Serial.println((data&SPEED_MASK)<<3);
			}
		}
	} else {
		if (!motors_enabled) {Stop();}
	}
}


void servo_midpoint(){
  Kicker.write(95);	// TODO: fix hardcoding
}

void loop(){
	motors_enabled = !digitalRead(MOTOR_TOGGLE_SWITCH);
	motor_control();
}


void _loop() {
  motors_enabled = !digitalRead(MOTOR_TOGGLE_SWITCH);
  Serial.println(motors_enabled);
  Fwd();
  delay(1000);
  Stop();
  kicker_move(1);

  delay(500);

  Rev();
  delay(1000);
  Stop();
  kicker_move(0);

  delay(500);
}
<file_sep>/Projects/2018/Rubik's Cube/stepper_test/stepper_test.ino
const int steps_per_rev = 200;
const int degrees_per_step = 360 / steps_per_rev;
const int motor_pin = 10;

void spin(float rpm, float num_rotations) {
  float min_delay = 300/rpm - 1;
  for(int i = 0; i <= num_rotations*200; i++) {
    digitalWrite(motor_pin, HIGH);
    delay(min_delay);
    digitalWrite(motor_pin, LOW);
    delay(1);
  }
}

void accel(float rpm, float ramp_up) {
  for(float current_rpm = 0; current_rpm <= rpm; current_rpm += rpm / 10) {
    spin(current_rpm, ramp_up/10);
  } 
}

void decel(float initial_rpm, float ramp_down) {
  for(float current_rpm = initial_rpm; current_rpm >= 0; current_rpm -= initial_rpm / 10) {
    spin(current_rpm, ramp_down);
  }
  digitalWrite(motor_pin, HIGH);
}

void ninety() { //total: 0.25 rotations
   accel(10,);
   spin(10,);
   decel(10,);
}

void setup() {
  pinMode(motor_pin, OUTPUT);
  accel(10);
}

void loop() {
  spin(10, 100); 
  //digitalWrite(motor_pin, HIGH); 
}
<file_sep>/Projects/2017/SoccerBots/arduino/sensors_boris/ultrasonic.h
// NOTE: DFRobot sensor board must be jumpered correctly for TTL operation and have been programmed (EEPROM via serial) to passive PWM mode!
// See https://www.dfrobot.com/wiki/index.php/URM37_V3.2_Ultrasonic_Sensor_(SKU:SEN0001)

const int trig = 5;
const int pwm = 8;
const int unit_division_factor = 40; // Supposedly 50 us/cm but Tobias found that 40 gave better accuracy.
int us_distance;
const int RANGE_SENSOR_TIMEOUT = 15000; // microseconds for pulseIn() timeout (to avoid excessive blocking delay)


void ultrasonic_setup() {
	pinMode(trig, OUTPUT); digitalWrite(trig, HIGH);
        pinMode(pwm, INPUT);
}

int getUSDistance() {
	digitalWrite(trig, LOW);
        delayMicroseconds(10);    
	digitalWrite(trig, HIGH);
	
	us_distance = pulseIn(pwm, LOW, RANGE_SENSOR_TIMEOUT);
	return us_distance / unit_division_factor;
}
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_synthbot/firmware_synthbot.ino
// SynthBot code for the 2016 MIDIBots
// Team _underscore_, Information Science Mechatronics, University of Otago
// NOTE: there's a limit to how low a frequency this can play, given the PWM settings.  Note frequencies will jump around below this limit.

/*
 * TODO:
 * [ ] Fix note-playing glitch at startup (first note is a weird bleep). Is it due to calling pwm_off() at startup?  Should we always play a dummy test tone at startup?
 * [ ] Also, even with a duty cycle of 0 it doesn't shut off completely.  This appears to explain the problem: http://stackoverflow.com/questions/23853066/how-to-achieve-zero-duty-cycle-pwm-in-avr-without-glitches
*/

#include <MIDIBot.h>
MIDIBot synthBot;

int current_note_number = 0;

// Tricky low-level code from Chris for programming the PWM output for precise frequencies...

// There's a good explanation of how this works here:
// http://maxembedded.com/2011/08/avr-timers-pwm-mode-part-i/

// On the Mega, we have timer1 attached to pins D11 and D12, D12 being the primary one.
// On "ordinary" Arduinos, it's on pins 9 and 10.  On the MIDIBot shield, pin 10 is Servo 2 (middle pin).
// Ideally we'd use the PWM MOSFET output on D6, but that uses Timer 0 and doesn't support frequency-accurate PWM, as far as I can tell.
// This will need an external circuit anyway to limit the current and block DC, so it's not a huge hassle to add a MOSFET circuit (with resistors and protection diode) to this as well.  It can be powered from one of the aux 12 V headers.
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
const int OUTPUT_PIN     = 12;
const int OUTPUT_PIN_AUX = 11;
#else
const int OUTPUT_PIN     = 10;
const int OUTPUT_PIN_AUX = 9;
#endif


// General settings first:

// The prescale is applied to the CPU clock (F_CPU) by frequency division.
// Only certain values are supported: /1, /8, /64, /256, /1024
const int prescale = 8;

// Some PWM control definitions:
#define TIMER_CLK_STOP		0x00	///< Timer Stopped
#define TIMER_CLK_DIV1		0x01	///< Timer clocked at F_CPU
#define TIMER_CLK_DIV8		0x02	///< Timer clocked at F_CPU/8
#define TIMER_CLK_DIV64		0x03	///< Timer clocked at F_CPU/64
#define TIMER_CLK_DIV256	0x04	///< Timer clocked at F_CPU/256
#define TIMER_CLK_DIV1024	0x05	///< Timer clocked at F_CPU/1024

// This defines which bits within the TCCRnB register determine the prescale factor:
#define TIMER_PRESCALE_MASK	0xF8        // 0B11111000

// Map desired prescale divider to register bits:
byte timer_prescale_bits(int prescale) {
	if (prescale == 1)
		return 0x01;
	else if (prescale == 8)
		return 0x02;
	else if (prescale == 64)
		return 0x03;
	else if (prescale == 256)
		return 0x04;
	else if (prescale == 1024)
		return 0x05;
	else
		return 0x00;	// Error?!
}

// Map MIDI note numbers to frequency in Hz:
float frequency(int note) {
	return 440 * pow(2.0, (note - 69) / 12.0);
}

// Function for computing the PWM wrap limit ("top" value) based on the desired frequency (and the CPU clock speed and prescaler setting):
unsigned int top(float frequency) {
	return round(F_CPU / (prescale * frequency) - 1);
}


void pwm(float frequency, float duty_cycle) {
	TCNT1 = 0;        // Reset timer counter
//	pwm_off(); // Maybe necessary to avoid stuck at 5 V condition? Nope, not enough...
	
	unsigned int wrap_limit = top(frequency);
	OCR1A = wrap_limit;
	OCR1B = int(wrap_limit * duty_cycle);

	// TCCR1A's timer mode should already be set, so just use TCCR1B to start PWM (is that sufficient?):
	TCCR1A = _BV(COM1A0) | _BV(COM1B1) | _BV(WGM11) | _BV(WGM10);
	TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11);	
	TCCR1B = (TCCR1B & TIMER_PRESCALE_MASK) | timer_prescale_bits(prescale);
}

void pwm_off() {
	TCCR1B = (TCCR1B & TIMER_PRESCALE_MASK) | TIMER_CLK_STOP;
	TCNT1 = 0;
	digitalWrite(OUTPUT_PIN, LOW); // This seems to be necessary to silence it properly (sometimes gets stuck at 5 V otherwise!)
}
	

void self_test() {
	// Robot-specific self-test routine goes here
	pwm(frequency(40), 0.1); delay(250);
	pwm(frequency(52), 0.1); delay(250);
	pwm(frequency(64), 0.1); delay(250);
	pwm(frequency(76), 0.1); delay(250);
	pwm_off();
}

void setup()
{
	synthBot.begin();
	pwm_off();
}

void loop()
{
	synthBot.process_MIDI();
}

void note_on(int note, int velocity){
	// Start note playing
	current_note_number = note;	// Store current note for future reference (since we're monophonic)
	// We map MIDI velocity to PWM duty cycle.  Divide by two since the upper half (1..0.5) sounds the same as the lower half (0..0.5).
	pwm(frequency(current_note_number), velocity / 127.0 / 2.0);
	// Plus illumination:
	digitalWrite(LED_PIN, HIGH);
	analogWrite(MOSFET_PWM_PIN, 64);	
}

void note_off(int note, int velocity){
	// Because this is monophonic, we only want to turn the note off if the note-off message we just received is for the note we're currently playing.
	// That is, ignore any note-offs for notes other than the current one.
	if (note == current_note_number) {
		pwm_off();
		digitalWrite(LED_PIN, LOW);
		analogWrite(MOSFET_PWM_PIN, 0);
	}
}
<file_sep>/Documentation/Arduino/EEPROM_demo/EEPROM_demo.ino
#include <EEPROM.h>

void setup() {
	Serial.begin(9600);
	Serial.println("EEPROM demo");
	
	/*
	// Not available in the Arduino version I'm using for testing.
	Serial.print("EEPROM length = ");
	Serial.print(EEPROM.length());
	Serial.println(" bytes");
	*/
	
	// Alternatively, you can also determine the EEPROM size at compile time (which relies on the board type being correctly selected when you compile):
	Serial.print("EEPROM length = ");
	Serial.print(E2END + 1);
	Serial.println(" bytes");
}

void stop() {while(1) {}}

// To write an int to EEPROM requires two write() calls, since write() works only in bytes and an int is two.
// Not sure whether to have address be a specific byte address or to multiply by sizeof(int) within the functions to create a virtual addressing scheme for int values only.
// Probably safer to leave as raw byte addresses in case we have to mix and match, but watch for accidental overwriting!

void EEPROM_write_int(int address, int value) {
	EEPROM.write(address, highByte(value));
	EEPROM.write(address + sizeof(byte), lowByte(value));
}

// Ditto for reading:

int EEPROM_read_int(int address) {
	/*
	byte highByte = EEPROM.read(address);
	byte  lowByte = EEPROM.read(address + sizeof(byte));
	return word(highByte, lowByte);
	*/
	// Or simply:
	return word(EEPROM.read(address), EEPROM.read(address + sizeof(byte)));
}

// TODO: ditto for unsigned int?  Or will this approach work for both?  Note that overflows can happen silently, e.g. 


void loop() {
	// Writing a single byte:
	EEPROM.write(0, 42);
	Serial.print(EEPROM.read(0));
	Serial.println();
	
	// Writing a multi-byte value:
	int val = 12345;
//	int val = 0;
	EEPROM.write(0,val); // NOTE: only writes the first byte!
	Serial.print(EEPROM.read(0));
	Serial.print(" ");
	Serial.print(EEPROM.read(1));
	Serial.println();
	
	// EEPROM I/O using the higher-level update() and get() functions:
	// Hmm, also not available in the Arduino version I'm using here. :(
	// Will use DIY functions for reading/writing int values instead...
	
	val = -12345;
	int out = 0;
//	EEPROM.update(0,val);
//	EEPROM.put(0,val);
	EEPROM_write_int(0, val);
	out = EEPROM_read_int(0);
//	EEPROM.get(0,out);
	Serial.print(out);
	Serial.println();

	// Dump out the entire EEPROM contents:
	for (int addr = 0; addr < E2END; addr++) {
		Serial.print(addr, HEX);
		Serial.print('	');
		Serial.println(EEPROM.read(addr), HEX);
	}
	
	stop();
}
<file_sep>/Projects/2017/SoccerBots/a_blob_test_cv2.py
import cv2
import numpy as np;
import camera_setup

# Read image

execfile("camera_setup.py")
camera.capture(rawCapture, format="bgr")
image = rawCapture.array

im = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

# Set up the detector with default parameters.

params = cv2.SimpleBlobDetector_Params()
# Change thresholds
params.minThreshold = 10;    # the graylevel of images
params.maxThreshold = 200;

params.filterByColor = True
params.blobColor = 255

# Filter by Area
params.filterByArea = False
params.minArea = 10000

params.filterByInertia = False
params.filterByConvexity = False

detector = cv2.SimpleBlobDetector(params)


# Detect blobs.
keypoints = detector.detect(im)
print keypoints

# Draw detected blobs as red circles.
# cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures the size of the circle corresponds to the size of blob
im_with_keypoints = cv2.drawKeypoints(im, keypoints, numpy.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)

largest_blob = [0,0]
x = 0
for key in keypoints:
	if key.size > largest_blob[1]:
		largest_blob[0] = x
		largest_blob[1] = key.size
	x = x+1
	
print largest_blob

# Show keypoints
cv2.imshow("Keypoints", im_with_keypoints)
cv2.waitKey(0)
<file_sep>/Projects/2017/SoccerBots/arduino/sensors/sensors.ino
// TODO: main statement of the purpose of this code!
#include <LIS3MDL.h>
#include <Wire.h>
#include <math.h>
#include <EEPROM.h>
#include <HTInfraredSeeker.h>


#define SHUTTER 1
#define DEBUGGING 0

const int NUM_SENSORS = 8;
const int analog_sensor_pins[] = {A0,A1,A2,A3,A4,A5,A6,A7};
const int ir_sensor_angles[] = {180, -107, -80, -53, -27, 0, 27, 53, 80, 107};
float ir_values[NUM_SENSORS];
const float IR_GAIN_THRESHOLD = 0.5; // For culling reflections, hopefully
const int IR_THRESHOLD = 980; // About 0.15 after converting to 0..1 float looked about right, which would be ~870 raw.  In practice, with no IR ball present, we never see a raw value less than 1000
const int CALIBRATION_MODE_SWITCH_PIN = 2;
const int SAVE_HEADING_BUTTON_PIN = 7;
const int EEPROM_MIN_X = 0;
const int EEPROM_MIN_Y = 2;
const int EEPROM_MIN_Z = 4;
const int EEPROM_MAX_X = 6;
const int EEPROM_MAX_Y = 8;
const int EEPROM_MAX_Z = 10;
const int EEPROM_TARGET_HEADING = 12;


const int hall_effect = A9;
const int BUZZER = 10;
int ball_detected = 0;
float ball_angle = 0;
float ball_distance;
int front_range = 0;
int back_range = 0; // so far unused
float angle_to_goal = 0;
int calibration_mode_switch = 0;
int light_sensor = 0;
int crotchet;

#if DEBUGGING ==1
	#define DEBUG(x) Serial.println (x)
	#define DEBUG_NOEOL(x) Serial.print (x)
#else
	#define DEBUG(x)
	#define DEBUG_NOEOL(x)
#endif



#include "nokia.h"
#include "mk_shortened.h"
#include "sherlockShort.h"
#include "magnetometer.h"
#include "ultrasonic.h"
#include "reflectance.h"

// int ir_val;
const float TAU = 2 * PI;

// To write an int to EEPROM requires two write() calls, since write() works only in bytes and an int is two.
// Not sure whether to have address be a specific byte address or to multiply by sizeof(int) within the functions to create a virtual addressing scheme for int values only.
// Probably safer to leave as raw byte addresses in case we have to mix and match, but watch for accidental overwriting!

void EEPROM_write_int(int address, int value) {
	EEPROM.write(address, highByte(value));
	EEPROM.write(address + sizeof(byte), lowByte(value));
}

// Ditto for reading:

int EEPROM_read_int(int address) {
	/*
	byte highByte = EEPROM.read(address);
	byte  lowByte = EEPROM.read(address + sizeof(byte));
	return word(highByte, lowByte);
	*/
	// Or simply:
	return word(EEPROM.read(address), EEPROM.read(address + sizeof(byte)));
}

void setup() {
	for(int n=0; n<NUM_SENSORS; n++) {
		pinMode(analog_sensor_pins[n], INPUT);
	}
	pinMode(CALIBRATION_MODE_SWITCH_PIN, INPUT_PULLUP);
	pinMode(SAVE_HEADING_BUTTON_PIN, INPUT_PULLUP);
	pinMode(hall_effect, INPUT);
	pinMode(BUZZER,OUTPUT);
	Serial.begin(115200);
	Serial.println("Shutter Starting");
        InfraredSeeker::Initialize();
	magnetometerSetup();
	ultrasonic_setup();
}

void test_loop() {
	for(int n=0; n<=NUM_SENSORS; n++) {
		DEBUG_NOEOL(analogRead(analog_sensor_pins[n]) + " ");
	}
	DEBUG();
	delay(50);
}

void loopy() {
	get_ball_angle();
	delay(500);
}

void loop() {
	calibration_mode_switch = digitalRead(CALIBRATION_MODE_SWITCH_PIN);
	if (!calibration_mode_switch) {
		magnetometer_calibrateMagnetometer(); 
	}
	if (!digitalRead(SAVE_HEADING_BUTTON_PIN)) { // allows the heading to be saved during use
		magnetometer_saveHeading();
	}
        readReflectance();
	//readIRsensors();
	//printIRsensors();
	get_ball_angle();
	
	angle_to_goal = magnetometer_getAngleToTarget();
	
       
   ball_detected = 1;
         
        //Serial.println("Sending Output");
	send_output();
#if DEBUGGING == 1
	delay(2000);
#else 
	delay(20);
#endif
}

void beep() {
	tone(BUZZER, 3600);
	delay(100);
	noTone(BUZZER);
}

float frequency(int note) {
	return 440 * pow(2.0, (note - 69) / 12.0);
}

void playNote(int pitch, int duration) {
	tone(BUZZER, (int)round(frequency(pitch)), duration);	
	delay(duration+5);
	noTone(BUZZER);
	delay(5);
}

void setNoteDurations(int bpm) {
	crotchet = 60000 / bpm;
}

void playTune(float tune[][2], int tempo, int arrayLength) {
	setNoteDurations(tempo);
	for(int x=0; x<arrayLength; x+=1) {
		if(tune[x][0] == 0) {
			delay(tune[x][1]*crotchet);
		} else {
			playNote(tune[x][0], tune[x][1]*crotchet);
		}
	}
}


void printIRsensors() { // used for debugging
	DEBUG_NOEOL("Raw sensor readings: ");
	for (int i=0; i<NUM_SENSORS; i++) {
		DEBUG_NOEOL(ir_values[i]);
		DEBUG_NOEOL(" ");
	} 
	DEBUG();
}

void send_output() {
	Serial.print(ball_detected);Serial.print(" ");
	Serial.print(ball_angle);Serial.print(" ");
	Serial.print(ball_distance);Serial.print(" ");
	Serial.print(analogRead(hall_effect));Serial.print(" ");
	Serial.print(back_range);Serial.print(" ");
	Serial.print(angle_to_goal);Serial.print(" ");
	Serial.print(calibration_mode_switch);Serial.print(" ");
	Serial.print(reflectance);Serial.print(" ");
	Serial.println(";");
}

int posneg(int input) {
  if(input > 180) {
    return(input - 360); 
  }
  return input;
}

float get_ball_angle() {
	DEBUG("ball angle called!");
	int small=2000;
    int sens=0;
    for(int x=0; x<NUM_SENSORS; x++) {
		
		//Serial.print(analogRead(analog_sensor_pins[x]));
		//Serial.print(" ");
		
		if(analogRead(analog_sensor_pins[x]) < small) {
			small=analogRead(analog_sensor_pins[x]);
			sens=x;
		}
     }

     if(small>900) {
		 ball_detected = 0;
                 ball_angle = 999;
	 } else {		 
                ball_detected = 1;
                 ball_angle = sens*45;
                 ball_angle = posneg(ball_angle);
                 
                 InfraredResult InfraredBall = InfraredSeeker::ReadAC();
                 ball_distance = InfraredBall.Strength;
                   //ball_angle = ir_sensor_angles[InfraredBall.Direction];
                 
	 }
     
     
    
    //Serial.print(ball_angle);
    //Serial.println("");
}

<file_sep>/Tools/Arduiglue/Makefile
# BIN=arduiglue-arduino-folder-setup

install:
	cp -v arduiglue-arduino-folder-setup /usr/local/bin
	cp -v arduiglue-arturo-folder-setup /usr/local/bin

<file_sep>/Projects/2017/SoccerBots/picamera-test.py
#!/usr/bin/env python
from time import sleep
from picamera import PiCamera

print("Starting camera...")
# Recommended parameters for initialisation: sensor_mode, resolution, framerate, framerate_range, and clock_mode
camera = PiCamera(resolution=(1640,922), framerate=25)
# Pi Camera v2 Modes:
# #2 is 3280 x 2464, full FoV, 4:3
# #4 is 1640 x 1232, full FoV, 4:3
# #5 is 1640 x 922, full H FoV, 16:9
#camera.resolution = (1640, 922)
camera.iso = 400
#camera.start_preview()
# Pause briefly to allow automatic settings to settle:

# NOTE:
# <<
# Exposure mode 'off' is special: this disables the camera's automatic gain control, fixing the values of digital_gain and analog_gain.
# Please note that these properties are not directly settable (although they can be influenced by setting iso prior to fixing the gains), and default to low values when the camera is first initialized. Therefore it is important to let them settle on higher values before disabling automatic gain control otherwise all frames captured will appear black.
# >>

print("camera.exposure_speed = ",camera.exposure_speed)
# Is this measured in microseconds?
# 1 / (28649 microseconds) is about 35 Hz..?!
#camera.shutter_speed = camera.exposure_speed
# Can't set exposure_speed manually? Only shutter_speed?

print("setting shutter_speed...")
#camera.exposure_speed = 100000
# 1 / (20 Hz) = 50000 microseconds
#camera.shutter_speed = 50000
# 1 / (25 Hz) = 40000 microseconds
#camera.shutter_speed = 40000
# 1 / (50 Hz) = 20000 microseconds
camera.shutter_speed = 20000
# 1 / (100 Hz) = 10000 microseconds
#camera.shutter_speed = 10000

# Give analog and digital gains time to adjust...
print("recalibrating...")
sleep(2)
camera.exposure_mode = 'off'
#print("camera.analog_gain = ",camera.analog_gain)
# ~7.5 indoors under fluorescent lighting
#camera.analog_gain = 8
#camera.analog_gain = Fraction(241, 32)

print("Hold a white card in front of the camera now!")
sleep(5)

g = camera.awb_gains
print("camera.awb_gains = ",g)
camera.awb_mode = 'off'
camera.awb_gains = g

#camera.rotation = 180

# TODO: fix focus settings? Ah, this requires a hardware adjustment! The cameras are set to focus at infinity from the factory, but I've 3D printed a couple of wrenches for adjusting it.

print("You can take away the white card now. :)")

#camera.capture('picamera-test.jpg', resize=(820,461))
# .resize()
camera.start_preview(alpha=127)
# Can we use resize with other capture functions as well? Seems so..also with start_recording().
#camera.capture_sequence(['image%02d.jpg' % i for i in range(99)], resize=(820,461))
# What's the best way to capture for sharing with SimpleCV or OpenCV? A shell pipeline? A socket? A NumPy array?
sleep(3600)

# Cature to a video file:
# Note that the video file is raw h.264, with no frame rate information!
#camera.start_recording('my_video.h264')
#camera.wait_recording(60)
#camera.stop_recording()
<file_sep>/Tools/Arduiglue/arduiglue-arturo-folder-setup
#!/bin/sh

# Take an existing Arduino IDE compatible project and set it up for use also with Arturo/ano.
# Command-line argument is expected to be the path to the directory containing an existing Arduino IDE project.
# e.g.
# arduiglue-arturo-folder-setup ~/Documents/Arduino/Blink
# Or, pass no argument and have it work on the current directory.

# We create an "ano" folder within that folder to satisfy Arturo's `ano init` requirement for an empty folder.
# We then replace the default ano sketch with a symlink to the existing Arduino sketch file.

# TODO:
# [ ] Make safe for with space-containing filenames/paths.
# [ ] Optionally file everything away in the default sketchbook folder?

SKETCH_PATH=${1:-$(pwd)}
SKETCH_NAME=$(basename $SKETCH_PATH)
#SKETCH_NAME="$(basename $(dirname $SKETCH_PATH))"

cd $SKETCH_PATH
mkdir -v ano
cd ano 
ano init
cd src
rm -v sketch.ino 
ln -s ../../$SKETCH_NAME.ino sketch.ino
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_whistlebot/firmware_whistlebot.ino
// WhistleBot code for the 2016 MIDIBots
// Team _underscore_, Information Science Mechatronics, University of Otago

// Define pin mappings for the MIDIBot shield:
#include <Servo.h> 
#include <MIDIBot.h>
MIDIBot whistleBot;

Servo valve;
Servo slide;

int pos = 0;    // variable to store the servo position

const int VALVE_MIN = 92; 
const int VALVE_MAX = 105;

const int
	VALVE_0 = 92,	// OK
	VALVE_1 = 93,	//
	VALVE_2 = 95,	// 
	VALVE_3 = 97,	// 
	VALVE_4 = 98,	//
	VALVE_5 = 100,	// 
	VALVE_6 = 101,	// OK
	VALVE_7 = 103,	// Maybe 102 - sometimes hits the next string
	VALVE_8 = 104,	// 
	VALVE_9 = 105;	// OK

// ..and the slide.
// For slide max/min, we probably want min to be at the nut end, and max at the soundhole end.
const int SLIDE_MIN = 29; //29
const int SLIDE_MAX = 83; //83

const int
	SLIDE_0 = 29,
	SLIDE_1 = 37,
	SLIDE_2 = 45,
	SLIDE_3 = 52,
	SLIDE_4 = 59,
	SLIDE_5 = 65,
	SLIDE_6 = 71,
	SLIDE_7 = 77,
	SLIDE_8 = 83;

// MIDI note numbers for valveming...
const int
	VALVE_0_NOTE = 0,
	VALVE_1_NOTE = 1,
	VALVE_2_NOTE = 2,
	VALVE_3_NOTE = 3,
	VALVE_4_NOTE = 4,
	VALVE_5_NOTE = 5,
	VALVE_6_NOTE = 6,
	VALVE_7_NOTE = 7,
	VALVE_8_NOTE = 8,
	VALVE_9_NOTE = 9;

// ...and picking:
const int
	SLIDE_0_NOTE = 12,
	SLIDE_1_NOTE = 13,
	SLIDE_2_NOTE = 14,
	SLIDE_3_NOTE = 15,
	SLIDE_4_NOTE = 16,
	SLIDE_5_NOTE = 17,
	SLIDE_6_NOTE = 18,
	SLIDE_7_NOTE = 19,
	SLIDE_8_NOTE = 20;

// TODO: constants (or an array?) of servo positions that correspond to musical notes.

// Keep track of last slide position set, in case we end up getting really fancy with the logic and trying to optimise the timings or something.
int slide_position = SLIDE_MIN;

// Unused?
const int VALVE_DELAY = 100;

const int SLIDE_DELAY = 300;	// Worst-case slide motion time (HOME..MIN). Unconfirmed!


int i;

void move_slide(int pos) {
	// Clip motion to valid range:
	if (pos < SLIDE_MIN) {pos = SLIDE_MIN;}
	if (pos > SLIDE_MAX) {pos = SLIDE_MAX;}
	// Move it:
	slide.write(pos);
}

// TODO: similar to move_slide() but for picking, maybe?

void note_on(int note, int velocity) {
	switch (note) {
		case VALVE_0_NOTE:	valve.write(VALVE_0); break;
		case VALVE_1_NOTE:	valve.write(VALVE_1); break;
		case VALVE_2_NOTE:	valve.write(VALVE_2); break;
		case VALVE_3_NOTE:	valve.write(VALVE_3); break;
		case VALVE_4_NOTE:	valve.write(VALVE_4); break;
		case VALVE_5_NOTE:	valve.write(VALVE_5); break;
		case VALVE_6_NOTE:	valve.write(VALVE_6); break;
		case VALVE_7_NOTE:	valve.write(VALVE_7); break;
		case VALVE_8_NOTE:	valve.write(VALVE_8); break;
		case VALVE_9_NOTE:	valve.write(VALVE_9); break;

		case SLIDE_0_NOTE:	move_slide(SLIDE_0); break;
		case SLIDE_1_NOTE:	move_slide(SLIDE_1); break;
		case SLIDE_2_NOTE:	move_slide(SLIDE_2); break;
		case SLIDE_3_NOTE:	move_slide(SLIDE_3); break;
		case SLIDE_4_NOTE:	move_slide(SLIDE_4); break;
		case SLIDE_5_NOTE:	move_slide(SLIDE_5); break;
		case SLIDE_6_NOTE:	move_slide(SLIDE_6); break;
		case SLIDE_7_NOTE:	move_slide(SLIDE_7); break;
		case SLIDE_8_NOTE:	move_slide(SLIDE_8); break;		
	}
}

void note_off(int note, int velocity) {}


void setup()
{
//	UkeBot.begin();
	Serial.begin(9600);
	// Servo setup:
	valve.attach(SERVO_3_PIN);
	slide.attach(SERVO_2_PIN);
	
	// Set initial position
	valve.write(VALVE_MIN);
	slide.write(126);
}

void self_test_picking() {
	valve.write(VALVE_1); delay(500);
	valve.write(VALVE_0); delay(500);
	valve.write(VALVE_3); delay(500);
	valve.write(VALVE_2); delay(500);
	valve.write(VALVE_5); delay(500);
	valve.write(VALVE_4); delay(500);
	valve.write(VALVE_7); delay(500);
	valve.write(VALVE_6); delay(500);
	valve.write(VALVE_9); delay(500);
	valve.write(VALVE_8); delay(500);

	valve.write(VALVE_8); delay(500);
	valve.write(VALVE_9); delay(500);
	valve.write(VALVE_6); delay(500);
	valve.write(VALVE_7); delay(500);
	valve.write(VALVE_4); delay(500);
	valve.write(VALVE_5); delay(500);
	valve.write(VALVE_2); delay(500);
	valve.write(VALVE_3); delay(500);
	valve.write(VALVE_0); delay(500);
	valve.write(VALVE_1); delay(500);
	
	valve.write(VALVE_0); delay(500);	// Return home
}

void self_test() {
	// TODO: test each semitone, not just min/max slide position. This would be easier if we used an array for the slide positions.
	digitalWrite(LED_PIN, HIGH);
	move_slide(SLIDE_MIN); delay(SLIDE_DELAY);
	self_test_picking();
	move_slide(SLIDE_MAX); delay(SLIDE_DELAY);
	self_test_picking();
	move_slide(SLIDE_MIN); delay(SLIDE_DELAY);
	digitalWrite(LED_PIN, LOW);
}

int angle = 126;

void angle_increase() {
	angle++;
	if (angle > 180) {angle = 180;}
	slide.write(angle);
}

void angle_decrease() {
	angle--;
	if (angle < 0) {angle = 0;}
	slide.write(angle);
	
}

int control_byte;

void loop()
{
	if (Serial.available()) {
		control_byte = Serial.read();
		// TODO: error-checking?
		switch (control_byte) {
			case '+': angle_increase(); break;
			case '-': angle_decrease(); break;
			case ' ': slide.write(180); break;
		}
		Serial.println(angle);
	}
}

/*
void loop()
{
	UkeBot.process_MIDI();
}
*/
<file_sep>/Projects/2016/MIDIBots/arduino/midi_receive/midi_receive.ino
/*
  Blink
  Turns on an LED on for one second, then off for one second, repeatedly.
 
  This example code is in the public domain.
 */

#define GREEN 8
#define RED   10
 
// Pin 13 has an LED connected on most Arduino boards.
// give it a name:
// const int greenLed = 8,
//          redLed   = 10;

int dataByte[2], statusByte = 0, i = 0;

// the setup routine runs once when you press reset:
void setup() {                
  Serial.begin(31250);
  // initialize the digital pin as an output.
  pinMode(GREEN, OUTPUT);
  pinMode(RED, OUTPUT);     
  clearData();
}

void clearData(){
  dataByte[0] = 0;
  dataByte[1] = 0;
  i = 0;
}

void flash(int color){
  digitalWrite(color, HIGH);   // turn the LED on (HIGH is the voltage level)
  delay(10);               // wait for a second
  digitalWrite(color, LOW);    // turn the LED off by making the voltage LOW
  delay(20);               // wait for a second
}

// the loop routine runs over and over again forever:
void loop() {
  if(Serial.available() > 0){
    int data = Serial.read();
    if(data > 127){
      statusByte = data;
      clearData();  
    }else{
      dataByte[i] = data;
      if(statusByte == 0x90 && i == 1){
        if(dataByte[1] == 0){
          flash(RED);
        }else{
          flash(GREEN);
        }
      }
      else if (statusByte == 0x80 && i == 1){
        flash(RED);
      }
      i++; 
    }
  }
}
<file_sep>/Projects/2016/MIDIBots/arduino/libraries/MIDIBot/Makefile
# TODO: make platform-aware:
# see http://stackoverflow.com/questions/714100/os-detecting-makefile

# Mac OS:
#SKETCHBOOK=~/Documents/Arduino/

# Linux (recent Arduino versions):
SKETCHBOOK=~/Arduino
# (NOTE: earlier versions had ~/sketchbook)

TARGET=${SKETCHBOOK}/libraries/MIDIBot

install: MIDIBot.h MIDIBot.cpp
	mkdir -p ${TARGET}
	cp -v MIDIBot.h MIDIBot.cpp ${TARGET}
<file_sep>/Tools/Arduiglue/arduiglue-arduino-folder-setup
#!/bin/sh

# Take an existing unfiled Arduino sketch filename as an argument and set it up with an Arduino IDE compatible filesystem subtree.
# e.g.:
# arduiglue-arduino-folder-setup ~/Documents/Arduino/untitled.ino
# Arduino IDE expects the structure $SKETCHBOOK/$SKETCH/$SKETCH.ino
# Older sketches might have had the name $SKETCH.pde but let's not worry about that initially.
# Note also that Arduino IDE would normally normalise filenames (e.g. 's/ /_/g'). Maybe this should do that too.

# TODO:
# [ ] Handle paths containing spaces
# [ ] Normalise filename a la Arduino IDE rules.
# [ ] Error handling (e.g. sketch folder or file already exists, preflight check for existence of source file)

FILENAME="$1"
SKETCH=`basename $FILENAME .ino`
FOLDER=`dirname $FILENAME`

# TODO: determine proper Sketchbook path...
#cd ~/Documents/Arduino
# or just assume we're given the full path to the existing sketch file.
cd $FOLDER
mkdir -v $SKETCH
mv -v -n $SKETCH.ino $SKETCH/
<file_sep>/Projects/2017/SoccerBots/boris_python.py
import serial
import time
serinput = serial.Serial('/dev/serial/by-id/usb-Arduino__www.arduino.cc__0042_852313632363516031B2-if00', 115200)
seroutput = serial.Serial('/dev/serial/by-id/usb-DSpace__www.dspace.org.nz__DSpace_Robot_2.0_55234313335351A0D161-if00', 19200)
left_motor_string = 0b10000000 #first bit 1 means motor message
right_motor_string = 0b11000000 #second bit 1 means right motor
kicker_mask = 0b00000000 #first bit -  0 means kicker message
speed_threshold = 0.4 #deadzone threshold


ball_angle=0

def readsensors():
    line = str(serinput.readline()).strip(' ;\r\n ').split(" ")
    print(len(line))
    print(line)
    try:
        ball_detected=line[0]
        ball_angle=line[1]
        ball_distance=line[2]
        hall_effect=line[3]
        rangee=line[4]
        compass_heading=line[5]
        calibration_mode=line[6]
        colour=line[7]
    except(serial.SerialException, ValueError, IndexError) as error:
        time.sleep(1)


def motoroutput(left, right):
    x = left_motor(left).strip("0b")
    print(x)
    seroutput.write(str(x).encode())
    y = right_motor(right).strip("0b")
    print(y)
    seroutput.write(str(y).encode())
    

def deadzone(speed, threshold):
    if(speed > 0):
        out = speed + threshold
    elif(speed < 0):
        out = speed - threshold
    elif(speed==0):
		out = 0
    if(out<-1):
        return -1
    elif(out > 1):
        return 1
    else:
        return out

def left_motor(speed): # input -1 to +1
    speed = deadzone(speed, speed_threshold)
    if speed >= 0:
        dir_bit_string = 0b00100000
    else:
        dir_bit_string = 0b00000000
    speedbits = (int(abs(speed)*255))>>3
    return bin(dir_bit_string | speedbits | left_motor_string)

def right_motor(speed):
    speed = deadzone(speed, speed_threshold)
    if speed >= 0:
        dir_bit_string = 0b00100000
    else:
        dir_bit_string = 0b00000000
    speedbits = (int(abs(speed)*255))>>3
    return bin(dir_bit_string | speedbits | right_motor_string)

def kicker(kick):
    return bin(kicker_mask | kick)


while True:
    motoroutput(-1,-1)
    time.sleep(1)
    #motoroutput(1,1)
    #time.sleep(1)
    #readsensors()
    #print(ball_angle)
<file_sep>/Projects/2016/MIDIBots/arduino/arduino-timeslice-shell.c

//Timeslice global variables
unsigned long TimeSlice_TC1_Interval = 20;
unsigned long TimeSlice_TC2_Interval = 100;
unsigned long TimeSlice_TC3_Interval = 500;

unsigned long TimeSlice_TC1_Start_Time;
unsigned long TimeSlice_TC2_Start_Time;
unsigned long TimeSlice_TC3_Start_Time;



void loop()
{


//----------------------------------------------------------TimeSlice code-----------------------------------------------------------
while(1){
//-------------------
//Timeclass 1 20 ms
//-------------------
TimeSlice_TC1_Start_Time = millis();

// ---------------------------------------------------------User application code


// ---------------------------------------------------------End of user application code
// Toggle TC1 LED
toggle_LED(1);
// check timeout
if (((millis() - TimeSlice_TC1_Start_Time) > TimeSlice_TC1_Interval)) // 20 ms
{
while(1){
       over_run_Fault(1); // abort program and loop forever and flash Time class 1 LED
       }          
}
else
{
while (((millis() - TimeSlice_TC1_Start_Time) <= TimeSlice_TC1_Interval)){ // remove to save MIPS but it removes precise timeclass execution 
 }  
} 
 
//-------------------
//Timeclass 2 100 ms
//-------------------
TimeSlice_TC2_Start_Time = millis();
// ----------------------------------------------------------User application code

// ----------------------------------------------------------End of user application code
// Toggle TC2 LED
toggle_LED(2); 

if (((millis() - TimeSlice_TC2_Start_Time) > TimeSlice_TC2_Interval)) // 100 ms
{
while(1){
       over_run_Fault(2); // abort program and loop forever flash Time class 2 LED
       }        
}
else
{
while (((millis() - TimeSlice_TC2_Start_Time) <= TimeSlice_TC2_Interval)){  // remove to save MIPS but it removes precise timeclass execution 
  }  
}

//------------------
//Timeclass 3 500 ms
//------------------
TimeSlice_TC3_Start_Time = millis();
//------------------------------------------------------------------User application code


// -----------------------------------------------------------------End of user application code
// Toggle TC3 LED
toggle_LED(3);
if (((millis() - TimeSlice_TC3_Start_Time) > TimeSlice_TC3_Interval)) // 500 ms
{
while(1){
        over_run_Fault(3); // abort program loop forever flash Time class 3 LED
        }     
}
else
{  
while (((millis() - TimeSlice_TC3_Start_Time) <= TimeSlice_TC3_Interval)){  // remove to save MIPS but it removes precise timeclass execution 
 }    
}  
} // End of main while loop



}// end of main loop
<file_sep>/Projects/2017/SoccerBots/blob_tracking_cv2.py
#!/usr/bin/env python2

# Demo/test of the Python OpenCV bindings (wot I built from source) with the picamera (Python-MMAL) module.
# Based on http://www.pyimagesearch.com/2015/03/30/accessing-the-raspberry-pi-camera-with-opencv-and-python/

execfile("camera_setup.py")

# That's it for the camera setup.
average_colour=(0,0,0)
sleep(0.1)

from fractions import Fraction

calibration_file_name = "camera_calibration_state_cv2.py"
execfile(calibration_file_name)

camera.awb_mode = 'off'
camera.awb_gains = calibrated_white_balance

def debug(msg):
	#sys.stderr.write(str(msg) + "\n")
	pass

def send2pd(message):
	print(str(message) + ";")
	sys.stdout.flush()
	
def x_coordinate_to_angle(coord): 
	#return coord*35.543 #calibrated for LogiTech Camera.
	return coord*33.0 #calibrated for RPi Camera. 

then = 0
now = 0
end_time = 0

# For colour matching testing:
# Helper for converting a decimal <r,g,b> triple into an integer [b,g,r] array:
def bgr(r,g,b):
		return [int(round(b*255)), int(round(g*255)), int(round(r*255))]

blue_lo = bgr(0.1, 0.24, 0.4)
blue_hi = bgr(0.2, 0.4, 0.6)
lower = numpy.array(blue_lo, dtype="uint8")
upper = numpy.array(blue_hi, dtype="uint8")

# What about HSV, which worked pretty well for the original SimpleCV attempt?
# OpenCV docs: For HSV, Hue range is [0,179], Saturation range is [0,255] and Value range is [0,255]
# Goal blue H was about 102/180, S=190/255, V=50/255 (or 50/100?)
goal_hsv = (104.3136,251.977,89.199)
#goal_hsv = average_colour
#goal_hsv = (average_colour[0], 127, 127)
debug(average_colour[0])
debug(goal_hsv)

# Should the range be applied additively or multiplicatively?
	
	
def clip8bit(x):
	if isinstance(x, int):
		if x < 0:
			return 0
		elif x > 255:
			return 255
		else: 
			return x
	elif isinstance(x, list):
		for y in range(len(x)):
			if x[y] < 0:
				x[y] = 0
			elif x[y] > 255:
				x[y] = 255
			else: 
				x[y] = x[y]
		return x
	

#goal_hsv_lo = numpy.array(lower(goal_hsv, goal_hsv_tolerance), dtype="uint8")
#goal_hsv_hi = numpy.array(upper(goal_hsv, goal_hsv_tolerance), dtype="uint8")
calibration = calibrated_goal_colour
tolerance = 20
debug(calibration)
for x in range(3):
	calibration[0][x] = calibration[0][x] - tolerance
	calibration[1][x] = calibration[1][x] + tolerance
debug(calibration)

clipped_cal = (clip8bit(calibration[0]),clip8bit(calibration[1]))

debug(clipped_cal)

goal_hsv_lo = numpy.array(clipped_cal[0], dtype="uint8")
goal_hsv_hi = numpy.array(clipped_cal[1], dtype="uint8")
debug(goal_hsv_lo)
debug(goal_hsv_hi)


debug("Press 'q' to quit...")
# , resize=resized_res


for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
	rawCapture.truncate(0)
	now = time()
	elapsed_time = now  - then
	debug('Frame rate: ' + str(round(1.0/elapsed_time,1)) + ' Hz\n')

	# grab the raw NumPy array representing the image, then initialize the timestamp
	# and occupied/unoccupied text
	#print("copy array")
	image = frame.array

	hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
	# Remember, it's (B,G,R) otherwise!	
	
	mask = cv2.inRange(hsv, goal_hsv_lo, goal_hsv_hi)
	#output = cv2.bitwise_and(image, image, mask=mask)

	# show the matching area:
	#print("show masked frame")
	# Detect blobs.
	keypoints = detector.detect(mask)
	
	for keypoint in keypoints:
		debug(keypoint)
		debug("Size: " + str(keypoint.size))
		debug("Co-ordinates: " + str(keypoint.pt))
	debug("Number of blobs " + str(len(keypoints)))
	

	# Draw detected blobs as red circles.
	# cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures the size of the circle corresponds to the size of blob
	mask_with_keypoints = cv2.drawKeypoints(mask, keypoints, numpy.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
	
	# Show keypoints
	#cv2.imshow("Keypoints", mask_with_keypoints)
	# Show hsv image
	#cv2.imshow("HSV", hsv)
	# Show raw image
	#cv2.imshow("Raw Image", image)
	
	blob_size = 0
	largest_blob_index = 0
	n=0
	
	if len(keypoints) == 0:
		debug("No blobs detected!")
		key = cv2.waitKey(1) & 0xFF
		if key == ord("q"):
			break
		send2pd(-180)
		continue
		
	for keypoint in keypoints:
		if keypoint.size > blob_size:
			blob_size = keypoint.size
			largest_blob_index = n
		n = n+1
		
	largest_blob = keypoints[largest_blob_index]
	debug(largest_blob_index)
	
	if numpy.isnan(largest_blob.pt[0]) | numpy.isnan(largest_blob.pt[1]):
		debug("Not a Number")
		send2pd(-180)
		continue

	
	#Show largest blob
	
	mask_with_largest_blob = cv2.drawKeypoints(mask, [largest_blob], numpy.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
	
	blob_x = largest_blob.pt[0]
	blob_y = largest_blob.pt[1]
	debug(str(blob_x) + str(blob_y))
	
	cv2.line(mask_with_largest_blob,(int(blob_x),0),(int(blob_x),96),(255,0,0),2)	
	cv2.line(mask_with_largest_blob,(0,int(blob_y))	,(128,int(blob_y)),(0,255,0),2)
	
	cv2.imshow("Largest Blob", mask_with_largest_blob)
	
	
	normalised_x = 2.0/capture_res[0] * blob_x - 1
	send2pd(str(x_coordinate_to_angle(normalised_x)) + " " + str(largest_blob.size))
	
	#print(" wait >")
	key = cv2.waitKey(16) & 0xFF
	# if the `q` key was pressed, break from the loop
	if key == ord("q"):
		break
	
	# clear the stream in preparation for the next frame
	#print("truncating...")
	
 
	then = now
	
	#print("end loop")
	#end_time = time()


<file_sep>/Projects/2017/SoccerBots/Makefile
install: bash_aliases
	ln -f -s ~/robotics_repo/Projects/2017/SoccerBots/bash_aliases ~/.bash_aliases
	echo "Start a new shell or run source for new aliases to take affect."
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_lightingbot/firmware_lightingbot.ino
// ____Bot code for the 2016 MIDIBots
// Team _underscore_, Information Science Mechatronics, University of Otago

#include <MIDIBot.h>
MIDIBot thisMIDIBot;

// Additional constants for the lighting setup:
const int BLUE_PIN = MOSFET_4_PIN;
const int WHITE_PIN = MOSFET_3_PIN;

const int BLUE_NOTE = 60; // MIDI middle C
const int WHITE_NOTE = 62; // MIDI D above middle C

// RGB LED strip setup:
const int R_PIN = SERVO_1_PIN; // red
const int G_PIN = SERVO_2_PIN; // green
const int B_PIN = SERVO_3_PIN; // blue

// RGB LED strip control notes:
const int R_NOTE = 67;	// G above middle C
const int G_NOTE = 69;	// A above middle C
const int B_NOTE = 71;  // B above middle C


// TODO: refactor to use switch statements in note_on(), note_off():
// (see PercussionBot firmware for example)

void note_on(int note, int velocity) {
	if (note ==  BLUE_NOTE) {digitalWrite( BLUE_PIN, HIGH);}
	if (note == WHITE_NOTE) {digitalWrite(WHITE_PIN, HIGH);}
	if (note == R_NOTE) {analogWrite(R_PIN, velocity*2);}
	if (note == G_NOTE) {analogWrite(G_PIN, velocity*2);}
	if (note == B_NOTE) {analogWrite(B_PIN, velocity*2);}
}

void note_off(int note, int velocity) {
	if (note ==  BLUE_NOTE) {digitalWrite( BLUE_PIN, LOW);}
	if (note == WHITE_NOTE) {digitalWrite(WHITE_PIN, LOW);}
	if (note == R_NOTE) {analogWrite(R_PIN, 0);}
	if (note == G_NOTE) {analogWrite(G_PIN, 0);}
	if (note == B_NOTE) {analogWrite(B_PIN, 0);}
}

void self_test() {
	digitalWrite(BLUE_PIN, HIGH);
	digitalWrite(WHITE_PIN, LOW);
	delay(500);
	digitalWrite(BLUE_PIN, LOW);
	digitalWrite(WHITE_PIN, HIGH);
	delay(500);
	digitalWrite(WHITE_PIN, LOW);

	RGB_colour_test();	
	RGB_fade_integer();
}

void setup() {
	thisMIDIBot.begin();
}

void loop() {
	thisMIDIBot.process_MIDI();
}


const float EXP = 2.0; // Stevens power-law brightness exponent

// Human perception of brightness is non-linear. This function attempts to compensate and provide linear control over perceived brightness.  Input and output are (assumed to be) normalised to the range 0..1.
float brightness(float level) {
	// Exponential (to compensate for logarithmic response):
	// TODO: ...
	// Stevens (power-law) model:
	return pow(level, EXP);	// Min. step size = (1/255)^(1/exponent)
}

// Compute minimum step size to achive full output resolution with 0..255 PWM (STEP computed WRT 0..1 range):
//const float STEP = pow(1.0 / 255.0, 1.0 / EXP); // Stevens
const float STEP = 0.0001;

// Plus another function to quantise to the 0..255 levels provided by the PWM output:
int quantise(float value) {
	return (int) round(value * 255.0);
}

void RGB_blink() {
	digitalWrite(R_PIN, HIGH);
	digitalWrite(G_PIN, LOW);
	digitalWrite(B_PIN, LOW);
	delay(150);

	digitalWrite(R_PIN, LOW);
	digitalWrite(G_PIN, HIGH);
	digitalWrite(B_PIN, LOW);
	delay(150);

	digitalWrite(R_PIN, LOW);
	digitalWrite(G_PIN, LOW);
	digitalWrite(B_PIN, HIGH);
	delay(150);

	digitalWrite(R_PIN, LOW);
	digitalWrite(G_PIN, LOW);
	digitalWrite(B_PIN, LOW);
//	delay(1500);
}

long r, g, b;
const int FADE_DELAY = 1;

void RGB_fade_integer() {
	digitalWrite(R_PIN, LOW);
	digitalWrite(G_PIN, LOW);
	digitalWrite(B_PIN, LOW);
	
	// Fade in:
	for (r = 0; r < 256; r++) {
		analogWrite(R_PIN, r);
		delay(FADE_DELAY);
	}
	for (g = 0; g < 256; g++) {
		analogWrite(G_PIN, g);
		delay(FADE_DELAY);
	}
	for (b = 0; b < 256; b++) {
		analogWrite(B_PIN, b);
		delay(FADE_DELAY);
	}

	// Fade out:
	for (r = 255; r >= 0; r--) {
		analogWrite(R_PIN, r);
		delay(FADE_DELAY);
	}
	for (g = 255; g >= 0; g--) {
		analogWrite(G_PIN, g);
		delay(FADE_DELAY);
	}
	for (b = 255; b >= 0; b--) {
		analogWrite(B_PIN, b);
		delay(FADE_DELAY);
	}
}

// TODO: finish testing this and complete it:
void RGB_fade() {
	digitalWrite(R_PIN, LOW);
	digitalWrite(G_PIN, LOW);
	digitalWrite(B_PIN, LOW);
	
	// Fade in:
//	for (r = 0; r < 256; r++) {
//		analogWrite(R_PIN, quantise(brightness(r));
//		delay(FADE_DELAY);
//	}
	for (g = 0; g < (long) round(1.0/STEP); g++) {
		analogWrite(G_PIN, quantise(brightness(g * STEP)));
	//	delay(FADE_DELAY);
	}
//	for (b = 0; b < 256; b++) {
//		analogWrite(B_PIN, b);
//		delay(FADE_DELAY);
//	}

	// Fade out:
//	for (r = 255; r >= 0; r--) {
//		analogWrite(R_PIN, r);
//		delay(FADE_DELAY);
//	}
	for (g = (long) round(1.0/STEP); g >= 0; g--) {
		analogWrite(G_PIN, quantise(brightness(g * STEP)));
	//	delay(FADE_DELAY);
	}
//	for (b = 255; b >= 0; b--) {
//		analogWrite(B_PIN, b);
//		delay(FADE_DELAY);
//	}
}

void RGB_colour_test() {
	const int DELAY = 500;
	RGB_write(255,   0,   0); delay(DELAY);	// red
	RGB_write(  0, 255,   0); delay(DELAY);	// green
	RGB_write(  0,   0, 255); delay(DELAY);	// blue
	RGB_write(  0, 255, 255); delay(DELAY);	// cyan
	RGB_write(255,   0, 255); delay(DELAY);	// magenta
	RGB_write(255, 255,   0); delay(DELAY);	// yellow
	RGB_write(255, 255, 255); delay(DELAY);	// white
	delay(DELAY);
}

void RGB_write(int R, int G, int B) {
	analogWrite(R_PIN, R);
	analogWrite(G_PIN, G);
	analogWrite(B_PIN, B);
}

<file_sep>/Projects/2016/MIDIBots/arduino/uke_test/uke_test.ino
// Testing servo strumming ukulele

#include <Servo.h> 

Servo servos [1];
int pos = 0;    // variable to store the servo position
const int CENTRE = 90;
const int RANGE = 20;
const int MAX_POS = CENTRE + RANGE/2;
const int MIN_POS = CENTRE - RANGE/2;

const int SERVO_TIME = 500; // ms delay to allow motion to happen


void setup() 
{
	servos[0].attach(11);
} 
 
void strum(Servo servo)
{
	servo.write(MAX_POS);
	delay(SERVO_TIME);
	servo.write(MIN_POS);
	delay(SERVO_TIME);
}

void loop() {
	strum(servos[0]);
}
<file_sep>/Projects/2016/MIDIBots/arduino/servo_drum_test/ano/src/sketch.ino
../../servo_drum_test.ino<file_sep>/Projects/2016/MIDIBots/arduino/firmware_drumbot/ano/src/sketch.ino
../../firmware_drumbot.ino<file_sep>/Projects/2017/SoccerBots/trigonometry/atan2.c
// Demonstrate/test the use of the two-argument atan2() function

// The atan2(x,y) function has a couple of advantages over conventional atan(), namely it avoids the division-by-zero problem, and returns values that correctly indicate the quadrant of the angle, based on the sign* of the inputs.
// * not the sin()!

#include <math.h>
#include <stdio.h>

const double TAU = asin(1) * 4.0;

double degrees(double radians) {return 360.0 / TAU * radians;}

int main() {
	for (int x = -1; x < 2; x++) {
		for (int y = -1; y < 2; y++) {
			printf("atan2(%i,%i) = %f\n", x, y, degrees(atan2(x,y)));
		}
	}
	return 0;
}
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_lightingbot/ano/src/sketch.ino
../../firmware_lightingbot.ino<file_sep>/Settings/Geany/Readme.txt
(Better) Arduino integration in Geany
CME 2016-07-18

Geany seems to have two kinds of file type handling:
 - Built-in filetypes
 - Custom filetypes

Custom filetypes are simpler to define, but less powerful.

Built-in filetypes 
 

Tags: I think these are things like function names that Geany can index for ease of navigation using the Symbols panel.

tag_parser: e.g. "C"

build_settings, build-menu sections 

http://www.geany.org/manual/dev/index.html#custom-filetypes

http://www.geany.org/manual/dev/index.html#build-menu-configuration

http://geany.org/manual/dev/#symbols-and-tags-files

--

Files to perhaps edit:

~/.config/geany/filetype_extensions.conf

	Add Arduino file type to the Programming group.

<<
...
[Extensions]
# <CME>
Arduino=*.ino;*.pde;
# </CME>
...
[Groups]
# <CME>
Programming=Arduino;Clojure;CUDA;Cython;Genie;Scala;
# </CME>
...
>>

Alternatively, could we just add .ino to the list of C++ suffixes in filetype_extensions.conf?

	Yes, but then it just inherits the C++ build/toolchain commands, which we don't want.

Create a new:
filedefs/filetypes.ino
	NOTE: can include a build-menu section (see above)

Create a new:
templates/files/sketch.ino

MIME type:
text/x-arduino


Files added (to be installed):
~/.config/geany/filetype_extensions.conf
~/.config/geany/filedefs/filetypes.Arduino.conf
~/.config/geany/templates/files/sketch.ino

Hmm, it's reading the file_type_extensions.conf file (cos if I add *.pde to the C++ list it gets set as that type), but it's ignoring my Arduino definition line from the same.

<<
sbis4114:geany admin$ /Applications/Geany.app/Contents/MacOS/geany -v 
Geany-INFO: Geany 1.28, en_US.UTF-8
Geany-INFO: GTK 2.24.30, GLib 2.48.0
Geany-INFO: System data dir: /Applications/Geany.app/Contents/Resources/share/geany
Geany-INFO: User config dir: /Users/admin/.config/geany
Geany-INFO: System plugin path: /Applications/Geany.app/Contents/Resources/lib/geany
Geany-INFO: Added filetype Clojure (61).
Geany-INFO: Added filetype CUDA (62).
Geany-INFO: Added filetype Cython (63).
Geany-INFO: Added filetype Genie (64).
Geany-INFO: Added filetype Graphviz (65).
Geany-INFO: Added filetype JSON (66).
Geany-INFO: Added filetype Scala (67).
Geany-INFO: Could not find filetype 'Arduino'.
Geany-INFO: Filetype 'Arduino' not found for group 'Programming'!
Geany-INFO: /Users/admin/.config/geany/filetype_extensions.conf : Conf (UTF-8)
>>

Ah, it needs to be called filetypes.Arduino.conf

[ ] Problem: New file should be called sketch.ino, but it's getting named as untitled.ino
[ ] Symbols (function names, constants, etc.) are not being picked up, even when using C as the file type.  Ah, they don't get scanned until you save the file!

--

Geany/ino/Arduino on Mac OS X

Sebastian discovered that ino doesn't work with the latest version of the Ardunio IDE installation, on which it depends.

lsusb equivalent on Mac OS X:

https://github.com/jlhonora/lsusb

ioreg -p IOUSB -l -w 0 -x -c IOUSBDevice -r

	-p -> I/O kit plane (dev type)
	-l -> list properties (including handy attributes such as idVendor, idProduct, "USB Vendor Name", "USB Product Name", bDeviceClass, bDeviceSubClass
	-w 0 -> no text wrapping
	-x -> show numbers as hexadecimal
	-a -> archive format (relatively nasty XML output)
	-c IOUSBDevice -> limit to IOUSBDevice classes (and subclasses)
	-r -> root subtrees according to filter criteria

Ugh, will still have to parse a lump of text to get the useful data out...

system_profiler SPUSBDataType

Would be preferable not to require installation of additional scripts/programs/packages.


/dev/cu.usbmodem1a21
/dev/tty.usbmodem1a21
USBDroid	20A0:4150


Tcl OS detection:

if {$tcl_platform(os) eq "Darwin"} {...}


# New toplevel for .boards? Or just use the main "." frame?

toplevel .boards
wm title .boards {Available Arduino Devices}


label .boards.label -text {Choose the device to program:}
pack .boards.label -expand 0 -fill x

# NOTE: column #0 is always the tree column (even if not shown).
ttk::treeview .boards.tree -columns {<tree> Device USB_ID Model Type} -selectmode browse -show headings
pack .boards.tree -expand 1 -fill both
.boards.tree heading #0 -anchor w -text <tree>
.boards.tree heading #1 -anchor w -text Device:
.boards.tree heading #2 -anchor w -text {USB ID:}
.boards.tree heading #3 -anchor w -text {Model ID:}
.boards.tree heading #4 -anchor w -text {Device Type:}

# To insert a row:
.boards.tree insert {} end -text the_text -values [list dev id model type]

# A more realistic one:
.boards.tree insert {} end -text NoMatter -values [list /dev/ttyACM0 dead:beef uno {Arduino Uno}]

# TODO: column widths

# TODO: scrolling

# Buttons
button .boards.cancel_button -text Cancel -command {}
button .boards.ok_button -text OK -command {}

pack .boards.ok_button .boards.cancel_button -side right


<file_sep>/Projects/2016/MIDIBots/arduino/midi_receive/ano/src/sketch.ino
../../midi_receive.ino<file_sep>/Documentation/Computer Vision/infoscimechatronics/camera_calibration_state.py
#!/usr/bin/env python2.7
import SimpleCV
import sys
import os
sys.path.append(os.environ['HOME'] + '/robotics_repo/Documentation/Computer Vision/infoscimechatronics')
import cvutils

#ser = serial.Serial('/dev/serial/by-id/usb-www.freetronics.com_Eleven_64935343233351909241-if00')

#camera = SimpleCV.Camera(0, {"width":320,"height":240})
#os.system(os.environ['HOME'] + '/robotics_repo/Projects/2017/SoccerBots/uvcdynctrl-settings.tcl')

#lab_grey_sample = cvutils.calibrate_white_balance(camera)
calibrated_grey_sample = (212.8181818181818, 205.9090909090909, 205.0909090909091)
#lab_goal_blue = cvutils.calibrate_colour_match(camera, lab_grey_sample)
calibrated_goal_sample = ((104.5, 0.5), (170.0, 31.0), (83.0, 37.0))



def col_grey():
	return calibrated_grey_sample
	
def col_blue():
	return calibrated_goal_sample
<file_sep>/Projects/2016/MIDIBots/kicad/make-package.sh
#!/bin/sh

# Put the Gerber files together for sending to DirtyPCBs...

BASENAME=MIDI_Shield

# ls $BASENAME*{.gto,.gts,.gtl,.gbl,.gbs,.gbo,.gml,.txt}

# Fix up file names:

mv ${BASENAME}-F.Cu.gtl ${BASENAME}.gtl
mv ${BASENAME}-B.Cu.gbl ${BASENAME}.gbl
mv ${BASENAME}-B.SilkS.gbo ${BASENAME}.gbo
mv ${BASENAME}-F.SilkS.gto ${BASENAME}.gto
mv ${BASENAME}-B.Mask.gbs ${BASENAME}.gbs
mv ${BASENAME}-F.Mask.gts ${BASENAME}.gts
mv ${BASENAME}-Edge.Cuts.gm1 ${BASENAME}.gml

# ...and the drill file:

mv ${BASENAME}.drl ${BASENAME}.txt

# Now archive them up:

#zip $BASENAME.zip $BASENAME*{.gto,.gts,.gtl,.gbl,.gbs,.gbo,.gml,.txt}
# Nope, the additional filename elements mean that won't work. And maybe `zip` doesn`t play nicely with wildcards.
#zip $BASENAME.zip $BASENAME-F.SilkS.gto $BASENAME-F.Mask.gts $BASENAME-F.Cu.gtl $BASENAME-B.Cu.gbl $BASENAME-B.Mask.gbs $BASENAME-B.SilkS.gbo $BASENAME.gml $BASENAME.txt
zip $BASENAME.zip $BASENAME.gto $BASENAME.gts $BASENAME.gtl $BASENAME.gbl $BASENAME.gbs $BASENAME.gbo $BASENAME.gml $BASENAME.txt
<file_sep>/Projects/2017/SoccerBots/arduino/ir_test/ir_test.ino
// TODO: main statement of the purpose of this code!

#include <math.h>

const int IR_1 = 2;
const int IR_2 = 3;
const int IR_3 = 4;
const int IR_4 = 5;
const int NUM_SENSORS = 8;
//this is now finished,  go and look at ir_ball_tracker.ino instead
//const int sensor_pins[] = {2,3,4,5,6,7,8,9};
const int analog_sensor_pins[] = {A0,A1,A2,A3,A4,A5,A6,A7};
float ir_values[8];
float IR_COORDINATES[NUM_SENSORS][2] = {{0.0,1.0},{0.71,0.71},{1.0,0.0},{0.71,-0.71},{0.0,-1.0},{-0.71, -0.71},{-1.0, 0.0},{-0.71, 0.71}};
const int IR_THRESHOLD = 980; // About 0.15 after converting to 0..1 float looked about right, which would be ~870 raw.  In practice, with no IR ball present, we never see a raw value less than 1000.


#define DEBUGGING 0

/*
#define debug(message) \
	do { if (DEBUGGING) Serial.println(message); } while (0)
*/

#ifdef DEBUGGING
	#define DEBUG(x) Serial.println (x)
#else
	#define DEBUG(x)
#endif


int ir_val;
const float TAU = 2 * PI;

void setup() {
	for(int n=0; n<NUM_SENSORS; n++) {
		//pinMode(sensor_pins[n], INPUT);
                pinMode(analog_sensor_pins[n], INPUT);

	}
	Serial.begin(115200);
	//Serial.println("Welcome Uno");
}

void test_loop() {
  for(int n=0; n<=NUM_SENSORS; n++) {
    Serial.print(analogRead(analog_sensor_pins[n]) + " ");
  }
  Serial.println();
  delay(50);
}


void loop() {
	readIRsensors();
	printIRsensors();
	ball_angle();
	delay(500);
}

float readIRsensor(int sensor_num){
	int reading = analogRead(analog_sensor_pins[sensor_num]);
	// TODO: debugging
	Serial.print("sensor "); Serial.print(sensor_num); Serial.print(": "); Serial.println(reading);
	if (reading>IR_THRESHOLD){
		return 0.0;
	}else{
		return (1023-reading)/1023.0;
	}
}

void readIRsensors(){
	for (int i=0; i<NUM_SENSORS; i++) {
		ir_values[i] = readIRsensor(i);
	} 
}

void printIRsensors(){
	Serial.print("Raw sensor readings: ");
	for (int i=0; i<NUM_SENSORS; i++) {
		Serial.print(ir_values[i]);
		Serial.print(" ");
	} 
	Serial.println();
}

void rad2degTest(){
	for (int i=0; i < 8; i ++) {
		Serial.println(i / 8.0 * TAU);
		Serial.println(normaliseDegrees(rad2deg(i / 8.0 * TAU)));
	}
}

float rad2deg(float rad){
	return (rad * -1.0 + TAU / 4.0) * (360.0 / TAU);
}

float normaliseDegrees(float d){
	//return fmod(d + 180, 360); 
	if (d < -180){
		return d + 360;
	}else if(d>180){
		return d - 360;
	}else{
		return d;
	}
}
float vector2distance(float vector_magnitude){
       return exp(0.33/vector_magnitude) * 0.03;
}
float ball_angle() {
	//Serial.println("ball angle called!");
	
	// Calculate the centroid of the ball detection vectors...
	
	float x_total = 0; float y_total = 0; int count = 0; 
	for(int n=0; n<NUM_SENSORS; n++) {
		x_total += IR_COORDINATES[n][0]*ir_values[n];
		y_total += IR_COORDINATES[n][1]*ir_values[n];
		/*
		Serial.print(n);
		Serial.print(": (");
		Serial.print(IR_COORDINATES[n][0]*ir_values[n]);
		Serial.print(",");
		Serial.print(IR_COORDINATES[n][1]*ir_values[n]);
		Serial.print("); ");
		*/
		count += 1;
	}
	//Serial.println();
	
	float x_average = x_total/(float)count;
	float y_average = y_total/(float)count;
	
	/*
	Serial.print("  X AVERAGE: ");
	Serial.print(x_average);
	Serial.print("  Y AVERAGE: ");
	Serial.print(y_average);
	Serial.println(" ");
	*/
	
	// Calculate angle:
	// This will use the atan2() function to determine the angle from the average x and y co-ordinates
	// You can use the degrees() function to convert for output/debugging.
	float angle = atan2(x_average, y_average);
        Serial.print("Angle to ball: ");
        Serial.print(degrees(angle));
	
	// Calculate approximate distance:
	// First, determine the length of the vector (use the Pythagorean theorem):
	float vector_magnitude = sqrt(pow(x_average, 2)+pow(y_average,2));	// TODO: your code here
        
	// We need to map the raw vector magnitudes to real-world distances. This is probably not linear! Will require some calibration testing...
	float distance = vector2distance(vector_magnitude); // TODO: your code here
	Serial.print(" Vector magnitude: ");
	Serial.print(vector_magnitude);
	Serial.print(" Distance: ");
	Serial.print(distance);
	Serial.println("");
	// TODO: maybe also check for infinity, and map that to a usable value (e.g. 0).
}
<file_sep>/Projects/2016/MIDIBots/arduino/pwm_examples/pwm_4.ino
// MIDI note tests for Mechatronics, CME 2016-03-21
// 1. Loop sanity check.  Watch for off-by-one errors!
// 2. Translate note frequency formula into C syntax and check (MIDI note 69 should give 440 Hz). 
// 3. Correct integer division in formula!
// 4. Derive OCR "top" value from frequency, and print for checking.

void setup() {
	delay(8000);  // safety delay for serial I/O
	Serial.begin(9600);
}

void loop() {
	int prescale = 8;
	for (int n = 0; n < 128; n++) {
		float f = 440 * pow(2, (n - 69) / 12.0);
		int top = round(F_CPU / (prescale * f) - 1);
		Serial.print(n);
		Serial.print(": f=");
		Serial.print(f);
		Serial.print(", top=");
		Serial.println(top);
	}
	while (1) {}
}
<file_sep>/Projects/2016/MIDIBots/arduino/ukulele_test/ukulele_test.ino
// Testing multiple servo motors

#include <Servo.h> 
 
Servo servo1;

int pos = 0;    // variable to store the servo position 

const int STRUM_MAX_POS = 100; //sets MAX and MIN servo postions for all strings
const int STRUM_MIN_POS = 80;

const int G_MAX_POS = 85;
const int G_MIN_POS = 82;

const int C_MAX_POS = 87; 
const int C_MIN_POS = 85; 

const int E_MAX_POS = 91; 
const int E_MIN_POS = 88; 

const int A_MAX_POS = 94;
const int A_MIN_POS = 92;

const int SERVO_TIME = 200           ; // ms delay to allow motion to happen

const int SERVO_1_PIN = 11; //Servo is connected to pin 11(pwm)

byte byteRead; //temporary storage of serial bytes

void setup() 
{ 
	pinMode(SERVO_1_PIN, OUTPUT);
	digitalWrite(SERVO_1_PIN, LOW);
	servo1.attach(SERVO_1_PIN);	
        Serial.begin(9600); //start serial protocol
} 
 
void G() //A function to play the G string on the Uke
{
	servo1.write(G_MAX_POS);
	delay(SERVO_TIME);
	servo1.write(G_MIN_POS);
	delay(SERVO_TIME);
}
void C()
{
	servo1.write(C_MAX_POS);
	delay(SERVO_TIME);
	servo1.write(C_MIN_POS);
	delay(SERVO_TIME);
}

void E()
{
	servo1.write(E_MAX_POS);
	delay(SERVO_TIME);
	servo1.write(E_MIN_POS);
	delay(SERVO_TIME);
}

void A()
{
	servo1.write(A_MAX_POS);
	delay(SERVO_TIME);
	servo1.write(A_MIN_POS);
	delay(SERVO_TIME);
}

void strum()
{
	servo1.write(STRUM_MAX_POS);
	delay(SERVO_TIME);
	servo1.write(STRUM_MIN_POS);
	delay(SERVO_TIME);
}

void manual(int max_pos, int min_pos, int servo_delay)
{
  servo1.write(max_pos);
  delay(servo_delay);
  servo1.write(min_pos);
  delay(servo_delay);
}
void serial_read()
{
  if (Serial.available() > 0) //checks if a serial device is listening
  {
    byteRead = Serial.read(); //captures serial data byte
    if(byteRead == 'G') //checks weather the user has typped "G"
    {
	G(); //if so call the G function which will play the G string on the ukulele.
    Serial.println("recived G"); //Alerts the user that it has been played
	
  }
  if(byteRead == 'C')
    {
	C();
 Serial.println("recived C");
  }
  if(byteRead == 'E')
    {
	E();
 Serial.println("recived E");
  }
  if(byteRead == 'A')
    {
	A();
	 Serial.println("recived A");
  }
  if(byteRead == 'S')
    {
	strum();
 Serial.println("recived strum");
  }
}
}

void loop() {
	serial_read();
}


<file_sep>/Projects/2017/SoccerBots/restarteverything.sh
#!/bin/bash
./stopeverything.sh
./runeverything.sh
<file_sep>/Projects/2017/SoccerBots/initial_blob_tracking.py
#!/usr/bin/env python2.7
import SimpleCV
import os
import sys
sys.path.append(os.environ['HOME'] + '/robotics_repo/Documentation/Computer Vision/infoscimechatronics')
import cvutils
import time
import serial
#sys.path.append(os.environ['HOME'] + '/robotics_repo/Projects/2017/SoccerBots')
execfile("camera_calibration_state.py")

#ser = serial.Serial('/dev/serial/by-id/usb-www.freetronics.com_Eleven_64935343233351909241-if00')

camera = SimpleCV.Camera(0, {"width":320,"height":240})
os.system(os.environ['HOME'] + '/robotics_repo/Projects/2017/SoccerBots/uvcdynctrl-settings.tcl')

speed = 0
#global current_angle
#current_angle = 0
prev_error = 0
integral = 0
derivative = 0
#global hunt_dir
hunt_dir = 1 
hunt_step = 15
#blobs_threshold = 0.0075
blobs_threshold = 0.00005
times = []
output = False
	

def average(numbers):
	x = 0
	for num in numbers:
		x += num
	x = float(x) / len(numbers)
	return x

def x_coordinate_to_angle(coord): 
	#return coord*35.543 #calibrated for LogiTech Camera.
	return coord*33.0 #calibrated for RPi Camera. 

def plant(control): #control input from -1...1 so -90...90 deg / sec
	current_angle += speed
	speed = control * 90
	return current_angle
	
#Sends serial data to connected Arduino, input data between -90 to 90, output data to servo is 0 to 180 deg
def servo(target_angle):
	current_angle = get_current_angle() + 90
	sys.stderr.write("current angle: " + str(current_angle) + ' ')
	sys.stderr.write("target angle:: " + str(target_angle) + '\n')
	for i in range(abs(int(target_angle))):
		if target_angle > 0:
			if current_angle < 180:
				ser.write('+')
		if target_angle < 0:
			if current_angle > 0:
				ser.write('-')
	#time.sleep(0.2)
	
def get_current_angle():
	global ser
	ser.write("?")
	return int(ser.readline().strip()) - 90
	
def clip_angle(angle):
	if angle < -90:
		return -90
	if angle > 90:
		return 90
	else:
		return angle
	
def servo_abs(target_angle):
	servo(target_angle - get_current_angle())

def seek():
	sys.stderr.write("Seek called & hunt_dir = " + str(hunt_dir) + "\n")
	global hunt_step
	global hunt_dir
	current_angle = get_current_angle()
	if current_angle >= 90 or current_angle <= -90:
		hunt_dir *= -1	
	servo_abs(clip_angle(current_angle + hunt_dir * hunt_step))
	time.sleep(0.05)
	
def control(target):
	kp = 1
	ki = 1
	kd = 1
	error = target - current_angle
	integral += error
	derivative = error - prev_error
	prev_error = error
	control = kp * error + ki * integral + kd * derivative
	sys.stderr.write(control)
	sys.stderr.write(error * kp)
	sys.stderr.write(integral * ki)
	sys.stderr.write(derivative * kd)
	sys.stderr.write(plant(control))
	return plant(control)

def send2pd(message):
	print(str(message) + ";")
	sys.stdout.flush()

while True:
	start_time = time.clock()
	image = cvutils.wb(camera.getImage().scale(0.075), calibrated_grey_sample)
	v,s,h = image.toHSV().splitChannels()
	hue_match = h.colorDistance((calibrated_goal_sample[0][0],calibrated_goal_sample[0][0],calibrated_goal_sample[0][0])).binarize(calibrated_goal_sample[0][1]*3)
	sat_match = s.colorDistance((calibrated_goal_sample[1][0],calibrated_goal_sample[1][0],calibrated_goal_sample[1][0])).binarize(calibrated_goal_sample[1][1]*3)
	matched = ((hue_match / 16) * (sat_match / 16))
	matched.show()
	blobs = matched.findBlobs(100, 1)
	if blobs is not None:
		blob_size = blobs[-1].area()
		image_size = image.area()
		sys.stderr.write ("Blob size / image size: " + str(blob_size / image_size) + "\n")
		if blob_size / image_size < blobs_threshold:
			sys.stderr.write("Blobs too small! \n")
			send2pd(-180) #404 not found
			#seek()
		else:
			(x,y) = blobs[-1].centroid()
			image.dl().line((x,0), (x,image.height), (255,0,0), antialias=False)
			image.dl().line((0,y), (image.width,y), (0,255,0), antialias=False)
			image.show()
			#sys.stderr.write float(x) / image.width
			converted_coord = float(x) / image.width
			converted_coord = x_coordinate_to_angle(converted_coord*2-1)
			sys.stderr.write("converted_coord: " + str(converted_coord) + ' ' + '\n')
			#servo(converted_coord*0.3)
			send2pd(converted_coord)
	else:
		sys.stderr.write("No blobs found! \n")
		#seek()
		send2pd(-180)
	end_time = time.clock()
	elapsed_time = end_time - start_time
	times.append(elapsed_time)
	frame_rate = 1 / average(times)
	sys.stderr.write('Frame rate:' + str(frame_rate) + '\n')
	#image.show()
<file_sep>/Projects/2017/SoccerBots/arduino/atan2_test/atan2_test.ino
#include <math.h>

const float TAU = asin(1);

// Oh, there's already a degrees() in math.h?
//float degrees(float radians) {return 360.0 / TAU * radians;}

void atan2_test() {
	for (int x = -1; x < 2; x++) {
		for (int y = -1; y < 2; y++) {
			Serial.print("atan2("); Serial.print(x); Serial.print(","); Serial.print(y); Serial.print(") = ");
			Serial.println(degrees(atan2(x,y)));
		}
	}
}

void setup() {
	Serial.begin(9600);
}

void halt() {
	while (1) {}
}

void loop() {
	atan2_test();
	halt();
}
<file_sep>/Projects/2016/MIDIBots/arduino/drum_roll/drum_roll.ino
#include "Timer.h"
#include <Servo.h> 

Timer *phase_timer = new Timer(50); //starts a new timer

Servo servos [2];

int pos = 0;    // variable to store the servo position
const int CENTRE = 90;
const int RANGE = 46;
const int MAX_POS = CENTRE + RANGE/2;
const int MIN_POS = CENTRE - RANGE/2;

const int SERVO_TIME = 100; // ms delay to allow motion to happen
int phase = 0;


void setup() 
{ 
	servos[0].attach(9);
	servos[1].attach(10);
	
	phase_timer->setOnTimer(&hit); //sets new timer
	
	phase_timer->Start(); //starts new timer
} 

void hit(){
	phase = !phase;
	down_strum(servos[phase]);
	up_strum(servos[!phase]);
}	
void down_strum(Servo servo){
	for (int i = 0; i < 4; i++)
		servo.write(MAX_POS);
	
}


void up_strum(Servo servo){
	for (int i = 0; i < 4; i++)
		servo.write(MIN_POS);
	
}

void loop() {
	
	phase_timer->Update();
}
<file_sep>/Projects/2017/SoccerBots/arduino/sensors/mk_shortened.h
//shortened version of In The Hall of The Mountain King for Shutter on startup

const int mk_shortened_tempo = 150;
const int mk_shortened_array_length = 13;
float mk_shortened[mk_shortened_array_length][2] = {{62,0.5},{64,0.5},{65,0.5},{67,0.5},{69,0.5},{65,0.5},{69,0.5},{74,0.5},
	{73,0.5},{69,0.5},{73,0.5},{76,0.5},{74,2}}; 
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_recorderbot/firmware_recorderbot.ino
// RecorderBot code for the 2016 MIDIBots
// Team _underscore_, Information Science Mechatronics, University of Otago

#include <MIDIBot.h>
MIDIBot recorderBot;

const int pipe1 = MOSFET_PWM_PIN;
const int pipe2 = MOSFET_2_PIN;
const int pipe3 = MOSFET_3_PIN;
const int pipe4 = MOSFET_4_PIN;

//midi note numbers
const int note1 = 60;
const int note2 = 61;
const int note3 = 62;
const int note4 = 63;

void note_on(int note, int velocity) {
    
        if(note == note1) {
          digitalWrite(pipe1, HIGH);
        } else if(note == note2) {
          digitalWrite(pipe2, HIGH);
        } else if(note == note3) {
          digitalWrite(pipe3, HIGH);
        } else if(note == note4) {
          digitalWrite(pipe4, HIGH);
        }
        digitalWrite(LED_PIN, HIGH);
       
}

void note_off(int note, int velocity) {
        if(note == note1) {
          digitalWrite(pipe1, LOW);
        } else if(note == note2) {
          digitalWrite(pipe2, LOW);
        } else if(note == note3) {
          digitalWrite(pipe3, LOW);
        } else if(note == note4) {
          digitalWrite(pipe4, LOW);
        }
        digitalWrite(LED_PIN, LOW);
}

void self_test() {
//	thisMIDIBot.test_MIDI_channel();
	// TODO: Bot-specific self-test routine...
        
        note_on(60, 100); delay(500);
        note_off(60, 100); delay(500);
        note_on(61, 100); delay(500);
        note_off(61, 100); delay(500); 
        note_on(62, 100); delay(500);
        note_off(62, 100); delay(500);
        note_on(63, 100); delay(500);
        note_off(63, 100); delay(500);
        
        note_on(60, 100); delay(3000);
        note_off(60, 100); note_on(61, 100); delay(3000);
        note_off(61, 100); note_on(62, 100); delay(3000);
        note_off(62, 100); note_on(63, 100); delay(3000);
        note_off(63, 100); delay(1000);
        
        note_on(60, 100); delay(1000);
        note_on(61, 100); delay(1000);
        note_on(62, 100); delay(1000);
        note_on(63, 100); delay(1000);  
     
        note_off(60, 100); note_off(61, 100); note_off(62, 100); note_off(63, 100);   
        
        /*for(int x=0; x<= 10; x += 1) {
          note_on(60, 100); delay(2500);
          note_off(60, 100); delay(2500);
        }*/
}

void setup() {
	//recorderBot.test_MIDI_channel();	// Indicate MIDI channel at startup
        recorderBot.begin();
}

void loop() {
        recorderBot.process_MIDI();
}
<file_sep>/Projects/2017/SoccerBots/todo.txt
Things to be done for the 2017 SoccerBots project

[ ] Register for Nationals!!!
[ ] Move this list to github's issue tracker?

Software:

[ ] Prioritise and complete Pd behaviour for Shutter(attacker)
[ ] Define a DEBUG function or macro for Arduino programs
[?] Rewrite "#ifdef"s to use "#if" conditionals (where it makes sense)
       e.g. "#if DEBUGGING==1" instead of "#ifdef DEBUGGING"
[ ] Robot control behaviour (in Pd)
[Y] Arrange for control program to launch when Raspberry Pi boots
	Pd startup: pd -noaudio -nomidi patch-filename.pd

[.] EEPROM goodness (motor PWM limits, servo ranges, magnetometer calibration data, etc.)
	[Y] Have done this for magnetometer stuff...
	[ ] ...but not for machine-dependent motor speed PWM values and servo ranges

[Y] Add restart-on-error to read_sensors.tcl
	[ ] Could still add catching of write errors if the pipe (TCP from pdsend to Pd) fails..perhaps restart Pd if so.

[Y] Implement shell "until" loops in runeverything.sh for more graceful and efficient startup.

[ ] Edit /etc/hosts (or equivalent) on remote client computers to add:
	192.168.43.253	shutter
	192.168.43.237	boris
[ ] ...and aliases:
	alias shutter='ssh -Y pi@shutter'
	alias boris='ssh -Y pi@boris'
	alias attack='ssh -Y pi@shutter'
	alias goalie='ssh -Y pi@boris'

[Y] Have runeverything.sh launch separate terminal windows for each task, e.g.
	urxvt -name Blah -title Blah -e sh -c "mycmd | targetcmd" &
	(might be different for LXTerminal...ah, but both bots already have urxvt as well)

[Y] Fix slow report frequency on sensor board
	This was due to the DFRobot board being in the wrong mode, and pulseIn() blocking for a long time when the ultrasound sensor got no reflection.
[Y] sensors.ino: reinstate debugging delay (now that we've figured out why the sensor reporting frequency was so low)
[ ] Get Pd patches to properly handle the 0 values reported by the ultrasonic sensor (these mean "out of range", not 0 cm distance)
[Y] Music for the buzzer!
	Tobias has created a nice library of tunes. :)

IR system:

[Y] Implement trigonometry stuff for ball direction finding (use atan2() function)
[Y] Calibrate IR ball distance sensing (want to determine approx. distance based on magnitude of the ball sense vector)
	Will probably need to talk about what a vector is and how you can use and combine them. :)

	To pipe input from serial:
	cu -l ttyUSB0 -s 115200
	
	To pipe a text stream (in FUDI format) into Pd:
	pdsend 7000
	
	Combining the two using a shell pipeline:
	cu -l ttyUSB0 -s 115200 | pdsend 7000

[N] The RCJ IR beacon pattern varies the signal strength! Might be a good idea for our Arduino program to read frequently and calculate the mean signal strength every n samples.
	NOTE: average the raw co-ordinates, not the angle (the sign flip at 180 degrees will cause problems otherwise)
	Probably not necessary now that we've added a hardware-based analog low-pass filter to each sensor. :)


Vision system:

[ ] refine debug function for debug levels

[ ] fix white balance bug when click off image

[Y] Add separate calibration program, and handle saving/loading calibration state

[ ] Try to reduce latency with the Pi Camera and SimpleCV


General:

[Y] Design and implement serial protocol for motor control


Hardware:

[ ] Design and build circuits for detecting and alerting on low battery.
	Could just have a big red LED that comes on when low.
	Or it could provide a signal to the sensor Arduino.
	What threshold? Somewhere around 5.5 V?
		For NiMH, 1.0-1.1 V per cell is probably about the beginning of the end.
		So around 6.0-6.6 V would be a sensible warning threshold.
		Of course, under load, the battery voltage will be a bit lower.
	http://www.edn.com/design/analog/4427219/Simple-circuit-indicates-a-low-battery
		For a threshold around 6-6.6 V, try R1 = 51..56 kohm (or a 100k trimpot)
		Also, R5 (100k) might need to be increased to prevent the LED from turning on slightly under normal conditions. Try 1M.

[Y] Redesign main chassis and see if we can get it lasercut (it might be too large for Chris's 3D printer)
	The revised design was just small enough to print. :)

[Y] Design and fab main robot platform (3D print? lasercut?)

[ ] Implement servo mechanism for wheels? Optical? Hall sensor?

[.] Design and test kicker mechanism. Servo? Maybe a high-speed one from Pololu.

[, ] Trial using a DSpace Robot board for motor and servo control (rather than separate Arduino + H-bridge)

[Y] Motor Enable switch (just a software switch for the DSpace board)

[Y] Build a power+ground bus board (screw terminals for input, header pins for multiple outputs)

[Y] Main power switch (to save having to disconnect the battery when not in use!)

[ ] A pushbutton switch to safely shut down the Pi (with accompanying software)

[ ] A pushbutton switch to set the orientation to the target goal (once EEPROM saving of magnetometer calibration is done)

[?] Add buzzer, and use it to warn of low voltage as reported by the DC-DC converter

[?] Maybe add lights? Just decorative? Must not confuse other robots!

[Y] Build second robot as Goalie.

[Y] Apply threadlocker (e.g. Loctite) to fasteners before competition
	or (perhaps better) use nylock locking nuts

[.] Build IR sensors for ball detection
	The high platform and horizontal sensor mounting means the ball can't reliably be detected when close!
	Could redesign the platform with an angled (conical?) base perhaps.

[.] Compass sensor (Sebastian and Tobias have been working on this)

[Y] Install range sensors front and/or back for collision avoidance.
	[ ] Finish CAD model of mounting bracket and print

[.] Colour sensors for approximate location on the field
	Need to get some - Adafruit?
	We have a prototype using just an active IR reflectance sensor, which seems to work pretty well.
<file_sep>/Projects/2016/MIDIBots/arduino/pwm_examples/pwm_1.ino
// MIDI note tests for Mechatronics, CME 2016-03-21
// 1. Loop sanity check.  Watch for off-by-one errors!

void setup() {
	delay(8000);  // safety delay for serial I/O
	Serial.begin(9600);
}

void loop() {
	for (int i = 0; i < 128; i++) {
		Serial.println(i);
	}
	while (1) {}
}
<file_sep>/Projects/2017/SoccerBots/arduino/sensors/reflectance.h
const int reflectancePin =  A8;
int reflectance;

void readReflectance() {
  reflectance = analogRead(reflectancePin);
}
<file_sep>/Projects/2017/SoccerBots/camera_calibration_state.py
calibrated_grey_sample = (175.0, 164.57142857142858, 166.42857142857142)
calibrated_goal_sample = ((101.5, 1.5), (189.5, 19.5), (50.0, 14.0))
<file_sep>/Projects/2016/MIDIBots/arduino/Readme.txt
NOTE: to make these files available in the Arduino IDE Sketchbook menu, you will need to symlink the containing "arduino" folder from the sketchbook folder.

The location of the sketchbook folder is customisable, and the default location varies by platform:

Linux: ~/sketchbook
Mac OS: ~/Documents/Arduino
Windows:
	%USERPROFILE%\My Documents\Arduino
	%USERPROFILE%\Documents\Arduino
	or search the Registry for the path to "My Documents" at "HKEY_CURRENT_USER\Software\Microsoft\Windows\CurrentVersion\Explorer\Shell Folders"
	There might also be useful Registry entries created by the Arduino IDE.

It might be possible to call the Arduino IDE in command-line mode to find out the sketchbook path, although even that would vary by platform ("Arduino"/"arduino", and it might not be in the path or have a standard installation location).

On Mac OS, assuming you've cloned the git repo into ~/robotics_repo:

ln -s ~/robotics_repo/Projects/2016/MIDIBots/arduino ~/Documents/Arduino/MIDIBots

--

Timer.h is from the PNG library:

https://github.com/aron-bordin/PNG-Arduino-Framework/blob/master/docs/installation.rst
<file_sep>/Projects/2016/MIDIBots/TODO.txt
2016 Otago Regional event was a success!  We placed 1st in the Senior Theatre, yay! :)

National event is in Auckland on Saturday, September 10 (entries close Friday, August 26).  Some things to work on between now and then:

[ ] Test the spare PSUs (power supply units) in case they're needed
	Oh dear. One shuts down, the other blows up! :(
[Y] Make custom Molex power wiring?  See Power.txt

[ ] Double-check latency settings for the bots that need it

[ ] Check that the latest code compiles and runs correctly, and that it's all in the repository and pushed to github
	This will be very important if we have to make last-minute code changes at the event!

[ ] Sequence the music and lighting in Rosegarden!
	See Music.txt for suggestions for the medley
	[ ] Add triangle hit between We Will Rock You and Smoke on the Water, to mark the transition.
	[ ] Change chords at end of Trololo - maybe should be 
	[ ] Occasional percussion during Fuer Elise?
	[ ] Check latency (delay) setting for each segment. Parts for one channel might need to be split across multiple tracks or segments, e.g. for PercussionBot's shaker, which has more delay I think.
	[ ] Check which system timer is being used for Rosegarden's sequencer. The default 250 Hz one might be a bit coarse.

[Y] Refactor the firmware programs to use our MIDIBot library:
	[Y] UkeBot
	[Y] LightingBot
	[Y] PercussionBot
	[Y] DrumBot
	[Y] SynthBot

[ ] Tweak lighting on individual bots (placement, colour, fading)
	Maybe illuminated balloon idea? They look great using the 12 V LED modules from Jaycar
	SynthBot
	PercussionBot
	LightingBot?

[ ] Fix bug in conditionally-compiled code for the ATmega1280/2560 in programs that use custom PWM control (comma should be semicolon)
	We're only using one Mega (for FanBot) but just in case...
[ ] Also, the custom PWM code doesn't correctly handle a duty cycle of 0, it seems - on the oscilloscope on the FanBot firmware test, a setting of 0 still gives a narrow pulse.
	Try phase-correct PWM. See:
	http://stackoverflow.com/questions/23853066/how-to-achieve-zero-duty-cycle-pwm-in-avr-without-glitches

UkeBot:
	[Y] Cut off excess board behind tuning pegs for easier access.
	[N] Upgrade tuners to geared ones?  Maybe not worth the expense.  Also, with some force, they're more stable now.
	[Y] Reduce friction in slide guideway: reshape a little, rub wax into wood.
	[Y] Slide mechanism for UkeBot (stepper motor)
		In the end we used a servo motor and a linkage

DrumBot:
	[ ] Get a proper snare drum for DrumBot (or adapt existing one)?

PercussionBot:
	[Y] Troubleshoot PercussionBot
		The shield was not properly seated, and digital pins 10 and 11 do not work properly any more (tested on oscilloscope).  With a new Arduino board it seems OK.
		I think the old board is more fried than just those two pins - it's been getting very hot, both in the power circuits and the main microcontroller chip it seems. Better not use it!
	[ ] Fix backlash in triangle striker
	[ ] Fix resonance in triangle mount
	[ ] add rattle (DC motor - fan with blade striker?)
	[ ] Tweak latencies per instrument
	[Y] use alternating note numbers for shaker movement

LightingBot:
	[Y] Finalise firmware for LightingBot to include driving the RGB lighting strip(s), maybe look at MIDI CC messages
		Works fine with MIDI note messages; don't bother with MIDI CC for now.
	[Y] Laser-cut logo (Henry)

FanBot:
	[Y] Solder a wire onto the shield for 3.3 V supply for the BMP085 pressure sensor
	[N] Add break-out headers to the stripboard for the pressure sensor (already have ground, but will need 3.3 V supply, and SDA and SCL for I2C.  These will go to pins 20 and 21 on the Mega.
	[Y] Wire up stripboard breakout for the fan. There's no connection yet between the tach output and the Mega, but everything else (including pull-up resistor) is there.
	[Y] Solder on a wire from pin 12 on the shield, since that's the pin that's used for custom PWM on the Mega.  This is for fan speed control, which is supposed to be ~25 kHz.

[Y] Air reservoir system for woodwind instruments (learn about control systems in the process)
[ ] WhistleBot (slide whistle)
[ ] RecorderBot. With a taped-over thumb-hole and solenoid finger-hole valves on the first four holes, we can play the important bits of the melody from "We Will Rock You".
[ ] Harmonica? Bird Whistle? Pan Pipes?
[ ] Could extend music to include a medley of different music. Suggestions..?
	Uke can now play bits of the melody from Fuer Elise (perhaps duet with SynthBot)
	Smoke on the Water?
	Percussion section solo (funky beats, with lighting show)

Chris:
[N] Buy a set of Chinese Arduino clones
	No, we have enough if we use Paul Campbell's and use a Mega for the FanBot (in order to get access to the I2C pins).
<file_sep>/Projects/2017/RescueBot/arduino/sensor_test/sensor_test.ino
// Testing the IR reflectance sensors with the DSpace Robot board
// Working towards a line-following robot for Robocup 2017

// Current config (on the 2013 Dalek test-bed) uses pin 2 on each of sensor blocks SENSOR0, SENSOR1, SENSOR2, and SENSOR3.
// These correspond to A0..A3.
// A4 and A5 are also available

#define L_PIN A0
#define M_PIN A1
#define R_PIN A2

#define WHITE_ON_BLACK 1


// Will need light/dark thresholds for each sensor for calibration...
// ...or just one for all, if they read consistently enough
const int SENSOR_THRESHOLD=250;

// TODO: a #define flag for black line on white background or vice versa.

void setup() {
	pinMode(A0, INPUT); digitalWrite(A0, LOW);
	pinMode(A1, INPUT); digitalWrite(A1, LOW);
	pinMode(A2, INPUT); digitalWrite(A2, LOW);
	Serial.begin(9600);
	
}

// Read and compare with threshold:

bool line_detected(int analogPin) {
	int raw_reading = analogRead(analogPin);
	// TODO: if debugging, print raw reading to serial output
	// If you need to flip the light/dark sense, do it here
#ifdef WHITE_ON_BLACK
	return (raw_reading < SENSOR_THRESHOLD);	// White line, black background
#else
	return (raw_reading > SENSOR_THRESHOLD);	// Black line, white background
#endif
}


void control() {
	bool l_line = isBlack(L_PIN);
	bool m_line = isBlack(M_PIN);
	bool r_line = isBlack(R_PIN);
	
	// TODO: could shift and combine the boolean values for more readable code
	// e.g. switch bits ... 0b000 -> lost ... 0b010 -> fwd ...
	
	if (!l_line && !m_line && !r_line) {Serial.println("Lost!");}
	if (!l_line && !m_line &&  r_line) {Serial.println("spin right");}
	if (!l_line &&  m_line && !r_line) {Serial.println("fwd");}
	if (!l_line &&  m_line &&  r_line) {Serial.println("veer right");}
	if ( l_line && !m_line && !r_line) {Serial.println("spin left");}
	if ( l_line && !m_line &&  r_line) {Serial.println("?!");}
	if ( l_line &&  m_line && !r_line) {Serial.println("veer left");}
	if ( l_line &&  m_line &&  r_line) {Serial.println("perpendicular?!");}
}

void debug() {
	Serial.print(line_detected(A0)); Serial.print("	");
	Serial.print(line_detected(A1)); Serial.print("	");
	Serial.print(line_detected(A2)); Serial.print("	");
	Serial.println();

	delay(100);
}

bool isBlack(int pin) { return analogRead(pin) > SENSOR_THRESHOLD;}

void loop() {
	control();
	//delay(100);
	
	//Serial.print("A0:"); Serial.print(analogRead(A0)); Serial.print("   ");
	//Serial.print("A1:"); Serial.print(analogRead(A1)); Serial.print("   ");
	//Serial.print("A2:"); Serial.println(analogRead(A2));
	
	//Serial.print(isBlack(A0)); Serial.print("   ");
	//Serial.print(isBlack(A1)); Serial.print("   ");
	//Serial.print(i475sBlack(A2)); Serial.print("   ");
	//Serial.println();
	
	delay(250);
}
<file_sep>/Projects/2017/SoccerBots/arduino/buzzer/buzzer.ino
#include "pokemon.h"
#include "nokia.h"
#include "mountainKing.h"
#include "fnaf.h"
#include "sherlock.h"

const int buzzerPin = 10;
int crotchet;

float frequency(int note) {
	return 440 * pow(2.0, (note - 69) / 12.0);
}

void playNote(int pitch, int duration) {
	tone(buzzerPin, (int)round(frequency(pitch)), duration);	
	delay(duration+5);
	noTone(buzzerPin);
	delay(5);
}

void setNoteDurations(int bpm) {
	crotchet = 60000 / bpm;
}

void playTune(float tune[][2], int tempo, int arrayLength) {
	setNoteDurations(tempo);
	for(int x=0; x<arrayLength; x+=1) {
		if(tune[x][0] == 0) {
			delay(tune[x][1]*crotchet);
		} else {
			playNote(tune[x][0], tune[x][1]*crotchet);
		}
	}
}

void setup() {
	Serial.begin(9600);
}

void loop() {
	/*playTune(pokemon, pokemonTempo, pokemonArrayLength);
    playTune(nokia, nokiaTempo, nokiaArrayLength);
    playTune(mountainKing, mountainKingTempo, mountainKingArrayLength);
    playTune(fnaf, fnafTempo, fnafArrayLength);*/
    playTune(sherlock, sherlockTempo, sherlockArrayLength);
}


<file_sep>/Projects/2017/SoccerBots/opencv-install.sh
# Clear some space:
sudo apt-get purge wolfram-engine
sudo apt-get clean
sudo apt-get install localepurge

sudo apt-get update
sudo apt-get upgrade

sudo apt-get install build-essential cmake pkg-config
sudo apt-get install libjpeg-dev libtiff5-dev libjasper-dev libpng12-dev


sudo apt-get install libavcodec-dev libavformat-dev libswscale-dev libv4l-dev
sudo apt-get install libxvidcore-dev libx264-dev

sudo apt-get install libgtk2.0-dev

sudo apt-get install libatlas-base-dev gfortran

sudo apt-get install python2.7-dev python3-dev

# That should be it for the dependencies

# Now get the OpenCV sources:
# At present (2017-09-17), v3.3.0 is the latest.

cd /usr/local/src
git clone https://github.com/opencv/opencv.git
git clone https://github.com/opencv/opencv_contrib.git
cd opencv
git checkout tags/3.3.0
cd ../opencv_contrib
git checkout tags/3.3.0

# Then virtualenv setup using Python pip...

# ...

# Calling cmake:

cd /usr/local/src/opencv
mkdir build
cd build
cmake -D CMAKE_BUILD_TYPE=RELEASE \
    -D CMAKE_INSTALL_PREFIX=/usr/local \
    -D INSTALL_PYTHON_EXAMPLES=ON \
    -D OPENCV_EXTRA_MODULES_PATH=/usr/local/src/opencv_contrib/modules \
    -D BUILD_EXAMPLES=ON ..


make -j 4


cd ~/.virtualenvs/cv/lib/python2.7/site-packages/
ln -s /usr/local/lib/python2.7/site-packages/cv2.so cv2.so

exit


# Oh dear

[ 86%] Building CXX object samples/cpp/CMakeFiles/cpp-tutorial-pnp_registration.dir/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp.o
c++: internal compiler error: Killed (program cc1plus)
Please submit a full bug report,
with preprocessed source if appropriate.
See <file:///usr/share/doc/gcc-4.9/README.Bugs> for instructions.
modules/python2/CMakeFiles/opencv_python2.dir/build.make:324: recipe for target 'modules/python2/CMakeFiles/opencv_python2.dir/__/src2/cv2.cpp.o' failed
make[2]: *** [modules/python2/CMakeFiles/opencv_python2.dir/__/src2/cv2.cpp.o] Error 4
CMakeFiles/Makefile2:20896: recipe for target 'modules/python2/CMakeFiles/opencv_python2.dir/all' failed
make[1]: *** [modules/python2/CMakeFiles/opencv_python2.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[ 86%] Building CXX object samples/cpp/CMakeFiles/cpp-tutorial-pnp_registration.dir/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp.o
[ 86%] Building CXX object samples/cpp/CMakeFiles/cpp-tutorial-pnp_registration.dir/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp.o
[ 86%] Building CXX object modules/stitching/CMakeFiles/opencv_perf_stitching.dir/perf/perf_stich.cpp.o
[ 86%] Linking CXX executable ../../bin/cpp-tutorial-pnp_registration
[ 86%] Built target cpp-tutorial-pnp_registration
[ 86%] Linking CXX executable ../../bin/opencv_perf_stitching
[ 86%] Built target opencv_perf_stitching
[ 86%] Linking CXX shared module ../../lib/python3/cv2.cpython-34m.so
[ 86%] Built target opencv_python3
Makefile:160: recipe for target 'all' failed
make: *** [all] Error 2

# Out of memory while compiling, maybe?

# Yep:

[26030.153193] Out of memory: Kill process 22510 (cc1plus) score 328 or sacrifice child
[26030.153206] Killed process 22510 (cc1plus) total-vm:348648kB, anon-rss:300820kB, file-rss:0kB, shmem-rss:0kB

# Tried again without -j 4, and it got there.  It was building the cv2.cpp.o object that was the big obstacle (took some minutes for that one file!).

# The build folder after compiling contains about 3.4 GB of data, so that much will need to be left free before building.

<file_sep>/Documentation/Computer Vision/infoscimechatronics/cvutils.py
# A collection of handy functions built on SimpleCV for our robotics projects

# TODO: sort out image.show() vs image.save(display) pros/cons and scaling weirdness.

# We need this, obviously:
import SimpleCV
import numpy
import time
import sys
# Some colour space conversion functions:
# These might be handy when converting from an HSV colour reported by GIMP to one that SimpleCV can use directly.
def hue_from_angle(degrees): return degrees / 360.0 * 255
def sat_from_percent(percent): return percent / 100.0 * 255
# Actually doesn't give the right numbers?!  TODO: test!


# Generate a solid colour image, matching the resolution and format of the supplied image:
# (I couldn't see any way to create a new blank image using Image - supposedly you can call the constructor with a width and height, but not sure about setting the default colour)

def solid(image, colour):
	solid = image.copy()
	solid[0:solid.width, 0:solid.height] = colour
	return solid

# Or, now that I know about this:
#def solid(image, colour):
#	solid=image.copy()
#	solid.drawRectangle(0,0,solid.width,solid.height, color=colour)
#	return solid
# No! drawRectangle draws only the border, not a solid area!

# Oh, wait, you pass Image() an (x,y) dimension tuple when creating it.

def solid_(template_image, colour):
	solid = SimpleCV.Image(template_image.width,template_image.height)
	solid[0:solid.width, 0:solid.height] = colour
	return solid
# Bah, problems with numpy array format...

def solid__(width, height, colour):
	solid = SimpleCV.Image(width,height)
	solid[0:width, 0:height] = colour
	return solid


# or would it be better to pass (width,height) as a tuple in our function too?

# e.g.
# image = camera.getImage()
# red = solid(image, (255,0,0))



# Pixel function for highlighting pixels (e.g. to assist in troubleshooting image processing)
# Actually, applying Python-based pixel functions is ridiculously slow, so maybe this is no use...
# Also, scaling the image first seems to bork things.  Maybe different pixel format?  Oh, well..

def highlight(pixels):
	r=pixels[0]
	g=pixels[1]
	b=pixels[2]
	return (int(255*pow(r/255.0, 0.25)), int(255*pow(g/255.0, 0.25)), int(255*pow(b/255.0, 0.25)))



# Correct an image for white balance based on a sample grey point:
# NOTE: assumes RGB - should probably check for that and warn/convert if necessary.
def wb(image,grey_sample):
	grey=numpy.mean(grey_sample)
	r_corr = grey / grey_sample[0]
	g_corr = grey / grey_sample[1]
	b_corr = grey / grey_sample[2]
	#print('r_corr=' + str(r_corr) + ', g_corr=' + str(g_corr) + ', b_corr=' + str(b_corr))
	r,g,b=image.splitChannels()
	r = r * float(r_corr)
	g = g * float(g_corr)
	b = b * float(b_corr)
	return SimpleCV.Image.mergeChannels(r,r,g,b)

# Test it:
#camera.live()
## coord: (225,264), color: (158.0, 164.0, 184.0)
#x=225
#y=264
#grey_sample=(158.0, 164.0, 184.0)
#image=camera.getImage()
#image.getPixel(x,y) # Um, row/column transposition?!
#corrected=wb(camera.getImage(), grey_sample)
#corrected.getPixel(x,y)

## How fast (slow!) is it?:

##while True:
#	wb(camera.getImage(), grey_sample).show()

## And with the undistortion?
#while True:
#	wb(camera.getImageUndistort(), grey_sample).show()

## And at reduced resolution?
#while True:
#	wb(camera.getImageUndistort().scale(0.25), grey_sample).show()



# Helper function for setting the white balance manually, based on a series of samples from the camera.

# NOTE: Display.mouseLeft returns the current state of the button, whereas Display.leftButtonDownPosition() returns None unless the button was only JUST pressed. We want Display.mouseX and Display.mouseY instead, I think.

def calibrate_white_balance(camera):
	display=SimpleCV.Display()
	#camera=SimpleCV.Camera()
	prev_mouse_state = False
	sample_pixels = []
	image = camera.getImage().scale(0.5)
	image.drawText("Hold mouse button over white/grey", 8,8,color=SimpleCV.Color.YELLOW,fontsize=14)
	image.save(display)
	time.sleep(2)
	sys.stderr.write('Press and hold left mouse button over a region that should be white/grey...')
	while display.isNotDone():
		image = camera.getImage().scale(0.5)
		image.save(display)
		if display.mouseLeft:
			x = display.mouseX
			y = display.mouseY
			if not prev_mouse_state:
				# Button just pressed - initialise new list:
				sample_pixels = []
				#print(sample_pixels)
			colour_sample = image.getPixel(x,y)
			# Indicate the point being sampled:
			image.drawCircle((x,y),rad=5,color=SimpleCV.Color.VIOLET,thickness=1)
			image.save(display)
			sys.stderr.write(str(x) + ', ' + str(y) + ': ' + str(colour_sample))
			sample_pixels.append(colour_sample)
			prev_mouse_state = True
		else:
			if prev_mouse_state:
				# Button just released:
				# Um, how to take the mean of a list of tuples?
				mean_sample = tuple(map(lambda y: sum(y) / float(len(y)), zip(*sample_pixels)))
				sys.stderr.write('mean grey sample: ' + str(mean_sample))
				return mean_sample # or rinse and repeat?  Maybe not - fn can just be called again if needed.
			prev_mouse_state = False

# Usage:
#camera=SimpleCV.Camera()
#grey_sample = calibrate_white_balance()
#while True:
#	wb(camera.getImage(), grey_sample).show()

# TODO: might be nice to calculate the correct colour temperature so at least the camera could be set to that in a fixed way...might give better colour resolution than doing it in SimpleCV, which seems to have only 8-bit colour depth per channel.


# Function for identifying black regions in the image:
def find_black(image,value_threshold=75,saturation_threshold=90):
	v,s,h = image.toHSV().splitChannels()
	dark = v.binarize(value_threshold)   # had 90 on ProBook cam
	grey = s.binarize(saturation_threshold)
	return (dark/16) * (grey/16)




# Function for calibrating the colour matcher, which will be quite similar to the white-balance sampler.
# Hmm, this should really use WB-corrected images! But how does it know what correction to apply? Might have to add a grey-sample parameter.

def calibrate_colour_match(camera, grey_point):
	display=SimpleCV.Display()
	#camera=SimpleCV.Camera()
	prev_mouse_state = False
	sample_pixels = []
	image = wb(camera.getImage().scale(0.5), grey_point).toHSV()
	image.drawText("Hold mouse button over target", 8,8,color=SimpleCV.Color.YELLOW,fontsize=14)
	image.save(display)
	time.sleep(2)
	sys.stderr.write('Press and hold left mouse button over the region whose colour you wish to match...')
	while display.isNotDone():
		image = wb(camera.getImage().scale(0.5), grey_point).toHSV()
		image.save(display)
		if display.mouseLeft:
			x = display.mouseX
			y = display.mouseY
			if not prev_mouse_state:
				# Button just pressed - initialise new list:
				sample_pixels = []
				#print(sample_pixels)
			colour_sample = image.getPixel(x,y)
			# Indicate the point being sampled:
			image.drawCircle((x,y),rad=5,color=SimpleCV.Color.VIOLET,thickness=1)
			image.save(display)
			sys.stderr.write(str(x) + ', ' + str(y) + ': ' + str(colour_sample))
			sample_pixels.append(colour_sample)
			prev_mouse_state = True
		else:
			if prev_mouse_state:
				# Button just released:
				# Find the mean hue, saturation, and value...
				sample_pixels_transposed = zip(*sample_pixels)
				mean_sample = tuple(map(lambda y: sum(y) / float(len(y)), zip(*sample_pixels)))
				sys.stderr.write('mean sample (HSV): ' + str(mean_sample))
				# ...and also the max and min in order to determine the range to match within each (probably ignore value, ultimately).
				# Could perhaps just take the smaller of the ranges symmetrically around the mean...or the larger...
				# Of course, the min and max are not necessarily equidistant from the mean...maybe better to use the midpoint of the min and max only...though I think it would be nice to recognise the mean somehow...
				# We could get super-fancy and build a probability distribution function, I guess...
				# ...but note that we are kind of assuming that the hue and saturation dimensions are completely independent, which is probably not realistic (even for diffuse reflections?).
				sample_min = tuple(map(min, sample_pixels_transposed))
				sample_max = tuple(map(max, sample_pixels_transposed))
				# Ugh, you can't just subtract two tuples in Python?!
				hue_midpoint = (sample_max[0] + sample_min[0]) / 2.0
				sat_midpoint = (sample_max[1] + sample_min[1]) / 2.0
				val_midpoint = (sample_max[2] + sample_min[2]) / 2.0
				hue_tolerance = sample_max[0] - hue_midpoint
				sat_tolerance = sample_max[1] - sat_midpoint
				val_tolerance = sample_max[2] - val_midpoint
				# Alternatively, mean-based tolerance:
				#hue_tolerance=min(sample_max[0]-mean_sample[0], mean_sample[0]-sample_min[0])
				#sat_tolerance=min(sample_max[1]-mean_sample[1], mean_sample[1]-sample_min[1])
				sys.stderr.write('hue midpoint:' + str(hue_midpoint))
				sys.stderr.write('hue tolerance:' + str(hue_tolerance))
				sys.stderr.write('saturation midpoint:' + str(sat_midpoint))
				sys.stderr.write('saturation tolerance:' + str(sat_tolerance))
				sys.stderr.write('value midpoint:' + str(val_midpoint))
				sys.stderr.write('value tolerance:' + str(val_tolerance))
				# What structure to return? Perhaps a pair of triples, each having the HSV means and thresholds?
				# Or a triple of pairs?
				return ((hue_midpoint,hue_tolerance),(sat_midpoint,sat_tolerance),(val_midpoint,val_tolerance))
				#return mean_sample # or rinse and repeat?
			prev_mouse_state = False


# Hmm, those .5s are suspicious - is something being quantised to integers somewhere?
# Oh, no, it's OK - the min and max will always be integers, therefore the precision of the midpoints will be 0.5.

#h,s,v=sample
#hue_midpoint,hue_tolerance=h
#sat_midpoint,sat_tolerance=s
#v_midpoint,v_tolerance=v

# Can we simply write:


# Nice. Let's do likewise for colour matching:
# TODO: refine saturation matching. I suspect we want to match any saturation below the threshold, not just within a range, because of specular highlights.
def find_colour_1(image, hue, hue_thresh, sat, sat_thresh):
	colour_hue = (hue,hue,hue)
	colour_sat = (sat,sat,sat)
	v,s,h = image.toHSV().splitChannels()
	hue_proximity = h.colorDistance(colour_hue).binarize(hue_thresh)
	sat_proximity = s.colorDistance(colour_sat).binarize(sat_thresh)
	return ((hue_proximity/16.0) * (sat_proximity/16.0))


# Now that we have a function that returns an HSV triple of midpoints and tolerances, let's use those.
# Also, probably more efficient to convert to greyscale somewhere along the line...
def find_colour(image, hsv_midpoints_and_tolerances):
	# Unpack HSV target midpoints and tolerances:
	h,s,v = hsv_midpoints_and_tolerances
	hue_midpoint,hue_tolerance = h
	sat_midpoint,sat_tolerance = s
	#v_midpoint,v_tolerance=v
	# (we ignore value for now)
	colour_hue = (hue_midpoint,hue_midpoint,hue_midpoint)
	colour_sat = (sat_midpoint,sat_midpoint,sat_midpoint)
	v,s,h = image.toHSV().splitChannels(grayscale=True)
	# NOTE: v,s,h at this point are each BGR images..why? Supposedly, Image.splitChannels() has a grayscale parameter, which is True by default.  But even with that explicitly set to true, the resulting images are BGR.
	h = h.toGray()
	s = s.toGray()
	#v = v.toGray()
	# Not sure why the tolerances are coming out too small, but I'm having to multiply them by a minimum of about 3 to get good matching overall.
	hue_proximity = h.colorDistance(colour_hue).binarize(hue_tolerance*3)
	sat_proximity = s.colorDistance(colour_sat).binarize(sat_tolerance*3)
	return ((hue_proximity/16.0) * (sat_proximity/16.0))


# Next we need a way to find the centroid of the matching (white) pixels in an image, e.g. for targeting.

# Also, how much of the horizontal field of vision is occupied by a particular colour, since when the goal is near, the angle is not critical.
# Could probably just use blobs for that, using the bounding box.
<file_sep>/Projects/2017/SoccerBots/goal-tracking.py
import SimpleCV
import os
import sys
sys.path.append(os.environ['HOME'] + '/robotics_repo/Documentation/Computer Vision/infoscimechatronics')
import cvutils
import time
import serial

ser = serial.Serial('/dev/serial/by-id/usb-Arduino__www.arduino.cc__0043_85431303736351D070E0-if00')

camera = SimpleCV.Camera(0, {"width":960,"height":540})
os.system(os.environ['HOME'] + '/robotics_repo/Projects/2017/SoccerBots/uvcdynctrl-settings.tcl')

speed = 0
#global current_angle
#current_angle = 0
prev_error = 0
integral = 0
derivative = 0
#global hunt_dir
hunt_dir = 1 
hunt_step = 15
#blobs_threshold = 0.0075
blobs_threshold = 0.0005
times = []
output = True
image_output = True
calibrating = False
scale_factor = 1/64.0

def debug(msg):
	global output
	if output == True:
		sys.stderr.write(str(msg) + '\n')

lab_grey_sample = (155.40740740740742, 150.55555555555554, 160.07407407407408)
lab_goal_blue = ((104, 2), (218.5, 6.5), (127.5, 14.5))

if calibrating:
	grey_sample = cvutils.calibrate_white_balance(camera)
	target_colour = cvutils.calibrate_colour_match(camera, grey_sample)
else:
	grey_sample = lab_grey_sample
	target_colour = lab_goal_blue

debug(target_colour)

def average(numbers):
	x = 0
	for num in numbers:
		x += num
	x = float(x) / len(numbers)
	return x

def x_coordinate_to_angle(coord):
	return coord*35.543
		
def image_debug(img):
	global image_output
	if image_output:
		img.show()
	
#Sends serial data to connected Arduino, input data between -90 to 90, output data to servo is 0 to 180 deg
def servo(target_angle):
	current_angle = get_current_angle() + 90
	debug("current angle: " + str(current_angle) + ' ' + "target angle: " + str(target_angle))
	for i in range(abs(int(target_angle))):
		if target_angle > 0:
			if current_angle < 180:
				ser.write('+')
		if target_angle < 0:
			if current_angle > 0:
				ser.write('-')
	#time.sleep(0.2)
	
def get_current_angle():
	global ser
	ser.write("?")
	return int(ser.readline().strip()) - 90
	
def clip_angle(angle):
	if angle < -90:
		return -90
	if angle > 90:
		return 90
	else:
		return angle
	
def servo_abs(target_angle):
	servo(target_angle - get_current_angle())

def seek():
	debug('Seek called & hunt_dir = ' + str(hunt_dir))
	global hunt_step
	global hunt_dir
	current_angle = get_current_angle()
	if current_angle >= 90 or current_angle <= -90:
		hunt_dir *= -1	
	servo_abs(clip_angle(current_angle + hunt_dir * hunt_step))
	time.sleep(0.05)

while True:
	start_time = time.clock()
	image = cvutils.wb(camera.getImage().scale(scale_factor), grey_sample)
	v,s,h = image.toHSV().splitChannels()
	hue_match = h.colorDistance((target_colour[0][0],target_colour[0][0],target_colour[0][0])).binarize(target_colour[0][1]*3)
	sat_match = s.colorDistance((target_colour[1][0],target_colour[1][0],target_colour[1][0])).binarize(target_colour[1][1]*3)
	matched = ((hue_match / 16) * (sat_match / 16))
	image_debug(matched)
	blobs = matched.findBlobs(100, 1)
	if blobs is not None:
		blob_size = blobs[-1].area()
		image_size = image.area()
		#print blob_size / image_size
		if blob_size / image_size < blobs_threshold:
			print 'Blobs too small!'
			seek()
		else:
			(x,y) = blobs[-1].centroid()
			image.dl().line((x,0), (x,image.height), (255,0,0), antialias=False)
			image.dl().line((0,y), (image.width,y), (0,255,0), antialias=False)
			image_debug(image)
			#print float(x) / image.width
			converted_coord = float(x) / image.width
			converted_coord = x_coordinate_to_angle(converted_coord*2-1)
			debug('converted_coord: ' + str(converted_coord) + ' ')
			servo(converted_coord*0.3)
	else:
		print 'No blobs found!'
		seek()
	end_time = time.clock()
	elapsed_time = end_time - start_time
	times.append(elapsed_time)
	frame_rate = 1 / average(times)
	sys.stderr.write('Frame rate:' + str(frame_rate) + '\n')
	#image_debug(image)
<file_sep>/Projects/2017/SoccerBots/arduino/ultrasonic_test/ultrasonic_test.ino
const int trig = 5;
const int pwm = 8;
const int unit_division_factor = 40;
int us_distance;

void setup() {
	pinMode(trig, OUTPUT);
	Serial.begin(9600);
}

void loop() {
	digitalWrite(trig, LOW);
	digitalWrite(trig, HIGH);
	
	us_distance = pulseIn(pwm, LOW);
	Serial.println(us_distance / unit_division_factor);
	
	delay(20);
}
<file_sep>/Projects/2016/MIDIBots/arduino/digital_pin_test/digital_pin_test.ino
// We've had so many issues with flaky digital pins that it's probably worth having a test program that toggles each one a few times for easy testing on the oscilloscope...


const int FIRST_PIN = 8;
const int LAST_PIN = 13;


void setup()
{
	for (int pin = FIRST_PIN; pin <= LAST_PIN; pin++) {
		pinMode(pin, OUTPUT);
		digitalWrite(pin, LOW);
	}
}

void loop()
{
	for (int pin = FIRST_PIN; pin <= LAST_PIN; pin++) {
		multiflash(pin, 4);
	}
}

void multiflash(int pin, int times) {
	for (int i = 0; i < times; i++) {
		flash(pin);
	}
}

void flash(int pin) {
	digitalWrite(pin, HIGH);
	delay(100);
	digitalWrite(pin, LOW);
	delay(150);
}
<file_sep>/Projects/2016/MIDIBots/arduino/bmp085_demo/bmp085_demo.ino
// Put this line at the top of your program
#define BMP085_ADDRESS 0x77  // I2C address of BMP085

// Read 1 byte from the BMP085 at 'address'
char bmp085Read(unsigned char address)
{
  unsigned char data;
  
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(address);
  Wire.endTransmission();
  
  Wire.requestFrom(BMP085_ADDRESS, 1);
  while(!Wire.available())
    ;
    
  return Wire.read();
}

// Read 2 bytes from the BMP085
// First byte will be from 'address'
// Second byte will be from 'address'+1
int bmp085ReadInt(unsigned char address)
{
  unsigned char msb, lsb;
  
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(address);
  Wire.endTransmission();
  
  Wire.requestFrom(BMP085_ADDRESS, 2);
  while(Wire.available()<2)
    ;
  msb = Wire.read();
  lsb = Wire.read();
  
  return (int) msb<<8 | lsb;
}

<file_sep>/Projects/2016/MIDIBots/arduino/firmware_fanbot/firmware_fanbot.ino
// Fan control firmware for 2016 MIDIBots
// Possibly doesn't need to use MIDIBot library, but the MIDI control is handy for testing.

/*
TODO:
[ ] Add code to read from pressure sensor. Should take initial reading for "zero" reference at startup, since the sensor is absolute, not relative (differential) and ambient pressure can vary.
[ ] Add code to control fan speed to maintain desired pressure. PID control? Have some sanity checks, e.g. spin down if fault detected/suspected (if monitoring the fan's tachometer output).
https://www.youtube.com/watch?v=UR0hOmjaHp0
http://robotics.stackexchange.com/questions/10127/pid-with-position-and-velocity-goal/10128#10128
http://robotics.stackexchange.com/questions/5260/the-aerial-refueling-problem-sketch-of-a-feedback-controller
https://www.youtube.com/watch?v=H4YlL3rZaNw
https://www.youtube.com/watch?v=taSlxgvvrBM
*/

#include <Servo.h> 
#include <Wire.h>
#include <MIDIBot.h>
MIDIBot fanBot;

Servo partyPopperServo;

#define BMP085_ADDRESS 0x77  // I2C address of BMP085

#define DEBUG 1
#define NO_PID 1
	
const unsigned char OSS = 0;  // Oversampling Setting

long target_pressure = 250; // Pa relative to initial reference reading

#ifdef DEBUG 
int control_enabled = 1;
#else
int control_enabled = 0;
#endif

unsigned long last_micros = 0;

const float Kp = 0.001,
	Ki = 0.0003 * 300000, //original constant were for 300ms sampling period //0.0003
	Kd = 0.0001 * 300000; //0.0001

long ref_pressure = 0;
float integral = 0;
long prev_error = 0;
float control = 0;

const int PARTY_POPPER_MAX = 170; 
const int PARTY_POPPER_MIN = 40;

// Calibration values
int ac1;
int ac2; 
int ac3; 
unsigned int ac4;
unsigned int ac5;
unsigned int ac6;
int b1; 
int b2;
int mb;
int mc;
int md;


// MIDI note mapping:
const int FAN_NOTE_ON = 1;
const int FAN_NOTE_OFF = 0;
const int FAN_UP_NOTE = 5;
const int FAN_DOWN_NOTE = 6;

const int PARTY_POPPER_FIRE_NOTE =  3;
const int PART_POPPER_RELEASE_NOTE= 4;


// b5 is calculated in bmp085GetTemperature(...), this variable is also used in bmp085GetPressure(...)
// so ...Temperature(...) must be called before ...Pressure(...).
long b5; 

short temperature;
long pressure;

// Use these for altitude conversions
const float p0 = 101325;     // Pressure at sea level (Pa)
float altitude;

const int PWM_FREQUENCY = 25000; // Intel 4-wire fan spec. The Delta fan datasheet indicates 30 kHz .. 300 kHz, however.

void setup() {
#ifdef DEBUG 
	Serial.begin(115200);
#else
	fanBot.begin();
#endif

//	fan_speed(0);	// Bit dodgy setting this before the setup routine, but the fan spools up for some seconds otherwise.
	Wire.begin();
	bmp085Calibration();
	partyPopperServo.attach(SERVO_3_PIN);
	partyPopperServo.write(PARTY_POPPER_MIN);
#ifdef NOPID
	// Start up immediately
#else
	// NOTE: the Mega tends to output HIGH on pin 12 during programming, so we need to wait a bit for the fan to stop completely and the pressure to return to ambient before taking our initial reading.
	digitalWrite(LED_PIN, HIGH); // Light the LED to indicate we're busy
	delay(13000);
	digitalWrite(LED_PIN, LOW);
#endif

	calibrate();
	// Gah, can't get fan speed control to work when using MIDI input. Works OK using plain serial?! Dunno....
//	set_fan_speed(0.5);
	party_popper_fire();
}

int control_byte = 0;

// For testing:
int fan_speed = 0;
//const int FAN_SPEED_INCREMENT = 5,
const int  MIN_SPEED = 0,
	MAX_SPEED = 255;
/*
void speed_increase() {
	fan_speed += FAN_SPEED_INCREMENT ;
	if (fan_speed > MAX_SPEED) {fan_speed = MAX_SPEED;}
	set_fan_speed(fan_speed / 255.0);
}

void speed_decrease() {
	fan_speed -= FAN_SPEED_INCREMENT;
	if (fan_speed < MIN_SPEED) {fan_speed = MIN_SPEED;}
	set_fan_speed(fan_speed / 255.0);
}
*/

void calibrate(){
	temperature = bmp085GetTemperature(bmp085ReadUT());
	ref_pressure = bmp085GetPressure(bmp085ReadUP());
//	Serial.println(ref_pressure);
}

int rel_pressure(){
	temperature = bmp085GetTemperature(bmp085ReadUT());
	return bmp085GetPressure(bmp085ReadUP()) - ref_pressure;
} 

unsigned long elapsed_micros(){
	unsigned long current_micros =  micros();
	unsigned long micros_difference = current_micros - last_micros;
	last_micros = current_micros;
	return micros_difference;
}

void loop() {
#ifdef NOPID
	// Fan speed controlled directly by MIDI note-on velocity
#else
	unsigned long delta_t = elapsed_micros();
	
	long pressure = rel_pressure();

	//altitude = (float)44330 * (1 - pow(((float) pressure/p0), 0.190295));
	//fanBot.test_blink();

	// Proportional:
	long error = target_pressure - pressure;
	
	// Integral:
	if (control <= 1 && control >= 0)
	{
		integral += error / float(delta_t) ;
	}
	
	// Derivative:
	float derivative = (error - prev_error ) / float(delta_t);
	
	// Combined control:
	if(control_enabled){ 
		
		control = error * Kp + integral * Ki + derivative * Kd;
		set_fan_speed(control); //control
	} else{
		set_fan_speed(0);
	}
	
	
	prev_error = error;
#endif

#ifdef DEBUG 
	if (Serial.available()) {
		int control_byte = Serial.read();
		// TODO: error-checking?
		switch (control_byte) {
			case '+': target_pressure_increase(); break;
			case '=': target_pressure_increase(); break;
			case '-': target_pressure_decrease(); break;
			case '_': target_pressure_decrease(); break;
		}
	}
	Serial.print("Target pressure: ");
	Serial.println(target_pressure);
	Serial.print("pressure: ");
	Serial.println(pressure);
	Serial.print("Proportional: ");
	Serial.println(error * Kp, 6);
	Serial.print("Integral: ");
	Serial.println(integral * Ki, 6);
	Serial.print("Derivative: ");
	Serial.println(derivative * Kd, 6);
	Serial.print("Combined control: ");
	Serial.println(control, 6);
	Serial.println("Elapsed Micros:");
	Serial.println(elapsed_micros());
	Serial.println();
	delay(300);
#else
	fanBot.process_MIDI();
#endif
}


// Tricky low-level code from Chris for programming the PWM output for precise frequencies...
// Delta fan speed control requires a PWM signal with a base frequency of about 25 kHz.
// The standard Arduino PWM runs much lower than this, so we have to program it directly using the AVR control registers.

// On the Mega, we have timer1 attached to pins D11 and D12, D12 being the primary one.
// On "ordinary" Arduinos, it's on pins 9 and 10.  On the MIDIBot shield, pin 10 is Servo 2 (middle pin).
// Ideally we'd use the PWM MOSFET output on D6, but that uses Timer 0 and doesn't support frequency-accurate PWM, as far as I can tell.
// This will need an external circuit anyway to limit the current and block DC, so it's not a huge hassle to add a MOSFET circuit (with resistors and protection diode) to this as well.  It can be powered from one of the aux 12 V headers.
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
const int OUTPUT_PIN     = 12;
const int OUTPUT_PIN_AUX = 11;
#else
const int OUTPUT_PIN     = 10;
const int OUTPUT_PIN_AUX = 9;
#endif


// General settings:
const int prescale = 8;

// Some PWM control definitions:
#define TIMER_CLK_STOP		0x00	///< Timer Stopped
#define TIMER_CLK_DIV1		0x01	///< Timer clocked at F_CPU
#define TIMER_CLK_DIV8		0x02	///< Timer clocked at F_CPU/8
#define TIMER_CLK_DIV64		0x03	///< Timer clocked at F_CPU/64
#define TIMER_CLK_DIV256	0x04	///< Timer clocked at F_CPU/256
#define TIMER_CLK_DIV1024	0x05	///< Timer clocked at F_CPU/1024

// This defines which bits within the TCCRnB register determine the prescale factor:
#define TIMER_PRESCALE_MASK	0xF8        // 0B11111000

// Map desired prescale divider to register bits:
byte timer_prescale_bits(int prescale) {
	if (prescale == 1)
		return 0x01;
	else if (prescale == 8)
		return 0x02;
	else if (prescale == 64)
		return 0x03;
	else if (prescale == 256)
		return 0x04;
	else if (prescale == 1024)
		return 0x05;
	else
		return 0x00;	// Error?!
}


// Function for computing the PWM wrap limit ("top" value) based on the desired frequency (and the CPU clock speed and prescaler setting):
// TO0xBCDO: parameterise for prescale, rather than relying on it being globally defined?
unsigned int top(float frequency) {
	return round(F_CPU / (prescale * frequency) - 1);
}


void pwm(float frequency, float duty_cycle) {
	TCNT1 = 0;        // Reset timer counter
//	pwm_off(); // Maybe necessary to avoid stuck at 5 V condition? Nope, not enough...

	if (duty_cycle < 0.0) {duty_cycle = 0.0;}
	if (duty_cycle > 1.0) {duty_cycle = 1.0;}
	
	unsigned int wrap_limit = top(frequency);
	OCR1A = wrap_limit;
	OCR1B = int(wrap_limit * duty_cycle);

	// TCCR1A's timer mode should already be set, so just use TCCR1B to start PWM (is that sufficient?):
	TCCR1A = _BV(COM1A0) | _BV(COM1B1) | _BV(WGM11) | _BV(WGM10);
	TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11);	
	TCCR1B = (TCCR1B & TIMER_PRESCALE_MASK) | timer_prescale_bits(prescale);
}

void pwm_off() {
	TCCR1B = (TCCR1B & TIMER_PRESCALE_MASK) | TIMER_CLK_STOP;
	TCNT1 = 0;
	digitalWrite(OUTPUT_PIN, LOW); // This seems to be necessary to silence it properly (sometimes gets stuck high otherwise!)
}

void set_fan_speed(float target) {
	pwm(PWM_FREQUENCY, target);
}

void self_test() {
	party_popper_fire();
/*
	fan_speed(0.1);
	delay(2000);
	fan_speed(0.5);
	delay(5000);
//	fan_speed(1.0);
//	delay(6000);
	fan_speed(0);
	delay(6000);
*/
}

void target_pressure_increase() {
	target_pressure++;
	if (target_pressure > 1000) {target_pressure = 1000;}
}

void target_pressure_decrease() {
	target_pressure--;
	if (target_pressure < 0) {target_pressure = 0;}
}

void party_popper_fire() {
	partyPopperServo.write(PARTY_POPPER_MAX);
	digitalWrite(LED_PIN, HIGH);
	delay(1000);
	digitalWrite(LED_PIN, LOW);
	partyPopperServo.write(PARTY_POPPER_MIN);
} 


void note_on(int note, int velocity) {
	digitalWrite(LED_PIN, HIGH);
	switch (note) {
//			case FAN_NOTE_ON: control_enabled = 1; set_fan_speed(velocity / 127.0);  break;
//			case FAN_NOTE_ON: control_enabled = 1; set_fan_speed(0.4);  break;
			case FAN_NOTE_OFF: control_enabled = 0; break;
			case FAN_UP_NOTE: target_pressure_increase(); break;
			case FAN_DOWN_NOTE: target_pressure_decrease(); break;
			case PARTY_POPPER_FIRE_NOTE: party_popper_fire(); break;
		// Could also derive fan speed or target pressure (TODO!) from note velocity
	}
}

void note_off(int note, int velocity) {
	digitalWrite(LED_PIN, LOW);
}


// Code below taken from example demo code from the Sparkfun

#ifdef NOPID

void bmp085Calibration()
{
  ac1 = bmp085ReadInt(0xAA);
  ac2 = bmp085ReadInt(0xAC);
  ac3 = bmp085ReadInt(0xAE);
  ac4 = bmp085ReadInt(0xB0);
  ac5 = bmp085ReadInt(0xB2);
  ac6 = bmp085ReadInt(0xB4);
  b1 = bmp085ReadInt(0xB6);
  b2 = bmp085ReadInt(0xB8);
  mb = bmp085ReadInt(0xBA);
  mc = bmp085ReadInt(0xBC);
  md = bmp085ReadInt(0xBE);
}

// Calculate temperature given ut.
// Value returned will be in units of 0.1 deg C
short bmp085GetTemperature(unsigned int ut)
{
  long x1, x2;
  
  x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;
  x2 = ((long)mc << 11)/(x1 + md);
  b5 = x1 + x2;

  return ((b5 + 8)>>4);  
}

// Calculate pressure given up
// calibration values must be known
// b5 is also required so bmp085GetTemperature(...) must be called first.
// Value returned will be pressure in units of Pa.
long bmp085GetPressure(unsigned long up)
{
  long x1, x2, x3, b3, b6, p;
  unsigned long b4, b7;
  
  b6 = b5 - 4000;
  // Calculate B3
  x1 = (b2 * (b6 * b6)>>12)>>11;
  x2 = (ac2 * b6)>>11;
  x3 = x1 + x2;
  b3 = (((((long)ac1)*4 + x3)<<OSS) + 2)>>2;
  
  // Calculate B4
  x1 = (ac3 * b6)>>13;
  x2 = (b1 * ((b6 * b6)>>12))>>16;
  x3 = ((x1 + x2) + 2)>>2;
  b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;
  
  b7 = ((unsigned long)(up - b3) * (50000>>OSS));
  if (b7 < 0x80000000)
    p = (b7<<1)/b4;
  else
    p = (b7/b4)<<1;
    
  x1 = (p>>8) * (p>>8);
  x1 = (x1 * 3038)>>16;
  x2 = (-7357 * p)>>16;
  p += (x1 + x2 + 3791)>>4;
  
  return p;
}

// Read 1 byte from the BMP085 at 'address'
char bmp085Read(unsigned char address)
{
  unsigned char data;
  
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(address);
  Wire.endTransmission();
  
  Wire.requestFrom(BMP085_ADDRESS, 1);
  while(!Wire.available())
    ;
    
  return Wire.read();
}

// Read 2 bytes from the BMP085
// First byte will be from 'address'
// Second byte will be from 'address'+1
int bmp085ReadInt(unsigned char address)
{
  unsigned char msb, lsb;
  
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(address);
  Wire.endTransmission();
  
  Wire.requestFrom(BMP085_ADDRESS, 2);
  while(Wire.available()<2)
    ;
  msb = Wire.read();
  lsb = Wire.read();
  
  return (int) msb<<8 | lsb;
}

// Read the uncompensated temperature value
unsigned int bmp085ReadUT()
{
  unsigned int ut;
  
  // Write 0x2E into Register 0xF4
  // This requests a temperature reading
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(0xF4);
  Wire.write(0x2E);
  Wire.endTransmission();
  
  // Wait at least 4.5ms
  delay(5);
  
  // Read two bytes from registers 0xF6 and 0xF7
  ut = bmp085ReadInt(0xF6);
  return ut;
}

// Read the uncompensated pressure value
unsigned long bmp085ReadUP()
{
  unsigned char msb, lsb, xlsb;
  unsigned long up = 0;
  
  // Write 0x34+(OSS<<6) into register 0xF4
  // Request a pressure reading w/ oversampling setting
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(0xF4);
  Wire.write(0x34 + (OSS<<6));
  Wire.endTransmission();
  
  // Wait for conversion, delay time dependent on OSS
  delay(2 + (3<<OSS));
  
  // Read register 0xF6 (MSB), 0xF7 (LSB), and 0xF8 (XLSB)
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(0xF6);
  Wire.endTransmission();
  Wire.requestFrom(BMP085_ADDRESS, 3);
  
  // Wait for data to become available
  while(Wire.available() < 3)
    ;
  msb = Wire.read();
  lsb = Wire.read();
  xlsb = Wire.read();
  
  up = (((unsigned long) msb << 16) | ((unsigned long) lsb << 8) | (unsigned long) xlsb) >> (8-OSS);
  
  return up;
}

#endif
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_synthbot/ano/src/sketch.ino
../../firmware_synthbot.ino<file_sep>/Projects/2016/MIDIBots/arduino/firmware_percussionbot/firmware_percussionbot.ino
// PercussionBot code for the 2016 MIDIBots
// Team _underscore_, Information Science Mechatronics, University of Otago

// TODO: implement seperate right/left command notes for the shaker.

// TODO: add a DC motor operating a football/cog rattle? And a similar mechanism for repeatedly striking the triangle?

#include <MIDIBot.h>
MIDIBot percussionBot;

#include "Timer.h"
#include <Servo.h> 

Servo
	triangle_servo,
	shaker_servo,
	drum_servo
;


// TODO: Unused? Remove if so.
int pos = 0;    // variable to store the servo position

// TODO: carefully test required delay for each instrument and tweak *_DELAY constants below.

const int TRIANGLE_NOTE = 60; // Middle C
const int TRIANGLE_MIN = 20;
const int TRIANGLE_MAX = 4;
const int TRIANGLE_DELAY = 100;
// Checked!

const int SHAKER_MID = 90;

const int SHAKER_L_NOTE = 39;
const int SHAKER_R_NOTE = 40;

//const int SHAKER_R_MIN = 100;
const int SHAKER_L_MAX = 70;
const int SHAKER_R_MAX = 110;

const int SHAKER_L_DELAY = 100;
const int SHAKER_R_DELAY = 100;
// Checked! 

const int DRUM_NOTE = 36; //64
const int DRUM_MIN = 160;
const int DRUM_MAX = 137;	
const int DRUM_DELAY = 100;
// Checked!


// Simple on/off motor for rattle:
const int RATTLE_PIN = MOSFET_PWM_PIN;
const int RATTLE_NOTE = 43;
const int RATTLE_SPEED = 255;


// Timers for asynchronous release of drum hits:
Timer *triangle_timer = new Timer(TRIANGLE_DELAY, &triangle_release, 1);
//Timer *shaker_l_timer = new Timer(SHAKER_L_DELAY, &shaker_l_release, 1);
//Timer *shaker_r_timer = new Timer(SHAKER_R_DELAY, &shaker_r_release, 1);
Timer *drum_timer = new Timer(DRUM_DELAY, &drum_release, 1);


// Separate functions for hit and release for each drum:
void triangle_hit() {
	triangle_servo.write(TRIANGLE_MAX);
	triangle_timer->Start();
}
void shaker_l_hit() {
	shaker_servo.write(SHAKER_L_MAX);
//	shaker_timer->Start();
}
void shaker_r_hit() {
	shaker_servo.write(SHAKER_R_MAX);
//	shaker_timer->Start();
}
void drum_hit() {
	drum_servo.write(DRUM_MAX);
	drum_timer->Start();
}

void triangle_release() {triangle_servo.write(TRIANGLE_MIN);}
//void   shaker_release() {  shaker_servo.write(SHAKER_MIN);}
void     drum_release() {   drum_servo.write(DRUM_MIN);}

//void rattle_start() {digitalWrite(RATTLE_PIN, HIGH);}
//void rattle_stop()  {digitalWrite(RATTLE_PIN, LOW);}
// Could also analogWrite() if full-speed not required.
void rattle_start() {analogWrite(RATTLE_PIN, RATTLE_SPEED);}
void rattle_stop()  {analogWrite(RATTLE_PIN, 0);}

void note_on(int note, int velocity) {
	switch (note) {
		case TRIANGLE_NOTE: triangle_hit(); break;
		case SHAKER_L_NOTE: shaker_l_hit(); break;
		case SHAKER_R_NOTE: shaker_r_hit(); break;
		case DRUM_NOTE:     drum_hit();     break;
		case RATTLE_NOTE:   rattle_start(); break;
	}					
}

void note_off(int note, int velocity) {
	// Nothing to do for percussion!
	// Oh, except maybe the fan/rattle thing
	switch (note) {
		case RATTLE_NOTE: rattle_stop(); break;
	}
}

void test_servo() {
	triangle_servo.write(60);
	delay(500);
	triangle_servo.write(110);
	delay(500);
}

void self_test() {
	digitalWrite(LED_PIN, HIGH);
	triangle_hit();
	shaker_r_hit();
	drum_hit();
	// And rattle? Tricky, cos we'd need to schedule an OFF event for it somehow..
	rattle_start();
	delay(1000);
	rattle_stop();
	digitalWrite(LED_PIN, LOW);
}

void setup()
{
	// Servo setup:
	triangle_servo.attach(SERVO_1_PIN);
	shaker_servo.attach(SERVO_2_PIN);
	drum_servo.attach(SERVO_3_PIN);
	
	// Set initial position to _MIN values
	triangle_servo.write(TRIANGLE_MIN);
	shaker_servo.write(SHAKER_MID);
	drum_servo.write(DRUM_MIN);

	percussionBot.begin();
}

void loop()
{
	triangle_timer->Update();
//	shaker_timer->Update();
	drum_timer->Update();
	
	percussionBot.process_MIDI();
}
<file_sep>/Settings/Geany/1.28/Makefile
install:
	cp -v filetype_extensions.conf ~/.config/geany/filetype_extensions.conf
	cp -v filedefs/filetypes.Arduino.conf ~/.config/geany/filedefs/filetypes.Arduino.conf
	cp -v templates/files/sketch.ino ~/.config/geany/templates/files/sketch.ino
<file_sep>/Projects/2017/SoccerBots/arduino/switch_test/switch_test.ino
const int switch_pin = 18;

void setup(){
  pinMode(switch_pin, INPUT); digitalWrite(switch_pin,HIGH);
  Serial.begin(9600);
}

void loop(){
  Serial.println(digitalRead(switch_pin));
  delay(50);
}
<file_sep>/Projects/2016/MIDIBots/arduino/multi_servo_test/multi_servo_test.ino
// Testing multiple servo motors

#include <Servo.h> 
 
Servo servo1;
Servo servo2;

int pos = 0;    // variable to store the servo position 
const int MAX_POS = 100;
const int MIN_POS = 80;

const int SERVO_TIME = 50; // ms delay to allow motion to happen

const int SERVO_1_PIN = 11;
const int SERVO_2_PIN = 10;

void setup() 
{ 
	pinMode(SERVO_1_PIN, OUTPUT);
	digitalWrite(SERVO_1_PIN, LOW);
	servo1.attach(SERVO_1_PIN);
	
	pinMode(SERVO_2_PIN, OUTPUT);
	digitalWrite(SERVO_2_PIN, LOW);
	servo2.attach(SERVO_2_PIN);	
} 
 
void playbeat (Servo servo, int duration)
{
	servo.write(MAX_POS);
	delay(SERVO_TIME);
	servo.write(MIN_POS);
	delay(SERVO_TIME);
	delay(duration - 2 * SERVO_TIME);
}

void loop() {
	rhythm_4();
}

void rhythm_4()
{
	playbeat(servo1, 400);
	playbeat(servo1, 400);
	playbeat(servo1, 200);
	playbeat(servo1, 200);
	playbeat(servo2, 400);
	playbeat(servo1, 200);
	playbeat(servo2, 200);
	playbeat(servo2, 200);
	playbeat(servo1, 200);
}

void rhythm_3()
{
	playbeat(servo1, 200);
	playbeat(servo2, 100);
	playbeat(servo2, 100);
	playbeat(servo1, 100);
	playbeat(servo1, 100);
	playbeat(servo2, 200);
	playbeat(servo1, 200);
	playbeat(servo2, 200);
	playbeat(servo1, 200);
	playbeat(servo2, 100);
	playbeat(servo2, 100);
}

void rhythm_2()
{
	playbeat(servo1, 400);
	playbeat(servo2, 400);
	playbeat(servo1, 400);
	playbeat(servo2, 200);
	playbeat(servo1, 100);
	playbeat(servo2, 100);
}

void rhythm_1()
{
	playbeat(servo1, 200);
	playbeat(servo2, 200);

	playbeat(servo1, 200);
	playbeat(servo2, 200);

	playbeat(servo1, 200);
	playbeat(servo2, 200);

	playbeat(servo1, 100);
	playbeat(servo1, 100);
	playbeat(servo2, 100);
	playbeat(servo2, 100);
}
<file_sep>/Projects/2016/MIDIBots/arduino/ukulele_test/ano/src/sketch.ino
../../ukulele_test.ino<file_sep>/Projects/2017/SoccerBots/arduino/sensors_boris/sensors_boris.ino
// TODO: main statement of the purpose of this code!
#include <LIS3MDL.h>
#include <Wire.h>
#include <math.h>
#include <EEPROM.h>

#define DEBUGGING 0

const int NUM_SENSORS = 8;
const int analog_sensor_pins[] = {A0,A1,A2,A3,A4,A5,A6,A7};
float ir_values[NUM_SENSORS];
const float IR_GAIN_THRESHOLD = 0.5; // For culling reflections, hopefully
const int IR_THRESHOLD = 980; // About 0.15 after converting to 0..1 float looked about right, which would be ~870 raw.  In practice, with no IR ball present, we never see a raw value less than 1000
const int CALIBRATION_MODE_SWITCH_PIN = 2;
const int SAVE_HEADING_BUTTON_PIN = 7;
const int EEPROM_MIN_X = 0;
const int EEPROM_MIN_Y = 2;
const int EEPROM_MIN_Z = 4;
const int EEPROM_MAX_X = 6;
const int EEPROM_MAX_Y = 8;
const int EEPROM_MAX_Z = 10;
const int EEPROM_TARGET_HEADING = 12;

const int BUZZER = 10;
int ball_detected = 0;
float ball_angle = 0;
float ball_distance = 0;
int front_range = 0;
int back_range = 0; // so far unused
float angle_to_goal = 0;
int calibration_mode_switch = 0;
int light_sensor = 0;
int crotchet;
const int hall_effect = A9;
const int battery_voltage_pin = A10;
float battery_voltage = 0;

#if DEBUGGING ==1
	#define DEBUG(x) Serial.println (x)
	#define DEBUG_NOEOL(x) Serial.print (x)
#else
	#define DEBUG(x)
	#define DEBUG_NOEOL(x)
#endif

const float IR_COORDINATES[NUM_SENSORS][2] = {{0.0,1.0},{0.71,0.71},{1.0,0.0},{0.71,-0.71},{0.0,-1.0},{-0.71, -0.71},{-1.0, 0.0},{-0.71, 0.71}};

#include "mk_shortened.h"
#include "magnetometer.h"
#include "ultrasonic.h"
#include "reflectance.h"


// int ir_val;
const float TAU = 2 * PI;

// To write an int to EEPROM requires two write() calls, since write() works only in bytes and an int is two.
// Not sure whether to have address be a specific byte address or to multiply by sizeof(int) within the functions to create a virtual addressing scheme for int values only.
// Probably safer to leave as raw byte addresses in case we have to mix and match, but watch for accidental overwriting!

void EEPROM_write_int(int address, int value) {
	EEPROM.write(address, highByte(value));
	EEPROM.write(address + sizeof(byte), lowByte(value));
}

// Ditto for reading:

int EEPROM_read_int(int address) {
	/*
	byte highByte = EEPROM.read(address);
	byte  lowByte = EEPROM.read(address + sizeof(byte));
	return word(highByte, lowByte);
	*/
	// Or simply:
	return word(EEPROM.read(address), EEPROM.read(address + sizeof(byte)));
}

void setup() {
	for(int n=0; n<NUM_SENSORS; n++) {
		pinMode(analog_sensor_pins[n], INPUT);
	}
	pinMode(CALIBRATION_MODE_SWITCH_PIN, INPUT_PULLUP);
	pinMode(SAVE_HEADING_BUTTON_PIN, INPUT_PULLUP);
	pinMode(BUZZER, OUTPUT);
        analogWrite(BUZZER, 50);
        pinMode(hall_effect, INPUT);
        pinMode(battery_voltage_pin, INPUT);
	Serial.begin(115200);
	magnetometerSetup();
	ultrasonic_setup();
}

void test_loop() {
	for(int n=0; n<=NUM_SENSORS; n++) {
		DEBUG_NOEOL(analogRead(analog_sensor_pins[n]) + " ");
	}
	DEBUG();
	delay(50);
}

void loop() {
	calibration_mode_switch = digitalRead(CALIBRATION_MODE_SWITCH_PIN);
	if (!calibration_mode_switch) {
		magnetometer_calibrateMagnetometer(); 
	}
	if (!digitalRead(SAVE_HEADING_BUTTON_PIN)) { // allows the heading to be saved during use
		magnetometer_saveHeading();
	}
        readReflectance();
	readIRsensors();
	//printIRsensors();
	get_ball_angle();
	//get_calibration_mode_switch();
	angle_to_goal = magnetometer_getAngleToTarget();
	front_range = getUSDistance();
        battery_voltage = analogRead(battery_voltage_pin)*(10/1023.0);
        
	//TODO: get_compass etc.
	send_output();
#if DEBUGGING == 1
	delay(2000);
#else 
	delay(20);
#endif
}

void beep() {
	tone(BUZZER, 3600);
	delay(100);
	noTone(BUZZER);
}

float frequency(int note) {
	return 440 * pow(2.0, (note - 69) / 12.0);
}

void playNote(int pitch, int duration) {
	tone(BUZZER, (int)round(frequency(pitch)), duration);	
	delay(duration+5);
	noTone(BUZZER);
	delay(5);
}

void setNoteDurations(int bpm) {
	crotchet = 60000 / bpm;
}

void playTune(float tune[][2], int tempo, int arrayLength) {
	setNoteDurations(tempo);
	for(int x=0; x<arrayLength; x+=1) {
		if(tune[x][0] == 0) {
			delay(tune[x][1]*crotchet);
		} else {
			playNote(tune[x][0], tune[x][1]*crotchet);
		}
	}
}

float readIRsensor(int sensor_num) { // takes single reading from one IR sensor
	int reading = analogRead(analog_sensor_pins[sensor_num]);
	DEBUG_NOEOL("sensor "); DEBUG_NOEOL(sensor_num); DEBUG_NOEOL(": "); DEBUG(reading);
	if (reading>IR_THRESHOLD) {
		return 0.0;
	} else {
		return (1023-reading)/1023.0;
	}
}

void readIRsensors() { // takes readings from all 8 sensors and stores them in an array.
	for (int i=0; i<NUM_SENSORS; i++) {
		ir_values[i] = readIRsensor(i);
	}
  printIRsensors();
  irAutoGain();
  printIRsensors();
  
}

void irAutoGain() { // stretch out the IR sensor values, apply a threshold, then scale back again, in an attempt to ignore IR reflections off the goal.
  float irMax = 0.0;
  float irMin = 1.0;
  // First, determine the min and max for the latest readings:
  for (int i=0; i<NUM_SENSORS; i++) {
    if(ir_values[i] < irMin && ir_values[i] != 0){irMin = ir_values[i];}
    if(ir_values[i] > irMax){irMax = ir_values[i];}
  }
  // Then scale to full scale, apply threshold, and scale back down:
  for(int i=0; i<NUM_SENSORS; i++){
   if((ir_values[i]-irMin)/(irMax-irMin) < IR_GAIN_THRESHOLD){
     ir_values[i]=0;
  }
  }
}

void printIRsensors() { // used for debugging
	DEBUG_NOEOL("Raw sensor readings: ");
	for (int i=0; i<NUM_SENSORS; i++) {
		DEBUG_NOEOL(ir_values[i]);
		DEBUG_NOEOL(" ");
	} 
	DEBUG();
}

void rad2degTest() {
	for (int i=0; i < 8; i ++) {
		DEBUG(i / 8.0 * TAU);
		DEBUG(normaliseDegrees(rad2deg(i / 8.0 * TAU)));
	}
}

float rad2deg(float rad) { // redundant as math.h provides degrees function
	return (rad * -1.0 + TAU / 4.0) * (360.0 / TAU);
}

float normaliseDegrees(float d) {
	//return fmod(d + 180, 360); 
	if (d < -180) {
		return d + 360;
	} else if (d>180) {
		return d - 360;
	} else {
		return d;
	}
}

float vector2distance(float vector_magnitude) { // calculates an approx. dist. to the ball from the IR array
	return exp(0.33/vector_magnitude) * 0.03;
}

void send_output() {
	Serial.print(ball_detected);Serial.print(" ");
	Serial.print(ball_angle);Serial.print(" ");
	Serial.print(ball_distance);Serial.print(" ");
        Serial.print(analogRead(hall_effect));Serial.print(" ");
	Serial.print(front_range);Serial.print(" ");
	Serial.print(angle_to_goal);Serial.print(" ");
	Serial.print(calibration_mode_switch);Serial.print(" ");
	Serial.print(reflectance);Serial.print(" ");
        Serial.print(battery_voltage);Serial.print(" ");
	Serial.println(";");
}

int posneg(int input) {
  if(input > 180) {
    return(input - 360); 
  }
  return input;
}

float get_ball_angle() {
	DEBUG("ball angle called!");
	
	// Calculate the centroid of the ball detection vectors...

	float x_total = 0.0; float y_total = 0.0;
	for (int n=0; n<NUM_SENSORS; n++) {
		x_total += IR_COORDINATES[n][0]*ir_values[n];
		y_total += IR_COORDINATES[n][1]*ir_values[n];

		DEBUG_NOEOL("Sensor #:");
		DEBUG_NOEOL(n);
		DEBUG_NOEOL("; value=");
		DEBUG_NOEOL(ir_values[n]);
		DEBUG_NOEOL(": (");
		DEBUG_NOEOL(IR_COORDINATES[n][0]*ir_values[n]);
		DEBUG_NOEOL(",");
		DEBUG_NOEOL(IR_COORDINATES[n][1]*ir_values[n]);
		DEBUG_NOEOL("); ");
	}

	float x_average = x_total/(float)NUM_SENSORS;
	float y_average = y_total/(float)NUM_SENSORS;
	
	DEBUG_NOEOL("  X AVERAGE: ");
	DEBUG_NOEOL(x_average);
	DEBUG_NOEOL("  Y AVERAGE: ");
	DEBUG_NOEOL(y_average);
	DEBUG(" ");
	
	// Calculate angle:
	// This will use the atan2() function to determine the angle from the average x and y co-ordinates
	// You can use the degrees() function to convert for output/debugging.
	//ball_angle = degrees(atan2(x_average, y_average));
    int small=2000;
    int sens=0;
    for(int x=0; x<NUM_SENSORS; x++) {
		
		//Serial.print(analogRead(analog_sensor_pins[x]));
		//Serial.print(" ");
		
		if(analogRead(analog_sensor_pins[x]) < small) {
			small=analogRead(analog_sensor_pins[x]);
			sens=x;
		}
     }

     if(small>900) {
		 ball_detected = 0;
                 ball_angle = 999;
	 } else {		 
                ball_detected = 1;
                 ball_angle = sens*45;
                 ball_angle = posneg(ball_angle);
         }
    DEBUG_NOEOL("Angle to ball: ");
    DEBUG_NOEOL(ball_angle);

	
	// Calculate approximate distance:
	// First, determine the length of the vector (use the Pythagorean theorem):
	float vector_magnitude = sqrt(pow(x_average, 2)+pow(y_average,2));	
    if (ball_angle == 0 && vector_magnitude == 0) {
		ball_detected = 0;
	} else {
		ball_detected = 1;
	}
	// We need to map the raw vector magnitudes to real-world distances. This is probably not linear! Will require some calibration testing...
	ball_distance = vector2distance(vector_magnitude); 
	DEBUG_NOEOL(" Vector magnitude: ");
	DEBUG_NOEOL(vector_magnitude);
	DEBUG_NOEOL(" Distance: ");
	DEBUG_NOEOL(ball_distance);
	DEBUG("");
	
	// TODO: maybe also check for infinity, and map that to a usable value (e.g. 0).
}

//TODO: define other get functions (getCompass etc)
<file_sep>/Projects/2016/MIDIBots/arduino/folder_format.sh
#!/bin/bash

SAVE_DIR="/home/henry/Documents/robotics_repo/Projects/2016/MIDIBots/arduino/"
cd $SAVE_DIR
: '
	task:
		need to locate the sketch.ino file under each folder (*/src/sketch.ino)
		then we need to rename the sketch.ino file to the name of the folder containing the src folder.
		move the renamed file to the arduino directory of git.
		then run "arduiglue-arduino-folder-setup "%f" on the file.
		finally run "arduiglue-arturo-folder-setup "%d" on the directory
		echo the path tree of the folder in question?
'

#This is not polished it was a quick and dirty program for one time use with folder setup.
while [ 1 ]
		do
			FILE=`zenity --file-selection --directory --title="Select sketch.ino" --separator="+++"`
			parent=${FILE%/*}
			finalName=${parent##*/}   

			mv $FILE/sketch.ino $finalName.ino
			rm -R "$finalName/"
			arduiglue-arduino-folder-setup "$finalName.ino"
			arduiglue-arturo-folder-setup "$finalName/"
        done    


<file_sep>/Projects/2016/MIDIBots/arduino/servo_serial_test/servo_serial_test.ino
#include <Servo.h> 

Servo servo;

int angle = 90;

void setup()
{
	Serial.begin(9600);
	// Servo setup:
	servo.attach(10);
	
	// Set initial position
	servo.write(angle);
}

void angle_increase() {
	angle++;
	if (angle > 180) {angle = 180;}
	servo.write(angle);
}

void angle_decrease() {
	angle--;
	if (angle < 0) {angle = 0;}
	servo.write(angle);
	
}

int control_byte;

void loop()
{
	if (Serial.available()) {
		control_byte = Serial.read();
		// TODO: error-checking?
		switch (control_byte) {
			case '+': angle_increase(); break;
			case '=': angle_increase(); break;
			case '-': angle_decrease(); break;
			case '_': angle_decrease(); break;
		}
		Serial.println(angle);
	}
}
<file_sep>/Projects/2017/SoccerBots/arduino/sensors_boris/sherlockShort.h
//Pursuit from Sherlock Season 1

const int sherlockTempo = 200;
const int sherlockArrayLength = 15;
float sherlock[sherlockArrayLength][2] = {{72,0.5},{74,0.5},
  {75,1},{75,0.5},{77,0.5},{74,1},{74,0.5},{75,0.5},
  {72,1},{72,0.5},{74,0.5},{71,1},{68,0.5},{65,0.5},
  {72,8}};
<file_sep>/Projects/2016/MIDIBots/arduino/servo_drum_test/servo_drum_test.ino
// Sweep
// by BARRAGAN <http://barraganstudio.com> 
// This example code is in the public domain.


#include <Servo.h> 
 
Servo myservo;  // create servo object to control a servo 
                // a maximum of eight servo objects can be created 
 
int pos = 0;    // variable to store the servo position 
const int max_pos = 100;
const int min_pos = 0;

const int servo_pin = 11;
 
void setup() 
{ 
  pinMode(servo_pin, OUTPUT);
  myservo.attach(servo_pin);  // attaches the servo on pin 9 to the servo object 
} 
 
void playbeat (int posttime)
{
  myservo.write(max_pos);
  delay(80);
  myservo.write(min_pos);
 delay(posttime-80);
}
 
void loop() 
{ 
  playbeat(400);
  playbeat(400);
  playbeat(200);
  playbeat(200);
  playbeat(200);
  playbeat(200);
} 
<file_sep>/Projects/2016/MIDIBots/arduino/fan_test/fan_test.ino
// FanBot speed test code
// Control fan speed using serial data
// "+" to increase speed
// "-" to decrease speed


const int FAN_OUTPUT = 6,
	MAX_SPEED = 255,
	MIN_SPEED = 0;

int fan_speed = 0;
int control_byte = 0;

void setup()
{
	pinMode(FAN_OUTPUT, OUTPUT);
	analogWrite(FAN_OUTPUT, LOW);
	Serial.begin(9600);
}

void speed_increase() {
	fan_speed++;
	if (fan_speed > MAX_SPEED) {fan_speed = MAX_SPEED;}
	analogWrite(FAN_OUTPUT, fan_speed);
}

void speed_decrease() {
	fan_speed--;
	if (fan_speed < MIN_SPEED) {fan_speed = MIN_SPEED;}
	analogWrite(FAN_OUTPUT, fan_speed);
}

void loop()
{
	if (Serial.available()) {
		control_byte = Serial.read();
		// TODO: error-checking?
		switch (control_byte) {
			case '+': speed_increase(); break;
			case '-': speed_decrease(); break;
		}
		Serial.println(fan_speed);
	}
}
<file_sep>/Projects/2016/MIDIBots/arduino/midi_keyboard/ano/src/sketch.ino
../../midi_keyboard.ino<file_sep>/Projects/2017/SoccerBots/arduino/servo_test/servo_test.ino
// Emergency PopperBot code! FanBot problems!

#include <Servo.h> 
#include <MIDIBot.h>
//#include "/home/pi/robotics_repo/Projects/2016/MIDIBots/arduino/libraries/MIDIBot/MIDIBot.h"
MIDIBot popperBot;

Servo partyPopperServo;

const int PARTY_POPPER_MAX = 50; 
const int PARTY_POPPER_MIN = 140;
const int PARTY_POPPER_MID = 95;

// MIDI note mapping:
const int PARTY_POPPER_FIRE_NOTE =  3;
const int PART_POPPER_RELEASE_NOTE= 4;


void setup() {
	popperBot.begin();

	partyPopperServo.attach(SERVO_3_PIN);
	partyPopperServo.write(PARTY_POPPER_MIN);
}

void loop() {
	popperBot.process_MIDI();
}

void self_test() {
	party_popper_fire();
}

void party_popper_fire() {
	partyPopperServo.write(PARTY_POPPER_MIN);
	digitalWrite(LED_PIN, HIGH);
	delay(1000);
	digitalWrite(LED_PIN, LOW);
	partyPopperServo.write(PARTY_POPPER_MAX);
	delay(1000);
	partyPopperServo.write(PARTY_POPPER_MID);
} 


void note_on(int note, int velocity) {
	switch (note) {
			case PARTY_POPPER_FIRE_NOTE: party_popper_fire(); break;
	}
}

void note_off(int note, int velocity) {
}
<file_sep>/Projects/2016/MIDIBots/arduino/multitasking_test/ano/src/sketch.ino
../../multitasking_test.ino<file_sep>/Tools/Arduiglue/Readme.txt
Some scripts for Arduino development without being so dependent on the (somewhat yucky) Arduino IDE.

inotool (`ino`) and its fork Arturo (`ano`) provide efficient command-line front-ends to the Arduino build tools (mainly gcc and its binutils friends plus avrdude).

I'd like to make these cross-platform (Linux, Mac OS, Windows, at least), support various front-end IDEs (Geany, Visual Studio, TextWrangler, etc.) and fall back to the Arduino IDE if necessary.


One basic script would be for taking an existing file and adjusting the path to match the Arduino IDE's requirements.
	Mac: ~/Documents/Arduino/$SKETCH/$SKETCH.ino
	Linux: 
	Windows: 

A function for determining the platform's sketchbook folder might be handy.
	On Windows, it might be a Registry entry.
	In general, it can be changed through the preferences.

	Mac default: ~/Documents/Arduino


A function for determining the location of the preferences file, e.g.
	Mac: ~/Library/Arduino15/preferences.txt

--

There are a number of hassles in getting the ino/ano folder structure to match the Arduino IDE's requirements.  Notably, the Arduino IDE will recursively try to build everything under the main folder for the sketch, so it looks like putting an ano-compatible subfolder under the main sketch folder is not an option.  I'd hoped that symlinks would allow us to keep both happy, but the Arduino IDE has problems either way:
	~/sketchbook/MySketch/MySketch.ino -> ~/sketchbook/MySketch/ano/src/sketch.ino
		Fails to build because Arduino IDE follows the symlink and ends up in the wrong dir (ano/src).
	~/sketchbook/MySketch/ano/src/sketch.ino -> ~/sketchbook/MySketch/MySketch.ino
		Fails to build in Arduino IDE because it tries to build the ano copy as well.

However, recent versions of ano support a --source-dir argument, allowing you to compile wherever the file happens to be, and some discussion on github indicates that it should now work with the Arduino IDE's folder structure.  That would make like much easier!

See:
https://github.com/scottdarch/Arturo/issues/7
<file_sep>/Projects/2016/MIDIBots/arduino/midi_keyboard_random/ano/src/sketch.ino
../../midi_keyboard_random.ino<file_sep>/Documentation/Assemblies.sql
create table Manufacturer
(
	manufacturer_name varchar primary key
);

create table Part
(
	part_name varchar primary key, 
	manufacturer_name varchar references Manufacturer,
	mass number,
	cost number
);

create table Assembly 
(
	assembly_name varchar primary key
);

create table Assembly_Part
(
	part_name varchar references Part,
	assembly_name varchar references Assembly,
	quantity number,
	primary key(part_name, assembly_name)
);
<file_sep>/Projects/2017/SoccerBots/bash_aliases
alias l='ls -hal'
alias frink="rlwrap java -cp /usr/share/java/frink.jar frink.parser.Frink"
alias jdiskreport='java -jar /usr/share/java/jdiskreport.jar'
alias lsserial='find /dev/serial/by-id -type l -print0 | xargs -0 -i ls -l {}'
alias re='./runeverything.sh'
alias se='./stopeverything.sh'
alias old_arduino="/usr/local/arduino-0105/arduino" 
alias shutter="ssh -Y pi@shutter"
alias boris="ssh -Y pi@boris"
<file_sep>/Projects/2016/MIDIBots/arduino/multitasking_test/multitasking_test.ino
#include "Timer.h"
#include <Servo.h> 

Timer *servo_0_timer = new Timer(300); //starts a new timer
Timer *servo_1_timer = new Timer(250); //starts a new timer

Servo servos [6];

int pos = 0;    // variable to store the servo position
const int CENTRE = 90;
const int RANGE = 46;
const int MAX_POS = CENTRE + RANGE/2;
const int MIN_POS = CENTRE - RANGE/2;

const int SERVO_TIME = 100; // ms delay to allow motion to happen


void setup() 
{ 
	
	servos[0].attach(3);
	servos[1].attach(5);
	servos[2].attach(6);
	servos[3].attach(9);
	servos[4].attach(10);
	servos[5].attach(11);

	servo_0_timer->setOnTimer(&up_strum); //sets new timer
	servo_1_timer->setOnTimer(&down_strum); //sets new timer
	servo_0_timer->Start(); //starts new timer
	servo_1_timer->Start(); //starts new timer
} 

void down_strum(){
	for (int i = 0; i < 6; i++)
		servos[i].write(MAX_POS);
	
}


void up_strum(){
	for (int i = 0; i < 6; i++)
		servos[i].write(MIN_POS);
	
}

void loop() {
	
	servo_1_timer->Update();
	servo_0_timer->Update();
}
<file_sep>/Projects/2016/MIDIBots/kicad/MIDI_Shield.pro
update=Mon 13 Jun 2016 21:31:57 NZST
last_client=kicad
[cvpcb]
version=1
NetIExt=net
[cvpcb/libraries]
EquName1=devcms
[pcbnew]
version=1
LastNetListRead=MIDI_Shield.net
UseCmpFile=1
PadDrill=0.600000000000
PadDrillOvalY=0.600000000000
PadSizeH=1.500000000000
PadSizeV=1.500000000000
PcbTextSizeV=1.500000000000
PcbTextSizeH=1.500000000000
PcbTextThickness=0.300000000000
ModuleTextSizeV=1.000000000000
ModuleTextSizeH=1.000000000000
ModuleTextSizeThickness=0.150000000000
SolderMaskClearance=0.000000000000
SolderMaskMinWidth=0.000000000000
DrawSegmentWidth=0.200000000000
BoardOutlineThickness=0.100000000000
ModuleOutlineThickness=0.150000000000
[pcbnew/libraries]
LibDir=
LibName1=sockets
LibName2=connect
LibName3=discret
LibName4=pin_array
LibName5=divers
LibName6=smd_capacitors
LibName7=smd_resistors
LibName8=smd_crystal&oscillator
LibName9=smd_dil
LibName10=smd_transistors
LibName11=libcms
LibName12=display
LibName13=led
LibName14=dip_sockets
LibName15=pga_sockets
LibName16=valves
LibName17=/home/cedwards/Documents/Settings/KiCAD/arduino-kicad-nicholasclewis/arduino_shields
[eeschema]
version=1
LibDir=/home/cedwards/Documents/Settings/KiCAD/freetronics_kicad_library;/home/cedwards/Documents/Settings/KiCAD/xess.pretty;/home/cedwards/Documents/Settings/KiCAD
NetFmtName=
RptD_X=0
RptD_Y=100
RptLab=1
LabSize=60
[eeschema/libraries]
LibName1=power
LibName2=device
LibName3=transistors
LibName4=conn
LibName5=linear
LibName6=regul
LibName7=74xx
LibName8=cmos4000
LibName9=adc-dac
LibName10=memory
LibName11=xilinx
LibName12=microcontrollers
LibName13=dsp
LibName14=microchip
LibName15=analog_switches
LibName16=motorola
LibName17=texas
LibName18=intel
LibName19=audio
LibName20=interface
LibName21=digital-audio
LibName22=philips
LibName23=display
LibName24=cypress
LibName25=siliconi
LibName26=opto
LibName27=atmel
LibName28=contrib
LibName29=valves
LibName30=lib/dips-s
LibName31=lib/arduino-kicad-nicholasclewis/arduino_shieldsNCL
LibName32=lib/freetronics_kicad_library/freetronics_schematic
[schematic_editor]
version=1
PageLayoutDescrFile=
PlotDirectoryName=
SubpartIdSeparator=0
SubpartFirstId=65
NetFmtName=
SpiceForceRefPrefix=0
SpiceUseNetNumbers=0
LabSize=60
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_drumbot/firmware_drumbot.ino
// DrumBot code for the 2016 MIDIBots
// Team _underscore_, Information Science Mechatronics, University of Otago

#include <MIDIBot.h>
MIDIBot drumBot;

#include "Timer.h"
#include <Servo.h> 

Servo
	bass_drum_servo,
	snare_drum_servo,
	cymbal_servo
;

const int BASS_DRUM_NOTE = 36;
const int BASS_DRUM_MIN = 30;
const int BASS_DRUM_MAX = 12;
const int BASS_DRUM_DELAY = 100;

const int SNARE_DRUM_NOTE = 38;
const int SNARE_DRUM_MIN = 40;
const int SNARE_DRUM_MAX = 62;
const int SNARE_DRUM_DELAY = 100;

const int CYMBAL_NOTE = 42;
const int CYMBAL_MIN = 75;
const int CYMBAL_MAX = 105;	
const int CYMBAL_DELAY = 100;

// Timers for asynchronous release of drum hits:

Timer *bass_drum_timer = new Timer(BASS_DRUM_DELAY, &bass_drum_release, 1);
Timer *snare_drum_timer = new Timer(SNARE_DRUM_DELAY, &snare_drum_release, 1);
Timer *cymbal_timer = new Timer(CYMBAL_DELAY, &cymbal_release, 1);

// Separate functions for hit and release for each drum:

void bass_drum_hit() {
	bass_drum_servo.write(BASS_DRUM_MAX);
	bass_drum_timer->Start();
	digitalWrite(MOSFET_PWM_PIN, HIGH);
}
void snare_drum_hit() {
	snare_drum_servo.write(SNARE_DRUM_MAX);
	snare_drum_timer->Start();
}
void cymbal_hit() {
	cymbal_servo.write(CYMBAL_MAX);
	cymbal_timer->Start();
}

void bass_drum_release() {bass_drum_servo.write(BASS_DRUM_MIN); digitalWrite(MOSFET_PWM_PIN, LOW);}
void snare_drum_release() {snare_drum_servo.write(SNARE_DRUM_MIN);}
void cymbal_release() {cymbal_servo.write(CYMBAL_MIN);}


void self_test() {
	digitalWrite(LED_PIN, HIGH);
	bass_drum_hit();
	snare_drum_hit();
	cymbal_hit();
	digitalWrite(LED_PIN, LOW);
}

void note_on(int note, int velocity) {
	switch (note) {
		case BASS_DRUM_NOTE:  bass_drum_hit();  break;
		case CYMBAL_NOTE:     cymbal_hit();     break;
		case SNARE_DRUM_NOTE: snare_drum_hit(); break;
	}
}

void note_off(int note, int velocity) {
	//nothing for drumBot.
}

void setup()
{
	// Servo setup:
	bass_drum_servo.attach(SERVO_1_PIN);
	snare_drum_servo.attach(SERVO_2_PIN);
	cymbal_servo.attach(SERVO_3_PIN);

	// Move motors to starting positions
	bass_drum_servo.write(BASS_DRUM_MIN);
	snare_drum_servo.write(SNARE_DRUM_MIN);
	cymbal_servo.write(CYMBAL_MIN);

	drumBot.begin();
}

void loop()
{
	bass_drum_timer->Update();
	snare_drum_timer->Update();
	cymbal_timer->Update();
	
	drumBot.process_MIDI();
}
<file_sep>/Projects/2017/SoccerBots/arduino/motor_control/motor_control.ino
//Currently working Motor Control using the DSpace robot board for RoboCup Soccer 2017.

// MOST IMPORTANT!!! Make sure this is set correctly before uploading:
#define SHUTTER 0
// (it might be nice to have the robot's identity stored in EEPROM and detected automatically by this program)


// Stuff below here shouldn't need to change very often.

#define BAUD_RATE 19200

// TODO: Do the duty cycles of the SoccerBot motors need adjusting?
// TODO: Move the duty cycles into the eeprom
// TODO: Remove unused stuff copied from the Dalek program.

#include <Servo.h> //incldued for kicker

Servo Kicker;
const int SERVO_PIN = 13; //Servo 2, Pin 2 SDK.
const int KICKER_DELAY = 1000;

// For PWM limiting output power to 4.5 V equivalent from an 8 V supply:
// duty = 4.5 V / (8 V) x 255 = 143

#if (SHUTTER==1)
// Motor speed and servo limits tweaked for Shutter (attacker):
const int MOTOR_L_DUTY=123;
const int MOTOR_R_DUTY=130;
const int KICKER_MIN = 105;  //tested
const int KICKER_MAX = 145;
#else
// Motor speed and servo limits tweaked for Boris (goalie):
const int MOTOR_L_DUTY=134;
const int MOTOR_R_DUTY=143;
const int KICKER_MAX = 125;  //tested
const int KICKER_MIN = 85;   //tested
#endif

const int MESSAGE_TYPE_MASK	= 0b10000000;
const int MOTOR_MASK 		= 0b01000000;
const int DIR_MASK 			= 0b00100000;
const int SPEED_MASK	 	= 0b00011111;
const int KICKER_MASK 		= 0b00000001;
const int MOTOR_TOGGLE_SWITCH = 18; //physical pin 2 on sensor block 2.

int motors_enabled = 0;

#define MOTOR_R_ENABLE 5
#define MOTOR_L_ENABLE 11

#define MOTOR_R_1_PIN 10
#define MOTOR_R_2_PIN 9

#define MOTOR_L_1_PIN 7
#define MOTOR_L_2_PIN 6


#define DEBUGGING 0
/*
#define debug(message) \
	do { if (DEBUGGING) Serial.println(message); } while (0)
*/
#if DEBUGGING == 1 //Do we need to print some values all the time? Pure data isn't expecting anything in return to the motor driving protocol.
	#define DEBUG(x) Serial.println (x)
	#define DEBUG_NOEOL(x) Serial.print (x)
#else
	#define DEBUG(x)
	#define DEBUG_NOEOL(x)
#endif


void setup() {
	Kicker.attach(SERVO_PIN);
	pinMode(MOTOR_TOGGLE_SWITCH, INPUT); digitalWrite(MOTOR_TOGGLE_SWITCH, 1);

	pinMode(MOTOR_L_ENABLE, OUTPUT); digitalWrite(MOTOR_L_ENABLE, LOW);
	pinMode(MOTOR_R_ENABLE, OUTPUT); digitalWrite(MOTOR_R_ENABLE, LOW);

	pinMode(MOTOR_L_1_PIN, OUTPUT); digitalWrite(MOTOR_L_1_PIN, LOW);
	pinMode(MOTOR_L_2_PIN, OUTPUT); digitalWrite(MOTOR_L_2_PIN, HIGH);

	pinMode(MOTOR_R_1_PIN, OUTPUT); digitalWrite(MOTOR_R_1_PIN, LOW);
	pinMode(MOTOR_R_2_PIN, OUTPUT); digitalWrite(MOTOR_R_2_PIN, HIGH);

	Serial.begin(BAUD_RATE);

	#ifdef DEBUGGING
	DEBUG("\n\nDspace Motor Controller for SoccerBots - Info Sci Mechatronics v0.01");
	#endif

	// Flap kicker paddle to signal startup (even if motors disabled):
	Kicker.write(KICKER_MAX); delay(100);
	Kicker.write(KICKER_MIN); delay(100);
	Kicker.write(KICKER_MAX); delay(100);
	Kicker.write(KICKER_MIN); delay(100);
	Kicker.write(KICKER_MAX); delay(100);
	Kicker.write(KICKER_MIN);
}

void Stop() {
	digitalWrite(MOTOR_L_1_PIN, LOW);
	digitalWrite(MOTOR_L_2_PIN, HIGH);
	analogWrite(MOTOR_L_ENABLE, 0);
	digitalWrite(MOTOR_R_1_PIN, LOW);
	digitalWrite(MOTOR_R_2_PIN, HIGH);
	analogWrite(MOTOR_R_ENABLE, 0);
}

//speed argument expected to be between 0-255
void L_Spd(int speed, bool dir) {
	if (!motors_enabled) {speed = 0;}
	if (speed < 0) {speed = 0;}
	if (speed > 255) {speed = 255;}
	digitalWrite(MOTOR_L_1_PIN, dir);
	digitalWrite(MOTOR_L_2_PIN, !dir);
	analogWrite(MOTOR_L_ENABLE, (int) round(speed/255.0 * MOTOR_L_DUTY));
}

void R_Spd(int speed, bool dir) {
	if (!motors_enabled) {speed = 0;}
	if (speed < 0) {speed = 0;}
	if (speed > 255) {speed = 255;}
	digitalWrite(MOTOR_R_1_PIN, dir);
	digitalWrite(MOTOR_R_2_PIN, !dir);
	analogWrite(MOTOR_R_ENABLE, (int) round(speed/255.0 * MOTOR_R_DUTY));
}

void kick(){ //this is unused, but it could be useful so it can stay. 
	Kicker.write(KICKER_MAX);
	delay(KICKER_DELAY);
	Kicker.write(KICKER_MIN);
	delay(KICKER_DELAY);
}

void kicker_move(int direction) {
	if (!motors_enabled) {
		Kicker.write(KICKER_MIN); 
	} else{
		Kicker.write(direction ? KICKER_MAX : KICKER_MIN);
	}
}

void motor_control(){
	if (Serial.available() > 0) {
		int data_int = Serial.read();
		if (data_int < 0) {
			return;
		}
		byte data = byte(data_int);
		if ((data&MESSAGE_TYPE_MASK)>>7==0){
			kicker_move(data&KICKER_MASK);
		} else {
			if ((data&MOTOR_MASK)>>6) {
				R_Spd((data&SPEED_MASK)<<3, (data&DIR_MASK)>>5);
				DEBUG("R forward (DIR, Speed): ");
				DEBUG((data&DIR_MASK)>>5);
				DEBUG((data&SPEED_MASK)<<3);
			} else {
				L_Spd((data&SPEED_MASK)<<3, (data&DIR_MASK)>>5);
				DEBUG("L forward (DIR, Speed): ");
				DEBUG((data&DIR_MASK)>>5);
				DEBUG((data&SPEED_MASK)<<3);
			}
		}
	} else {
		if (!motors_enabled) {Stop();}
	}
}

void loop(){
	motors_enabled = !digitalRead(MOTOR_TOGGLE_SWITCH);
	motor_control();
}

<file_sep>/Projects/2017/SoccerBots/old_calibrate_vision.py
#!/usr/bin/env python2.7
import SimpleCV
import sys
import os
sys.path.append(os.environ['HOME'] + '/robotics_repo/Documentation/Computer Vision/infoscimechatronics')
import cvutils

calibration_file_name = "camera_calibration_state.py"
camera = SimpleCV.Camera(0, {"width":320,"height":240})
os.system(os.environ['HOME'] + '/robotics_repo/Projects/2017/SoccerBots/rpicam-setup.sh')

grey_sample = cvutils.calibrate_white_balance(camera)
blue_sample = cvutils.calibrate_colour_match(camera, grey_sample)

#grey_sample = (221.5625, 219.75, 225.8125)
#blue_sample = ((108.0, 2.0), (159.0, 30.0), (90.5, 26.5))


def file_output():
	output_file = open(calibration_file_name, "w")
	output_file.write("calibrated_grey_sample = " + str(grey_sample) + "\n")
	output_file.write("calibrated_goal_sample = " + str(blue_sample) + "\n")
	output_file.close()
	
file_output()

<file_sep>/Projects/2017/SoccerBots/boris_python2.py
import serial
import time
import sys
from threading import Thread

serinput = serial.Serial('/dev/serial/by-id/usb-Arduino__www.arduino.cc__0042_852313632363516031B2-if00', 115200)
seroutput = serial.Serial('/dev/serial/by-id/usb-DSpace__www.dspace.org.nz__DSpace_Robot_2.0_55234313335351A0D161-if00', 19200)
left_motor_string = 0b10000000 #first bit 1 means motor message
right_motor_string = 0b11000000 #second bit 1 means right motor
kicker_mask = 0b00000000 #first bit -  0 means kicker message
speed_threshold = 0.4 #deadzone threshold
intercept_time = 1 #time for intercepting the ball

distance_threshold = 0.8

ball_near = 0
homed = 0
inside_goal_zone = 0
goal_detected = 0
ball_detected = 0
ball_angle = 0
ball_distance = 0
hall_effect = 0
rangee = 0
compass_heading = 0
calibration_mode = 0
colour = 0
voltage = 0
angle_to_goal = 0
blob_size = 0

def read_sensors(): #function which constantly reads sensors
	while True:
		serinput.flush()
		line = str(serinput.readline()).strip(' ;\r\n ').split(" ")
		#print(len(line))
		#print(line)
		global ball_near, homed, inside_goal_zone, goal_detected, ball_detected, ball_angle, ball_distance, hall_effect, rangee, compass_heading, calibration_mode, colour, voltage, angle_to_goal, blob_size
		try:
			ball_detected = line[0]
			ball_angle = float(line[1])
			ball_distance = float(line[2])
			hall_effect = line[3]
			rangee = float(line[4])
			compass_heading = float(line[5])
			calibration_mode = line[6]
			colour = float(line[7])
			voltage = float(line[8])
			time.sleep(0.1)
		except(serial.SerialException, ValueError, IndexError) as error:
			#time.sleep(1)
			#print(error)
			pass
			
		#calculate and set ball side variable
		if ball_angle<900:
			if compass_heading-ball_angle<-180:
				temp_ball_side = compass_heading-ball_angle+360
			else:
				temp_ball_side = compass_heading-ball_angle
			if abs(temp_ball_side)<90:
				ball_side = 1
			else:
				ball_side = 0
		
		#print("voltage: " + str(voltage))
		#read angle to goal and blob size from stdin		
		stdin_line = sys.stdin.readline().strip(";").split()
		
		try:
			angle_to_goal = float(stdin_line[0].strip(";"))
		except(IndexError) as error:
			#print(error)
			pass
		try:
			blob_size = stdin_line[1]
		except(IndexError) as error:
			#print(error)
			pass
		
		#print("angle to goal: " + angle_to_goal)
		
		#decide how close the ball is
		#TODO: see if battery voltage affects the ball distance
		if ball_distance < distance_threshold:
			ball_near = True
		else:
			ball_near = False
			
		
		#calculate whether the goal is detected
		if angle_to_goal != -180:
			goal_detected = 1
		else:
			goal_detected = 0
			
		#calculate whether we are inside the goal zone
		if colour > 800:
			inside_goal_zone = True
		else:
			inside_goal_zone = False
		
		
		#calculate whether we are homed
		if (inside_goal_zone==1 and goal_detected==1 and rangee<25 and abs(compass_heading)<90):
			homed = True
		else:
			homed = False
				
		#print("homed in thread " + str(homed))
		#print("gd: " + str(goal_detected) + " in gz: " +  str(inside_goal_zone) + " range: " + str(rangee) + " cmps: " + str(compass_heading))
		
		serinput.flushInput()
			
reading_of_sensors = Thread(target=read_sensors)
reading_of_sensors.start() #start thread to read sensors constantly


def clip(x): #clip x within the range -1..1
	if x<-1:
		return -1
	elif x>1:
		return 1
	else:
		return x
		
def homing():
	global homed
	#print("homing: " + str(homed))
	while homed == False:
		#print(angle_to_goal)
		if angle_to_goal < 10 and angle_to_goal > -10:
			motor_output(-0.5, -0.5)
		if angle_to_goal < -11:
			motor_output(-0.3, -0.7)
		elif angle_to_goal > 11:
			motor_output(-0.7, -0.3)
			
		
def deadzone(speed, threshold):
    if speed>0:
        out = speed + threshold
    elif speed<0:
        out = speed - threshold
    elif speed==0:
		out = 0
    return clip(out)
        
def speed_to_bin(speed): #convert speed to binary number
	speed = deadzone(speed, speed_threshold)
	if speed>=0:
		dir_bit_string = 0b00100000
	else:
		dir_bit_string = 0b00000000
		
	speedbits = (int(abs(speed)*255))>>3
	return dir_bit_string | speedbits 
	
def left_motor(speed):
	return speed_to_bin(speed) | left_motor_string

def right_motor(speed):
	return speed_to_bin(speed) | right_motor_string		


def send_byte(i):
	# For debugging:
	#print(bin(i))
	# Output for real:
	seroutput.write(chr(i))
		
def motor_output(left_speed, right_speed): #drive both motors with left_speed and right_speed as the left and right motor speeds
	send_byte(int(left_motor(left_speed*(voltage*-0.5 + 5))))
	send_byte(int(right_motor(right_speed*(voltage*-0.5 + 5))))
	
	
def turn_to_ball():
	#read_sensors()
	#print(ball_angle)
	while ball_angle != 0:
		if ball_angle<0:	
			#print("turn left")
			motor_output(-0.5,0.5)
		elif ball_angle>0:
			#print("turn right")
			motor_output(0.5,-0.5)
		else:
			#print("0!")
			motor_output(0,0)

def intercept_ball():
	motor_output(1,1)	
	time.sleep(intercept_time)
	motor_output(-0.75,-0.75)
	time.sleep(intercept_time)
	motor_output(0,0)

while True:	
	"""if ball_near == False:
		print("turning to ball")
		turn_to_ball()
		motor_output(0,0)
	elif ball_near == True:
		print("intercepting ball")
		intercept_ball()
		
	if homed == True:
		print("homed")
		pass
	elif homed == False:
		print(rangee)
		print(colour)
		print("homing")
		homing()"""
#	homing()
	motor_output(0,0)
	motor_output(-0.8,-0.2)
	time.sleep(1)
	motor_output(0,0)
	motor_output(-0.2,-0.8)
	time.sleep(1)
			
	
<file_sep>/Projects/2016/MIDIBots/arduino/midi_keyboard/midi_keyboard.ino
// A simple MIDI keyboard for Arduino
// CME 2016-03

// This polls for input switch states and outputs MIDI note on/off messages when they change, i.e. when a switch is pressed/released.
// We reserve a block of contiguous input pins to keep the code simple and scalable.

// To monitor MIDI messages:
// aseqdump -p 20:0

// To play via Yoshimi:
// yoshimi --alsa-midi=20:0 --alsa-audio=pulse --load-instrument=/usr/share/yoshimi/banks/Will_Godfrey_Collection/0033-Medium\ Saw.xiz


// Here we define a couple of arrays for the current and previous state of all the key switches:
// Notably, we need to keep the TX pin free for MIDI transmission. Also the LED_BUILTIN pin so we can indicate activity.
// NOTE: on the Arduino Mega, the big double-row of pins starts has a pair of 5V pins, then digital pins 22 and 23.  The last digital pin is 53, and then there are two grounds at the end.

const int NUM_KEYS = 32;
const int FIRST_KEY_PIN = 22;

// Here are the arrays.
// NOTE: awkward mismatch of pin numbers and array indexes, since we're not necessarily starting at 0!  It's a bit wasteful of memory, but let's just make the array bigger, and ignore any we aren't using, and use the same numbering for the array elements as for the pin numbers for the input switches.

int currSwitchState[NUM_KEYS + FIRST_KEY_PIN];
int prevSwitchState[NUM_KEYS + FIRST_KEY_PIN];


void setup() {
	//  Set up serial port for MIDI baud rate (31.25 kbit/s):
	Serial.begin(31250);
	
	// We might want to use the LED to indicate activity (though don't use delay() unnecessarily (e.g. blink())).
	pinMode(LED_BUILTIN, OUTPUT);
	digitalWrite(LED_BUILTIN, LOW);

	// Set up input pins and initialise switch state arrays:
	for (int pin = FIRST_KEY_PIN; pin <= FIRST_KEY_PIN + NUM_KEYS - 1; pin++) {
		pinMode(pin, INPUT_PULLUP);
		currSwitchState[pin] = 0;
		prevSwitchState[pin] = 0;
	}

}

void loop() {
	readSwitches();
}

void ledOn() {digitalWrite(LED_BUILTIN, HIGH);}

void ledOff() {digitalWrite(LED_BUILTIN, LOW);}

// This function is used to conveniently generate note on/off MIDI messages.
// The first message byte is called the status byte, and indicates the type of message and the channel number being addressed.
// Status bytes have a 1 for the MSB, and data bytes always have a 0 there.
// 0x9 (0b1001) is the Note On status nybble.
// NOTE: the channel number parameter should be the logical (user-centric) channel numbers (1..16), not the internal values used to represent them.

void playNote(int channel, int pitch, int velocity) {
	if (channel > 16 || channel < 1) channel = 1;	// Clip to valid range
	Serial.write(0x90 | (channel-1));	// write status byte, converting channel number to wire format
	Serial.write(pitch);			// data byte 1
	Serial.write(velocity);			// data byte 2
}


// Trigger a MIDI note based input switch activity.  We'll use polling for this (there wouldn't be enough interrupts available for an interrupt-based approach). As a result, we need to remember the state of the switch(es) from the last poll so we can recognise on/off transitions.

void readSwitches() {
	// Check each of the pins:
	for (int pin = FIRST_KEY_PIN; pin <= FIRST_KEY_PIN + NUM_KEYS - 1; pin++) {
		currSwitchState[pin] = !digitalRead(pin);	// Active-low; negate here.
		// If state changed, output note on/off:
		if (currSwitchState[pin] != prevSwitchState[pin]) {
			ledOn();
			playNote(1, 36 + pin, currSwitchState[pin] * 100);	// Note on/off depending on whether the key is pressed.
			ledOff();
		}
		prevSwitchState[pin] = currSwitchState[pin];	// Store the state for next time.
	}
	delay(1); // Hold off just a little do reduce spurious note on/off due to switch contact bouncing.
}

<file_sep>/Projects/2017/SoccerBots/old_blob_tracking.py
#!/usr/bin/env python2.7


import SimpleCV
import os
import sys
sys.path.append(os.environ['HOME'] + '/robotics_repo/Documentation/Computer Vision/infoscimechatronics')
import cvutils
import time
import serial
execfile("camera_calibration_state.py")

camera = SimpleCV.Camera(0, {"width":320,"height":240})
os.system(os.environ['HOME'] + '/robotics_repo/Projects/2017/SoccerBots/rpicam-setup.sh')

speed = 0
prev_error = 0
integral = 0
derivative = 0
hunt_dir = 1 
hunt_step = 15
#blobs_threshold = 0.0075
blobs_threshold = 0.00005
times = []
output = False
then = 0
now = 0	
scaling_factor = 0.250 #0.075


def average(numbers):
	x = 0
	for num in numbers:
		x += num
	x = float(x) / len(numbers)
	return x

def x_coordinate_to_angle(coord): 
	#return coord*35.543 #calibrated for LogiTech Camera.
	return coord*33.0 #calibrated for RPi Camera. 

def clip_angle(angle):
	if angle < -90:
		return -90
	if angle > 90:
		return 90
	else:
		return angle

def send2pd(message):
	print(str(message) + ";")
	sys.stdout.flush()

while True:
	now = time.time()
	elapsed_time = now  - then
	sys.stderr.write('Frame rate:' + str(1.0/elapsed_time) + '\n')
	#times.append(elapsed_time)
	#frame_rate = 1 / average(times) not calcultating the correct result
	
	image = cvutils.wb(camera.getImage().scale(scaling_factor), calibrated_grey_sample)
	v,s,h = image.toHSV().splitChannels()
	hue_match = h.colorDistance((calibrated_goal_sample[0][0],calibrated_goal_sample[0][0],calibrated_goal_sample[0][0])).binarize(calibrated_goal_sample[0][1]*3)
	sat_match = s.colorDistance((calibrated_goal_sample[1][0],calibrated_goal_sample[1][0],calibrated_goal_sample[1][0])).binarize(calibrated_goal_sample[1][1]*3)
	matched = ((hue_match / 16) * (sat_match / 16))
	matched.show()
	blobs = matched.findBlobs(threshval=100, minsize=1, maxsize=0, threshblocksize=0, threshconstant=5)
	if blobs is not None:
		blob_size = blobs[-1].area()
		image_size = image.area()
		sys.stderr.write ("Blob size / image size: " + str(blob_size / image_size) + "\n")
		if blob_size / image_size < blobs_threshold:
			sys.stderr.write("Blobs too small! \n")
			send2pd(-180) #404 not found
			time.sleep(0.02)
		else:
			(x,y) = blobs[-1].centroid()
			image.dl().line((x,0), (x,image.height), (255,0,0), antialias=False)
			image.dl().line((0,y), (image.width,y), (0,255,0), antialias=False)
			image.show()
			#sys.stderr.write float(x) / image.width
			converted_coord = float(x) / image.width
			converted_coord = x_coordinate_to_angle(converted_coord*2-1)
			sys.stderr.write("converted_coord: " + str(converted_coord) + ' ' + '\n')
			send2pd(converted_coord)
	else:
		sys.stderr.write("No blobs found! \n")

		send2pd(-180)
		time.sleep(0.1)
		
		
	time.sleep(0.03)
	then = now
	
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_percussionbot/ano/src/sketch.ino
../../firmware_percussionbot.ino<file_sep>/Projects/2016/MIDIBots/arduino/libraries/MIDIBot/MIDIBot.cpp
/*
 * Arduino library for our MIDIBot shield circuit board, Robocup 2016
 * Team _underscore_, Information Science Mechatronics, University of Otago
*/

#include "Arduino.h"
#include "MIDIBot.h"

// Constructor method:
// This performs most of the low-level setup.  Much of this will be standard, but some firmware-specific setup code will also be likely.  That can be done in the usual setup() in the main sketch.
// Some setup functions cannot be done in the constructor (notably Serial.begin() and anything that uses delay()).  Thus there is a separate begin() method that should be called from the main sketch during setup().
// self_test() will need to be implemented by the sketch using this library.
// Ditto note_on(), note_off().

MIDIBot::MIDIBot()
{

	// Set up pin modes for the MIDIBot Shield (input, internal pull-up resistor enabled):
	pinMode(MIDI_1x_PIN, INPUT_PULLUP);
	pinMode(MIDI_2x_PIN, INPUT_PULLUP);
	pinMode(MIDI_4x_PIN, INPUT_PULLUP);
	pinMode(MIDI_8x_PIN, INPUT_PULLUP);
	
	// Ditto for self-test button:
	pinMode(SELF_TEST_PIN, INPUT_PULLUP);
	
	// TODO: output pins (servos, MOSFETs, LED)
	pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, LOW);
	pinMode(MOSFET_PWM_PIN, OUTPUT); analogWrite(MOSFET_PWM_PIN, 0);
	pinMode(MOSFET_2_PIN, OUTPUT); digitalWrite(MOSFET_2_PIN, LOW);
	pinMode(MOSFET_3_PIN, OUTPUT); digitalWrite(MOSFET_3_PIN, LOW);
	pinMode(MOSFET_4_PIN, OUTPUT); digitalWrite(MOSFET_4_PIN, LOW);	
	pinMode(SERVO_1_PIN, OUTPUT); digitalWrite(SERVO_1_PIN, LOW);
	pinMode(SERVO_2_PIN, OUTPUT); digitalWrite(SERVO_2_PIN, LOW);
	pinMode(SERVO_3_PIN, OUTPUT); digitalWrite(SERVO_3_PIN, LOW);
	
	// Initialise MIDI channel number according to DIP switch settings:
	read_MIDI_channel();
//	flash_number(_MIDI_channel + 1);	// This seems to be the cause of the problems with using this as a library!  I suspect it's because it uses delay() internally.
	
	// NOTE: MIDI serial communication must be set up elsewhere, in the ::begin() method.  If it's done here, the program won't hang, but serial communication will not work!
	// Also note that use of the delay() function in this constructor will cause the program to hang!

	clearData();
}

void MIDIBot::begin(){
	delay(250);
	test_MIDI_channel();
	delay(1500);
	// self_test();
	flash(100, 100);
	Serial.begin(SERIAL_SPEED);
}

// erase the MIDI data buffer
void MIDIBot::clearData(){
	_dataByte[0] = 0;
	_dataByte[1] = 0;
	_i = 0;
}


void MIDIBot::flash(int on_time, int off_time) {
	digitalWrite(LED_PIN, HIGH);
	delay(on_time);
	digitalWrite(LED_PIN, LOW);
	delay(off_time);	
}


// Routine for displaying a number by blinking the LED (since hardware serial will be taken already by the MIDI comms):
void MIDIBot::flash_number(int n) {
	delay(500);
	// Hundreds:
	for (int i = 0; i < (n % 1000 / 100); i++) {
		flash(800, 800);
	}
	delay(500);
	// Tens:
	for (int i = 0; i < (n % 100 / 10); i++) {
		flash(400, 400);
	}
	delay(500);
	// Fastest flashing for the ones:
	for (int i = 0; i < (n % 10); i++) {
		flash(200, 200);
	}
	delay(1000);
}


// Read MIDI channel DIP switches and store the result:
// NOTE: Freetronics USBDroid has a pull-down resistor on pin D4, which makes that pin unusable! It acts as if D4 is always grounded, i.e. the switch is on.  Multiply that bit by 0 (instead of 4) here to force it off.
void MIDIBot::read_MIDI_channel() {
	_MIDI_channel =
		!digitalRead(MIDI_8x_PIN) * 8 +
		!digitalRead(MIDI_4x_PIN) * 4 +
		!digitalRead(MIDI_2x_PIN) * 2 +
		!digitalRead(MIDI_1x_PIN);
}


// The process_MIDI() function performs receiving incoming MIDI data via serial, recognising note-on and note-off messages (and, perhaps later on, things like Control Change messages), and calls the note_on(pitch, velocity), note_off(pitch, velocity) functions defined by the calling sketch.  It should be called repeatedly by the loop() function in the main sketch.
// It also handles calling the self_test() function (defined by the sketch using this library) if the self-test button is pressed.  That's done here so that no extra code is required in the calling sketch for this to work.
// TODO: Perhaps define constants, such as NOTE_ON=0x90 (or 0x9)?
// TODO: copy _dataByte[0..1] to meaningfully-named variables (pitch/note, velocity) for better readability? Though it's not really necessary, and possibly inefficient.
void MIDIBot::process_MIDI() {
	
	if (!digitalRead(SELF_TEST_PIN)) {
		self_test();
	}

	if (Serial.available() > 0) {
		// flash(20,0); // Consider enabling this for debugging. Warning: blocks!
		int data = Serial.read();
		if (data > 127) {
			// It's a status byte. Store it for future reference.
			_statusByte = data;
			clearData();  
		} else {
			// It's a data byte.
			_dataByte[_i] = data;
			if (_statusByte == (0x90 | _MIDI_channel) && _i == 1) {
				// Note-on message received
				if (_dataByte[1] == 0) {
					// Note-on with velocity=0: equialent to note-off, so stop note playing (nothing to do for percussion!)
					note_off(_dataByte[0], _dataByte[1]);
				} else {
					// Start note playing
					note_on(_dataByte[0], _dataByte[1]);
				}
			} else if (_statusByte == (0x80 | _MIDI_channel) && _i == 1) {
				// Note-off message received
				note_off(_dataByte[0], _dataByte[1]);
			}
			_i++;
			// TODO: error detection if _i goes beyond the array size.
		}
	}
}


// Some standard self-test routines:

void MIDIBot::test_blink() {
	digitalWrite(LED_PIN, HIGH);
	delay(500);
	digitalWrite(LED_PIN, LOW);
	delay(500);
}


void MIDIBot::test_button() {
	digitalWrite(LED_PIN, !digitalRead(SELF_TEST_PIN));
}


void MIDIBot::test_MIDI_channel() {
	read_MIDI_channel();
	flash_number(_MIDI_channel + 1);
}


void test_MOSFETs() {
	if (!digitalRead(SELF_TEST_PIN)) {
		analogWrite(MOSFET_PWM_PIN, 64);
		digitalWrite(MOSFET_2_PIN, HIGH);
		digitalWrite(MOSFET_3_PIN, HIGH);
		digitalWrite(MOSFET_4_PIN, HIGH);
	} else {
		analogWrite(MOSFET_PWM_PIN, 0);
		digitalWrite(MOSFET_2_PIN, LOW);
		digitalWrite(MOSFET_3_PIN, LOW);
		digitalWrite(MOSFET_4_PIN, LOW);
	}
}


void test_MOSFETs_cycle() {
	analogWrite(MOSFET_PWM_PIN, 64);
	delay(250);
	analogWrite(MOSFET_PWM_PIN, 0);
	
	digitalWrite(MOSFET_2_PIN, HIGH);
	delay(250);
	digitalWrite(MOSFET_2_PIN, LOW);
	
	digitalWrite(MOSFET_3_PIN, HIGH);
	delay(250);
	digitalWrite(MOSFET_3_PIN, LOW);

	digitalWrite(MOSFET_4_PIN, HIGH);
	delay(250);
	digitalWrite(MOSFET_4_PIN, LOW);
}


void test_PWM() {
	analogWrite(MOSFET_PWM_PIN, 64);
	delay(250);
	analogWrite(MOSFET_PWM_PIN, 0);
	delay(500);
}
<file_sep>/Projects/2016/MIDIBots/arduino/firmware_template/firmware_template.ino
// ____Bot code for the 2016 MIDIBots
// Team _underscore_, Information Science Mechatronics, University of Otago

#include <MIDIBot.h>
MIDIBot thisMIDIBot;

void note_on(int note, int velocity) {
	// TODO...
}

void note_off(int note, int velocity) {
	// TODO...
}

void self_test() {
//	thisMIDIBot.test_MIDI_channel();
	// TODO: Bot-specific self-test routine...
}

void setup() {
	thisMIDIBot.test_MIDI_channel();	// Indicate MIDI channel at startup
}

void loop() {
	if (!digitalRead(SELF_TEST_PIN)) {
		self_test();
	}
	thisMIDIBot.process_MIDI();
}
<file_sep>/Projects/2016/MIDIBots/arduino/midi_drum/ano/src/sketch.ino
../../midi_drum.ino<file_sep>/Projects/2016/MIDIBots/arduino/firmware_ukebot/ano/src/sketch.ino
../../firmware_ukebot.ino<file_sep>/Documentation/Computer Vision/cvutils2.py
# A collection of handy functions built on SimpleCV for our robotics projects

# Do we need to import SimpleCV?  I assume the import statement will ignore/fail gracefully if asked to import something that's already been imported...
import SimpleCV
import numpy

# Basic setup and check:
#camera = SimpleCV.Camera(prop_set={'width':320, 'height':240})
#camera.loadCalibration('default')
#camera.getCameraMatrix()
#camera.live()

# NOTE: it may be necessary to set many of the camera's capture settings externally, using guvcview or uvcdynctrl
# uvcdynctrl -c -v

# Some colour space conversion functions:

def hue_from_angle(degrees): return degrees / 360.0 * 255
def sat_from_percent(percent): return percent / 100.0 * 255
# Actually doesn't give the right numbers?!  TODO: test and correct...


# Generate a solid colour image, matching the resolution and format of the supplied image:
# (I couldn't see any way to create a new blank image using Image)

def solid(image, colour):
	solid = image.copy()
	solid[0:solid.width, 0:solid.height] = colour
	return solid

# Various also-rans follow...

#def solid(image, colour):
#	solid=image.copy()
#	solid.drawRectangle(0,0,solid.width,solid.height, color=colour)
#	return solid
# No! drawRectangle draws only the border, not a solid area!

# Oh, wait, you pass Image() an (x,y) dimension tuple when creating it.
#def solid_(template_image, colour):
#	solid = Image(template_image.width,template_image.height)
#	solid[0:solid.width, 0:solid.height] = colour
#	return solid
# Bah, problems with numpy array format...

#def solid__(width, height, colour):
#	solid = Image(width,height)
#	solid[0:width, 0:height] = colour
#	return solid

# or would it be better to pass (width,height) as a tuple in our function too?

# e.g.
# image = camera.getImage()
# red = solid(image, (255,0,0))



# Pixel function for highlighting pixels (e.g. to assist in troubleshooting image processing)
# Actually, applying Python-based pixel functions is ridiculously slow, so maybe this is no use...
# Also, scaling the image first seems to bork things.  Maybe different pixel format?  Oh, well..

def highlight(pixels):
	r=pixels[0]
	g=pixels[1]
	b=pixels[2]
	return (int(255*pow(r/255.0, 0.25)), int(255*pow(g/255.0, 0.25)), int(255*pow(b/255.0, 0.25)))



# The following code relates to white balance correction (it will often be preferable to set the camera to a fixed white-balance, and correct for it in software in a more controlled and uniform fashion).

# This function corrects an image's white balance based on a sample "grey" point:
# (The calibrate_white_balance() function below can be used to sample a "supposed-to-be-grey" pixel by clicking with the mouse)

def wb(image,grey_sample):
	grey=numpy.mean(grey_sample)
	r_corr = grey / grey_sample[0]
	g_corr = grey / grey_sample[1]
	b_corr = grey / grey_sample[2]
	#print('r_corr=' + str(r_corr) + ', g_corr=' + str(g_corr) + ', b_corr=' + str(b_corr))
	r,g,b=image.splitChannels()
	r = r * float(r_corr)
	g = g * float(g_corr)
	b = b * float(b_corr)
	return SimpleCV.ImageClass.Image.mergeChannels(r,r,g,b)

# Test it:
#camera.live()
## coord: (225,264), color: (158.0, 164.0, 184.0)
#x=225
#y=264
#grey_sample=(158.0, 164.0, 184.0)
#image=camera.getImage()
#image.getPixel(x,y) # Um, row/column transposition?!
#corrected=wb(camera.getImage(), grey_sample)
#corrected.getPixel(x,y)

## How fast (slow!) is it?:

##while True:
#	wb(camera.getImage(), grey_sample).show()

## And with the undistortion?
#while True:
#	wb(camera.getImageUndistort(), grey_sample).show()

## And at reduced resolution?
#while True:
#	wb(camera.getImageUndistort().scale(0.25), grey_sample).show()



# Helper function for setting the white balance manually, based on a series of samples from the camera.

# NOTE: Display.mouseLeft returns the current state of the button, whereas Display.leftButtonDownPosition() returns None unless the button was only JUST pressed. We want Display.mouseX and Display.mouseY instead, I think.

def calibrate_white_balance(camera):
	display=SimpleCV.Display()
	#camera=Camera()
	prev_mouse_state = False
	sample_pixels = []
	print('Press and hold left mouse button over a region that should be white/grey...')
	while display.isNotDone():
		image = camera.getImage()
		image.save(display)
		if display.mouseLeft:
			x = display.mouseX
			y = display.mouseY
			if not prev_mouse_state:
				# Button just pressed - initialise new list:
				sample_pixels = []
				#print(sample_pixels)
			colour_sample = image.getPixel(x,y)
			# Indicate the point being sampled:
			image.drawCircle((x,y),rad=5,color=SimpleCV.Color.VIOLET,thickness=1)
			image.save(display)
			print(str(x) + ', ' + str(y) + ': ' + str(colour_sample))
			sample_pixels.append(colour_sample)
			prev_mouse_state = True
		else:
			if prev_mouse_state:
				# Button just released:
				# Um, how to take the mean of a list of tuples?
				mean_sample = tuple(map(lambda y: sum(y) / float(len(y)), zip(*sample_pixels)))
				print('mean grey sample: ' + str(mean_sample))
				return mean_sample # or rinse and repeat?
			prev_mouse_state = False

# Usage:
#grey_sample = calibrate_white_balance(camera)

#while True:
#	wb(camera.getImage(), grey_sample).show()



# Some functions for identifying which regions of the image match a certain colour.  Let's try with black first:

def find_black(image):
	v,s,h = image.toHSV().splitChannels()
	dark = v.binarize(75)	# had 90 on ProBook cam
	grey = s.binarize(90)
	return (dark/16) * (grey/16)

# The same, but with parameters for value and saturation thresholds:

def find_black(image,v_thresh,s_thresh):
	v,s,h = image.toHSV().splitChannels()
	dark = v.binarize(v_thresh)	# had 90 on ProBook cam
	grey = s.binarize(s_thresh)
	return (dark/16) * (grey/16)

# It wouldn't be too hard to identify which regions of the image are (close enough to) grey as well, using just the saturation channel...
# ...although consider that specular highlights will basically appear white/light grey no matter what the actual hue of the surface

def find_grey(image,s_thresh):
	v,s,h = image.toHSV().splitChannels()
	return(s.binarize(s_thresh))


# For finding colours, we might be able to do reasonably well just by examining the hue channel, and perhaps the saturation as well.
# TODO: refine saturation matching. I suspect we want to match any saturation below the threshold, not just within a range, because of specular highlights.
# Specular reflections will likely cause problems, though, as parts of the image will have a very low saturation, high value, and likely completely wrong hue (the hue for greys defaults to red).

# TODO: might be able to speed this up a little by converting things to greyscale images..?
# Image.toGrey() or Image.greyscale()?!

# Note that we don't have to invert() here, when going from the colour distance to proximity to the target hue/saturation, because, bizarrely, binarize() seems to do that as well.

def find_colour(image, hue, hue_thresh, sat, sat_thresh):
	colour_hue = (hue,hue,hue)
	colour_sat = (sat,sat,sat)
	v,s,h = image.toHSV().splitChannels()
	hue_proximity = h.colorDistance(colour_hue).binarize(hue_thresh)
	sat_proximity = s.colorDistance(colour_sat).binarize(sat_thresh)
	return ((hue_proximity/16.0) * (sat_proximity/16.0))

# Also, note that there is a hueDistance() function as well, which might be more efficient than using colorDistance() in these sorts of situations.
# Presumably there is also a saturationDistance() and valueDistance()...?


# Here's a helper function for finding the target hue, hue threshold, saturation, and saturation threshold for matching a colour.
# TODO: implement a clickable colour picker using Display, similar to calibrate_white_balance() above.  Could infer the target centre and thresholds automatically, based on the range of colours selected.

def calibrate_colour_finder(camera):
	image = camera.getImage()
	image.show()
	highlight=solid(image, (255,0,255))
	v,s,h = image.toHSV().splitChannels()
	target_hue = 0
	target_sat = 0
	hue_threshold=0
	sat_threshold=0
	# Hue:
	response = raw_input('\nTarget Hue (or Enter when found) => ')
	while response != '':
		# TODO: handle bogus response robustly...
		target_hue = int(response)
		match = h.colorDistance((target_hue,target_hue,target_hue)).invert()
		((image * 0.1) + (match * 0.1) + (match.binarize(250).invert()/255 * highlight)).show()
		response = raw_input('\nTarget Hue => ')
	response = raw_input('\nHue Threshold => ')
	while response != '':
		# TODO: handle bogus response robustly...
		hue_threshold=int(response)
		#print(type(hue_threshold))
		# Gah, binarize() and it's built-in invert behaviour!! >:^p
		match = h.colorDistance((target_hue,target_hue,target_hue)).binarize(hue_threshold)
		((image * 0.1) + (match/255 * highlight)).show()
		response = raw_input('\nHue Threshold => ')
	response = raw_input('\nTarget Saturation (or Enter when found) => ')
	while response != '':
		# TODO: handle bogus response robustly...
		target_sat = int(response)
		match = s.colorDistance((target_sat,target_sat,target_sat)).invert()
		((image * 0.1) + (match * 0.1) + (match.binarize(250).invert()/255 * highlight)).show()
		response = raw_input('\nTarget Saturation => ')
	response = raw_input('\nSaturation Threshold => ')
	while response != '':
		# TODO: handle bogus response robustly...
		sat_threshold=int(response)
		match = s.colorDistance((target_sat,target_sat,target_sat)).binarize(sat_threshold)
		((image * 0.1) + (match/255 * highlight)).show()
		response = raw_input('\nSaturation Threshold => ')
	print('Target hue:', target_hue)
	print('Hue threshold:', hue_threshold)
	print('Target saturation:', target_sat)
	print('Saturation threshold:', sat_threshold)
	return (target_hue, hue_threshold, target_sat, sat_threshold)

# Try it out:
#calibrate_colour_finder()


# Trial hue distances:
#h.colorDistance((54,54,54)).show()
# Trial hue threshold:
#h.colorDistance((54,54,54)).binarize(48).show()
# Likewise for saturation:
#s.colorDistance((150,150,150)).show()
#s.colorDistance((150,150,150)).binarize(160).show()
<file_sep>/Documentation/Arduino/stepper_tests/stepper_tests.ino
// Test program for Pololu-style Allegro stepper driver carrier, CME 2015-11-02
// Wantai 42BYGHW811 test: at 4 kHz (1200 RPM) a lower frequency vibration starts to appear.
// Interestingly, it won't start reliably at high frequencies - you have to ramp it up I guess.  Acceleration setting in CNC control!  Reaches 7 kHz (2100 RPM) OK.  Fails to reach 10 kHz (3000 RPM) with ~1 A current limit.  8 kHz, indeed.

// It's also very rough and skips steps at low speeds as well.  The severity of this seems to be somewhat sensitive to the current limiting, with more current being a bigger problem.

// 17HS42 motor: 8 kHz is very marginal. More current not necc better!
// Wantai can achieve 10 kHz but nees more current to do so.

// Microstepping: initial experiment with 4x microstepping: I think higher speeds MIGHT be achievable, and the resonance problem is reduced.  Of course, you need 4x the pulse frequency to achieve the same speeds!  The Wantai can do 90 kHz -> 3375 RPM.  120 kHz -> 4500 RPM (erm, sometimes! It's pretty marginal).

// 2015-11-04: Have rigged up the 24-V power supply and the modded-for-24-V RAMPS board for testing.  Need to pull the enable pin for the stepper drive low.  Also, the RAMPS is jumpered for 16x microstepping, so the frequency-to-RPM mapping is changed again.  192 kHz would be 3600 RPM, I think
// 192 kHz / 200 / 16 -> min^-1 = 3600
// With delayMicroseconds(), I think it's a bit tough to reach high frequencies, and certainly not with any precision.  I managed to get it to just over 30 kHz
//17HS19 reached 690RPM with no load 

// For initial testing:
//const int STEP_PIN = 2;
//const int DIR_PIN = 3;

// RAMPS E0 stepper:
const int STEP_PIN = 26;
const int DIR_PIN = 28;
const int EN_PIN = 24; // Maybe this has to be set high^H^H^H^Hlow(!) for anything much to happen...

const int STEPS_PER_REVOLUTION = 200; // Depends on the stepper motor
const int MICROSTEPPING = 1; // Typically 16 on RAMPS boards for 3D printing.

// Should define constants for the timing signals too..


const int PULSE_LOW_TIME = 1000; // A4988
const int PULSE_HIGH_TIME = 1000;
// MadRobot router's YAKO drives have 2500 us

const int LED_PIN = 13;

const int MIN_FREQ = 50;
const long int MAX_FREQ = 1500; // Max step frequency in Hz.  Typical stepper motors are 1.8 degrees/step (200 steps/revolution), although microstepping could increase this up to 16-fold.  Hmm, but I'm seeing the frequency not go any higher than about 12 kHz on the scope..may need more optimised code...
const float FREQ_STEP = 10;

float freq = 0.0;
long int period_int = 0; // Global variable for current stepping pulse period (so it can be reused without recomputing every time).

int current_direction = 1;

void setup() {
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN,  OUTPUT);
  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);
  cw();
  pinMode(LED_PIN, OUTPUT);
}

// Compute and store the per-symbol period as an integer:
void set_period(float period) {
  period_int = (long int)round(period / 2.0); // Account for time for both high and low period.
}

void step() {
  digitalWrite(STEP_PIN, HIGH); digitalWrite(LED_PIN, HIGH); delayMicroseconds(period_int);
  digitalWrite(STEP_PIN, LOW); digitalWrite(LED_PIN, LOW); delayMicroseconds(period_int);
}

// Set direction to clockwise:
void cw() {
  digitalWrite(DIR_PIN, HIGH);
  // TODO: direction hold delay
}

// ...and counterclockwise:
void ccw() {
  digitalWrite(DIR_PIN, LOW);
  // TODO: direction hold delay
}

void _loop() {
}


void loop() {
  // Ramp speed up:
  // TODO: try exponential acceleration curve
  current_direction = !current_direction;
  if(current_direction){
    cw();
  }
  else{
    ccw();
  }
  
  for (freq = MIN_FREQ; freq < MAX_FREQ; freq = freq + FREQ_STEP) {
    set_period(1.0 / freq * 1e6); // Convert to period in microseconds
    step(); step(); step(); step(); step(); step(); step(); step();
  }

  // I've noticed the speed increase during this loop. Ah, because it's not having to do so much computation!  Let's compute and store period in a global variable to speed it up even more.
  // Run for a while at max...
  // TODO: have num iterations here based on time rather than a fixed number!
  for (int i = 0; i < 2000; i++) {
     step(); step(); step(); step(); step(); step(); step(); step();
  }
  
  // ...and ramp it down again:
  for (freq = MAX_FREQ; freq > MIN_FREQ; freq = freq - FREQ_STEP) {
    set_period(1.0 / freq * 1e6); // Convert to period in microseconds
    step(); step(); step(); step(); step(); step(); step(); step();
  }
}

// What about variable speed 
<file_sep>/Projects/2016/MIDIBots/arduino/multi_servo_test/ano/src/sketch.ino
../../multi_servo_test.ino<file_sep>/Projects/2017/SoccerBots/camera_setup.py
# <NAME>'s excellent picamera library:
from picamera.array import PiRGBArray
from picamera import PiCamera

# The OpenCV library:
import cv2

from time import sleep,time
import sys
import numpy


# Set up camera and a NumPy array wrapper suitable for transferring the image data into OpenCV

target_framerate = 20
# 0.08 s frame capture with 50 Hz target

# Hmm, at 128 x 96 the chroma subsampling is pretty bad. Can we influence the format, or do we just have to increase the resolution?
capture_res = (128,96)
#capture_res = (256,192)

# How much of the frame to crop to when doing the calibration sampling?
xfrac = 0.1
yfrac = 0.1

camera = PiCamera(resolution=capture_res, framerate=target_framerate)
camera.iso = 400
camera.shutter_speed = 30000
#camera.shutter_speed = 15000


# array wrapper:
rawCapture = PiRGBArray(camera)

params = cv2.SimpleBlobDetector_Params()
# Change thresholds
params.minThreshold = 10;    # the graylevel of images
params.maxThreshold = 200;

params.filterByColor = True
params.blobColor = 255

# Filter by Area
params.filterByArea = False
params.minArea = 10000

params.filterByInertia = False
params.filterByConvexity = False

detector = cv2.SimpleBlobDetector(params)



<file_sep>/Projects/2017/RescueBot/TODO.txt
[ ] Test how frequently an Arduino sketch can read from a digital port and react.
[ ] Obtain colour sensors, test, and integrate
[ ] Range sensing (for water tower, rescue target): ultrasonic or IR?
[ ] Develop and test lifter designs
[ ] Develop a sensor "trolley" design, to keep the sensor distance and orientation consistent with the terrain. Should also provide shielding from ambient light.
[ ] Wireless communication for programming and debugging?
[ ] Test motors and drive trains (Tamiya planetary gear sets? Brushless motors?  Maybe make our own brushless motor controller?!)
[ ] Implement a proportional (and possibly integral/derivative) control system?  Ideally would be able to adapt turning to the curvature of the line.
[ ] Make the system adaptible to different tile surfaces and printing characteristics?
[ ] How to do high-level control?  Subroutines for specialised actions for certain tiles?  Include magnetometer so we can have some sense of direction?
[ ] Computer vision system??? Would need a Raspberry Pi or comparable computer onboard.
