# HG changeset patch
# User Daniel O'Connor <darius@dons.net.au>
# Date 1436602479 -34200
# Node ID d25e14956c658cad1a705ddd1cb55b87a82d0888
# Parent  7d4ec58da1d8248034ec1f58351df5a3145a5972
Rework log transfer function code and tidy up

diff -r 7d4ec58da1d8 -r d25e14956c65 shore/shore.ino
--- a/shore/shore.ino	Sat Jul 11 15:34:59 2015 +0930
+++ b/shore/shore.ino	Sat Jul 11 17:44:39 2015 +0930
@@ -31,10 +31,80 @@
 	lcd.print("Hello world!");
 }
 
+// Based on http://playground.arduino.cc/Main/Fscale
+float
+fscale(float originalMin, float originalMax, float newBegin,
+    float newEnd, float inputValue, float curve) {
+	float OriginalRange = 0;
+	float NewRange = 0;
+	float zeroRefCurVal = 0;
+	float normalizedCurVal = 0;
+	float rangedValue = 0;
+	boolean invFlag = 0;
+
+	// condition curve parameter
+	// limit range
+	if (curve > 10) curve = 10;
+	if (curve < -10) curve = -10;
+
+	curve = (curve * -.1) ; // - invert and scale - this seems more intuitive - postive numbers give more weight to high end on output
+	curve = pow(10, curve); // convert linear scale into lograthimic exponent for other pow function
+
+	/*
+	  Serial.println(curve * 100, DEC);   // multply by 100 to preserve resolution
+	  Serial.println();
+	*/
+
+	// Check for out of range inputValues
+	if (inputValue < originalMin) {
+		inputValue = originalMin;
+	}
+	if (inputValue > originalMax) {
+		inputValue = originalMax;
+	}
+
+	// Zero Reference the values
+	OriginalRange = originalMax - originalMin;
+
+	if (newEnd > newBegin) {
+		NewRange = newEnd - newBegin;
+	} else {
+		NewRange = newBegin - newEnd;
+		invFlag = 1;
+	}
+
+	zeroRefCurVal = inputValue - originalMin;
+	normalizedCurVal  =  zeroRefCurVal / OriginalRange;   // normalize to 0 - 1 float
+
+	/*
+	  Serial.print(OriginalRange, DEC);
+	  Serial.print("   ");
+	  Serial.print(NewRange, DEC);
+	  Serial.print("   ");
+	  Serial.println(zeroRefCurVal, DEC);
+	  Serial.println();
+	*/
+
+	// Check for originalMin > originalMax  - the math for all other cases i.e. negative numbers seems to work out fine
+	if (originalMin > originalMax) {
+		return 0;
+	}
+
+	if (invFlag == 0) {
+		rangedValue = (pow(normalizedCurVal, curve) * NewRange) + newBegin;
+
+	} else {
+		// invert the ranges
+		rangedValue = newBegin - (pow(normalizedCurVal, curve) * NewRange);
+	}
+
+	return rangedValue;
+}
+
 void loop() {
 	int joy1X, joy1Y, joy2X, joy2Y, but1, but2;
-	int lint, rint, vint, ldir, rdir, vdir, ethrot;
-	float k, l, r, v, x, y;
+	int lint, rint, vint, ldir, rdir, vdir;
+	float k, l, r, v, x, y, tmp;
 	char lcdbuf[16 + 1]; // Buffer 1 line
 
 	k = 1.5;
@@ -64,57 +134,45 @@
 	if (joy2Y > 520 && joy2Y < 530) // Sits at 525
 		joy2Y = 512;
 
+	// Convert each axis to -1..1
+	x = ((float)joy1X - 512.0) / 512.0;
+	y = ((float)joy1Y - 512.0) / 512.0;
+	v = ((float)joy2Y - 512.0) / 512.0;
+
 	// 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;
+	// Apply some gain so full forward is a max speed
+	l *= 2.0;
+	r *= 2.0;
 
-	// 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;
+	// Apply transfer function and limit
+	tmp = fscale(0, 1, 0, 1, abs(l), -1.5);
+#if 0
+	Serial.print("L: "); Serial.print(l); Serial.print(" Tmp: "); Serial.print(tmp); Serial.println();
+#endif
+	l = tmp * (l < 0 ? -1 : 1);
+	tmp = fscale(0, 1, 0, 1, abs(r), -1.5);
+	r = tmp * (r < 0 ? -1 : 1);
+	tmp = fscale(0, 1, 0, 1, abs(v), -1.5);
+	v = tmp * (v < 0 ? -1 : 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));
-
 	Serial.print("L:");
-	Serial.print(lint * (ldir == 0 ? -1 : 1));
+	Serial.print(lint);
 	Serial.print(" R:");
-	Serial.print(rint * (rdir == 0 ? -1 : 1));
+	Serial.print(rint);
 	Serial.print(" V:");
-	Serial.print(vint * (vdir == 0 ? -1 : 1));
+	Serial.print(vint);
 	Serial.println();
 
-	snprintf(lcdbuf, sizeof(lcdbuf) - 1, "L%c%3dR%c%3dV%c%3d", ldir == 0 ? '-' : '+', lint,
-	    rdir == 0 ? '-' : '+', rint, vdir == 0 ? '-' : '+', vint);
+	snprintf(lcdbuf, sizeof(lcdbuf) - 1, "L%3dR%3dV%3d", lint, rint, vint);
 	lcd.setCursor(0, 0);
 	lcd.print(lcdbuf);
 	snprintf(lcdbuf, sizeof(lcdbuf) - 1, "  B1: %d  B2: %d   ", but1, but2);