# HG changeset patch
# User Daniel O'Connor <darius@dons.net.au>
# Date 1436593805 -34200
# Node ID fe0a2f223e10e3b402de3db6686c74ee4cfc1b34

Initial commit of split brain code for the Sea Dragon.

diff -r 000000000000 -r fe0a2f223e10 sea/sea.ino
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sea/sea.ino	Sat Jul 11 15:20:05 2015 +0930
@@ -0,0 +1,75 @@
+#include <Wire.h>
+#include <Adafruit_MotorShield.h>
+#include "utility/Adafruit_PWMServoDriver.h"
+
+// Orange	- Vertical M2+
+// White/orange - Vertical M2-
+// Green        - Left M3+
+// White/green  - Left M3-
+// Blue         - Right M1+
+// White/blue   - Right M1-
+// Brown        - RS485 B
+// Brown/white  - RS485 A
+
+// Motor shield object at default address
+Adafruit_MotorShield AFMS = Adafruit_MotorShield();
+
+// Motor objects
+Adafruit_DCMotor *leftMotor = AFMS.getMotor(3);
+Adafruit_DCMotor *rightMotor = AFMS.getMotor(1);
+Adafruit_DCMotor *vertMotor = AFMS.getMotor(2);
+
+void setup() {
+	Serial.begin(115200);
+
+	AFMS.begin();	// Start at default frequency
+}
+
+int
+parseIn(char dir) {
+	int tmp;
+
+	while (Serial.read() != dir)
+		;
+	while (Serial.read() != ':')
+		;
+	tmp = Serial.parseInt();
+
+	return tmp;
+}
+
+void loop() {
+	int tmp;
+
+	Serial.println("Parsing L...");
+	tmp = parseIn('L');
+	if (tmp > 0)
+		leftMotor->run(FORWARD);
+	else
+		leftMotor->run(BACKWARD);
+	leftMotor->setSpeed(abs(tmp));
+
+	Serial.println("Parsing R...");
+	tmp = parseIn('R');
+	if (tmp > 0)
+		rightMotor->run(FORWARD);
+	else
+		rightMotor->run(BACKWARD);
+	rightMotor->setSpeed(abs(tmp));
+
+	Serial.println("Parsing V...");
+	tmp = parseIn('V');
+	if (tmp > 0)
+		vertMotor->run(FORWARD);
+	else
+		vertMotor->run(BACKWARD);
+	vertMotor->setSpeed(abs(tmp));
+
+	Serial.println("Done");
+}
+
+/*
+ * Local variables:
+ * mode: c++
+ * End:
+ */
diff -r 000000000000 -r fe0a2f223e10 shore/log_lookup.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shore/log_lookup.h	Sat Jul 11 15:20:05 2015 +0930
@@ -0,0 +1,262 @@
+// Logarithmic transfer function
+// Generated using..
+// map(lambda x: int(math.log(x) * 255 / 2.2), numpy.linspace(1, math.exp(2.2), 256))
+
+const uint8_t log_lookup[] PROGMEM = {
+ 0,
+ 3,
+ 7,
+ 10,
+ 13,
+ 16,
+ 20,
+ 23,
+ 26,
+ 28,
+ 31,
+ 34,
+ 37,
+ 39,
+ 42,
+ 44,
+ 47,
+ 49,
+ 52,
+ 54,
+ 56,
+ 58,
+ 60,
+ 63,
+ 65,
+ 67,
+ 69,
+ 71,
+ 73,
+ 75,
+ 77,
+ 78,
+ 80,
+ 82,
+ 84,
+ 86,
+ 87,
+ 89,
+ 91,
+ 92,
+ 94,
+ 96,
+ 97,
+ 99,
+ 100,
+ 102,
+ 103,
+ 105,
+ 106,
+ 108,
+ 109,
+ 110,
+ 112,
+ 113,
+ 115,
+ 116,
+ 117,
+ 119,
+ 120,
+ 121,
+ 122,
+ 124,
+ 125,
+ 126,
+ 127,
+ 129,
+ 130,
+ 131,
+ 132,
+ 133,
+ 134,
+ 136,
+ 137,
+ 138,
+ 139,
+ 140,
+ 141,
+ 142,
+ 143,
+ 144,
+ 145,
+ 146,
+ 147,
+ 148,
+ 149,
+ 150,
+ 151,
+ 152,
+ 153,
+ 154,
+ 155,
+ 156,
+ 157,
+ 158,
+ 159,
+ 160,
+ 161,
+ 162,
+ 163,
+ 163,
+ 164,
+ 165,
+ 166,
+ 167,
+ 168,
+ 169,
+ 170,
+ 170,
+ 171,
+ 172,
+ 173,
+ 174,
+ 174,
+ 175,
+ 176,
+ 177,
+ 178,
+ 178,
+ 179,
+ 180,
+ 181,
+ 182,
+ 182,
+ 183,
+ 184,
+ 185,
+ 185,
+ 186,
+ 187,
+ 187,
+ 188,
+ 189,
+ 190,
+ 190,
+ 191,
+ 192,
+ 192,
+ 193,
+ 194,
+ 194,
+ 195,
+ 196,
+ 196,
+ 197,
+ 198,
+ 198,
+ 199,
+ 200,
+ 200,
+ 201,
+ 202,
+ 202,
+ 203,
+ 204,
+ 204,
+ 205,
+ 205,
+ 206,
+ 207,
+ 207,
+ 208,
+ 208,
+ 209,
+ 210,
+ 210,
+ 211,
+ 211,
+ 212,
+ 213,
+ 213,
+ 214,
+ 214,
+ 215,
+ 215,
+ 216,
+ 217,
+ 217,
+ 218,
+ 218,
+ 219,
+ 219,
+ 220,
+ 220,
+ 221,
+ 222,
+ 222,
+ 223,
+ 223,
+ 224,
+ 224,
+ 225,
+ 225,
+ 226,
+ 226,
+ 227,
+ 227,
+ 228,
+ 228,
+ 229,
+ 229,
+ 230,
+ 230,
+ 231,
+ 231,
+ 232,
+ 232,
+ 233,
+ 233,
+ 234,
+ 234,
+ 235,
+ 235,
+ 236,
+ 236,
+ 237,
+ 237,
+ 238,
+ 238,
+ 238,
+ 239,
+ 239,
+ 240,
+ 240,
+ 241,
+ 241,
+ 242,
+ 242,
+ 243,
+ 243,
+ 243,
+ 244,
+ 244,
+ 245,
+ 245,
+ 246,
+ 246,
+ 247,
+ 247,
+ 247,
+ 248,
+ 248,
+ 249,
+ 249,
+ 250,
+ 250,
+ 250,
+ 251,
+ 251,
+ 252,
+ 252,
+ 252,
+ 253,
+ 253,
+ 254,
+ 254,
+ 255
+};
diff -r 000000000000 -r fe0a2f223e10 shore/shore.ino
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shore/shore.ino	Sat Jul 11 15:20:05 2015 +0930
@@ -0,0 +1,136 @@
+#include <avr/pgmspace.h>
+
+#include <Servo.h>
+#include <Wire.h>
+#include <LiquidCrystal_I2C.h>
+#include "log_lookup.h"
+
+// DFRobot I/O Expansion Shield V7 - http://www.dfrobot.com/wiki/index.php/IO_Expansion_Shield_for_Arduino_V7_SKU:DFR0265
+// 2xPS3 joysticks connected like so
+// Joy 1 Horizontal - Left/right   - AD0
+// Joy 1 Vertical   - Forward/back - AD1
+// Joy 1 Button     - Unused       - D2
+// Joy 2 Horizontal - Unused       - AD2
+// Joy 2 Vertical   - Up/down      - AD3
+// Joy 2 Button     - Unused       - D3
+
+// LCD panel
+LiquidCrystal_I2C lcd(0x27, 16, 2);  // I/O expander is at 0x27, LCD is 16x2
+
+void setup() {
+	int byte;
+
+	Serial.begin(115200);
+
+	// Set joystick button pins as inputs
+	pinMode(7, INPUT);
+	pinMode(8, INPUT);
+
+	lcd.init();
+	lcd.backlight();
+	lcd.print("Hello world!");
+}
+
+void loop() {
+	int joy1X, joy1Y, joy2X, joy2Y, but1, but2;
+	int lint, rint, vint, ldir, rdir, vdir, ethrot;
+	float k, l, r, v, x, y;
+	char lcdbuf[16 + 1]; // Buffer 1 line
+
+	k = 1.5;
+
+	joy1X = analogRead(A0);
+	joy1Y = analogRead(A1);
+	but1 = !digitalRead(7); // Buttons are active low
+	joy2X = analogRead(A2);
+	joy2Y = analogRead(A3);
+	but2 = !digitalRead(8);
+
+#if 0
+	Serial.print("Joy 1 X: "); Serial.print(joy1X); Serial.print("  ");
+	Serial.print("Y: "); Serial.print(joy1Y); Serial.print("   Button:  ");
+	Serial.println(but1);
+	Serial.print("Joy 2 X: "); Serial.print(joy2X); Serial.print("  ");
+	Serial.print("Y: "); Serial.print(joy2Y); Serial.print("   Button:  ");
+	Serial.println(but2);
+#endif
+	// Create deadband in the centre because they don't sit at 512
+	if (joy1X > 500 && joy1X < 510) // Sits at 505
+		joy1X = 512;
+	if (joy1Y > 516 && joy1Y < 526) // Sits at 521
+		joy1Y = 512;
+	if (joy2X > 500 && joy2X < 510) // Sits at 505
+		joy2X = 512;
+	if (joy2Y > 520 && joy2Y < 530) // Sits at 525
+		joy2Y = 512;
+
+	// Mix 'joystick' input to left/right thrust (elevon mixing)
+	// http://eastbay-rc.blogspot.com.au/2011/05/elevon-v-tail-mixing-calculations.html
+	x = (joy1X - 512.0) / 512.0; // More positive = right
+	y = (joy1Y - 512.0) / 512.0; // More positive = forward, convert to -1..1
+
+	l =  1 * (x / 2 + y / 2);
+        r = -1 * (x / 2 - y / 2);
+
+	v = ((float)(joy2Y - 512) / 512.0);
+
+	// Apply some gain
+	l *= 2.2;
+	r *= 2.2;
+
+	// Limit
+	if (l > 1)
+		l = 1;
+	if (l < -1)
+		l = -1;
+	if (r > 1)
+		r = 1;
+	if (r < -1)
+		r = -1;
+	if (v < -1)
+		v = -1;
+	if (v > 1)
+		v = 1;
+
+	// Map values to -255..255
+	lint = l * 255;
+	rint = r * 255;
+	vint = v * 255;
+
+	// Apply log transfer function
+	ldir = lint < 0 ? 0 : 1; // 0 = reverse, 1 = forward
+	lint = pgm_read_byte_near(log_lookup + abs(lint));
+	rdir = rint < 0 ? 0 : 1;
+	rint = pgm_read_byte_near(log_lookup + abs(rint));
+	vdir = vint < 0 ? 0 : 1;
+	vint = pgm_read_byte_near(log_lookup + abs(vint));
+
+#if 0
+	Serial.print("l = "); Serial.print(l); Serial.print(" "); Serial.print(lint); Serial.print(" "); Serial.print(ldir);
+	Serial.print(" r = "); Serial.print(r); Serial.print(" "); Serial.print(rint); Serial.print(" "); Serial.print(r);
+	Serial.print(" v = "); Serial.print(vint); Serial.print(" "); Serial.println(vdir);
+#else
+	Serial.print("L:");
+	Serial.print(lint * (ldir == 0 ? -1 : 1));
+	Serial.print(" R:");
+	Serial.print(rint * (rdir == 0 ? -1 : 1));
+	Serial.print(" V:");
+	Serial.print(vint * (vdir == 0 ? -1 : 1));
+	Serial.println();
+#endif
+
+	snprintf(lcdbuf, sizeof(lcdbuf) - 1, "L%c%3dR%c%3dV%c%3d", ldir == 0 ? '-' : '+', lint,
+	    rdir == 0 ? '-' : '+', rint, vdir == 0 ? '-' : '+', vint);
+	lcd.setCursor(0, 0);
+	lcd.print(lcdbuf);
+	snprintf(lcdbuf, sizeof(lcdbuf) - 1, "  B1: %d  B2: %d   ", but1, but2);
+	lcd.setCursor(0, 1);
+	lcd.print(lcdbuf);
+	delay(200);
+}
+
+/*
+ * Local variables:
+ * mode: c++
+ * End:
+ */