Mercurial > ~darius > hgwebdir.cgi > splitbrain

annotate shore/shore.ino @ 3:d25e14956c65 default tip

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Rework log transfer function code and tidy up
author Daniel O'Connor <darius@dons.net.au>
date Sat, 11 Jul 2015 17:44:39 +0930
parents 7d4ec58da1d8
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
1 #include <avr/pgmspace.h>
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
2
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
3 #include <Servo.h>
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
4 #include <Wire.h>
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
5 #include <LiquidCrystal_I2C.h>
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
6 #include "log_lookup.h"
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
7
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
8 // DFRobot I/O Expansion Shield V7 - http://www.dfrobot.com/wiki/index.php/IO_Expansion_Shield_for_Arduino_V7_SKU:DFR0265
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
9 // 2xPS3 joysticks connected like so
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
10 // Joy 1 Horizontal - Left/right - AD0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
11 // Joy 1 Vertical - Forward/back - AD1
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
12 // Joy 1 Button - Unused - D2
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
13 // Joy 2 Horizontal - Unused - AD2
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
14 // Joy 2 Vertical - Up/down - AD3
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
15 // Joy 2 Button - Unused - D3
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
16
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
17 // LCD panel
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
18 LiquidCrystal_I2C lcd(0x27, 16, 2); // I/O expander is at 0x27, LCD is 16x2
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
19
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
20 void setup() {
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
21 int byte;
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
22
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
23 Serial.begin(115200);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
24
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
25 // Set joystick button pins as inputs
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
26 pinMode(7, INPUT);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
27 pinMode(8, INPUT);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
28
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
29 lcd.init();
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
30 lcd.backlight();
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
31 lcd.print("Hello world!");
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
32 }
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
33
3
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
34 // Based on http://playground.arduino.cc/Main/Fscale
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
35 float
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
36 fscale(float originalMin, float originalMax, float newBegin,
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
37 float newEnd, float inputValue, float curve) {
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
38 float OriginalRange = 0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
39 float NewRange = 0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
40 float zeroRefCurVal = 0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
41 float normalizedCurVal = 0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
42 float rangedValue = 0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
43 boolean invFlag = 0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
44
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
45 // condition curve parameter
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
46 // limit range
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
47 if (curve > 10) curve = 10;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
48 if (curve < -10) curve = -10;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
49
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
50 curve = (curve * -.1) ; // - invert and scale - this seems more intuitive - postive numbers give more weight to high end on output
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
51 curve = pow(10, curve); // convert linear scale into lograthimic exponent for other pow function
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
52
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
53 /*
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
54 Serial.println(curve * 100, DEC); // multply by 100 to preserve resolution
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
55 Serial.println();
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
56 */
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
57
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
58 // Check for out of range inputValues
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
59 if (inputValue < originalMin) {
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
60 inputValue = originalMin;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
61 }
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
62 if (inputValue > originalMax) {
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
63 inputValue = originalMax;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
64 }
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
65
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
66 // Zero Reference the values
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
67 OriginalRange = originalMax - originalMin;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
68
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
69 if (newEnd > newBegin) {
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
70 NewRange = newEnd - newBegin;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
71 } else {
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
72 NewRange = newBegin - newEnd;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
73 invFlag = 1;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
74 }
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
75
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
76 zeroRefCurVal = inputValue - originalMin;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
77 normalizedCurVal = zeroRefCurVal / OriginalRange; // normalize to 0 - 1 float
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
78
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
79 /*
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
80 Serial.print(OriginalRange, DEC);
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
81 Serial.print(" ");
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
82 Serial.print(NewRange, DEC);
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
83 Serial.print(" ");
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
84 Serial.println(zeroRefCurVal, DEC);
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
85 Serial.println();
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
86 */
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
87
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
88 // Check for originalMin > originalMax - the math for all other cases i.e. negative numbers seems to work out fine
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
89 if (originalMin > originalMax) {
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
90 return 0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
91 }
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
92
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
93 if (invFlag == 0) {
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
94 rangedValue = (pow(normalizedCurVal, curve) * NewRange) + newBegin;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
95
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
96 } else {
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
97 // invert the ranges
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
98 rangedValue = newBegin - (pow(normalizedCurVal, curve) * NewRange);
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
99 }
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
100
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
101 return rangedValue;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
102 }
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
103
0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
104 void loop() {
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
105 int joy1X, joy1Y, joy2X, joy2Y, but1, but2;
3
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
106 int lint, rint, vint, ldir, rdir, vdir;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
107 float k, l, r, v, x, y, tmp;
0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
108 char lcdbuf[16 + 1]; // Buffer 1 line
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
109
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
110 k = 1.5;
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
111
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
112 joy1X = analogRead(A0);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
113 joy1Y = analogRead(A1);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
114 but1 = !digitalRead(7); // Buttons are active low
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
115 joy2X = analogRead(A2);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
116 joy2Y = analogRead(A3);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
117 but2 = !digitalRead(8);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
118
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
119 #if 0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
120 Serial.print("Joy 1 X: "); Serial.print(joy1X); Serial.print(" ");
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
121 Serial.print("Y: "); Serial.print(joy1Y); Serial.print(" Button: ");
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
122 Serial.println(but1);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
123 Serial.print("Joy 2 X: "); Serial.print(joy2X); Serial.print(" ");
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
124 Serial.print("Y: "); Serial.print(joy2Y); Serial.print(" Button: ");
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
125 Serial.println(but2);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
126 #endif
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
127 // Create deadband in the centre because they don't sit at 512
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
128 if (joy1X > 500 && joy1X < 510) // Sits at 505
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
129 joy1X = 512;
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
130 if (joy1Y > 516 && joy1Y < 526) // Sits at 521
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
131 joy1Y = 512;
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
132 if (joy2X > 500 && joy2X < 510) // Sits at 505
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
133 joy2X = 512;
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
134 if (joy2Y > 520 && joy2Y < 530) // Sits at 525
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
135 joy2Y = 512;
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
136
3
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
137 // Convert each axis to -1..1
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
138 x = ((float)joy1X - 512.0) / 512.0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
139 y = ((float)joy1Y - 512.0) / 512.0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
140 v = ((float)joy2Y - 512.0) / 512.0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
141
0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
142 // Mix 'joystick' input to left/right thrust (elevon mixing)
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
143 // http://eastbay-rc.blogspot.com.au/2011/05/elevon-v-tail-mixing-calculations.html
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
144 l = 1 * (x / 2 + y / 2);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
145 r = -1 * (x / 2 - y / 2);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
146
3
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
147 // Apply some gain so full forward is a max speed
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
148 l *= 2.0;
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
149 r *= 2.0;
0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
150
3
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
151 // Apply transfer function and limit
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
152 tmp = fscale(0, 1, 0, 1, abs(l), -1.5);
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
153 #if 0
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
154 Serial.print("L: "); Serial.print(l); Serial.print(" Tmp: "); Serial.print(tmp); Serial.println();
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
155 #endif
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
156 l = tmp * (l < 0 ? -1 : 1);
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
157 tmp = fscale(0, 1, 0, 1, abs(r), -1.5);
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
158 r = tmp * (r < 0 ? -1 : 1);
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
159 tmp = fscale(0, 1, 0, 1, abs(v), -1.5);
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
160 v = tmp * (v < 0 ? -1 : 1);
0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
161
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
162 // Map values to -255..255
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
163 lint = l * 255;
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
164 rint = r * 255;
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
165 vint = v * 255;
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
166
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
167 Serial.print("L:");
3
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
168 Serial.print(lint);
0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
169 Serial.print(" R:");
3
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
170 Serial.print(rint);
0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
171 Serial.print(" V:");
3
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
172 Serial.print(vint);
0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
173 Serial.println();
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
174
3
d25e14956c65 Rework log transfer function code and tidy up
Daniel O'Connor <darius@dons.net.au>
parents: 2
diff changeset
175 snprintf(lcdbuf, sizeof(lcdbuf) - 1, "L%3dR%3dV%3d", lint, rint, vint);
0
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
176 lcd.setCursor(0, 0);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
177 lcd.print(lcdbuf);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
178 snprintf(lcdbuf, sizeof(lcdbuf) - 1, " B1: %d B2: %d ", but1, but2);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
179 lcd.setCursor(0, 1);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
180 lcd.print(lcdbuf);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
181 delay(200);
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
182 }
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
183
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
184 /*
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
185 * Local variables:
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
186 * mode: c++
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
187 * End:
fe0a2f223e10 Initial commit of split brain code for the Sea Dragon.
Daniel O'Connor <darius@dons.net.au>
parents:
diff changeset
188 */