Mercurial > ~darius > hgwebdir.cgi > splitbrain

comparison 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
comparison
equal deleted inserted replaced
2:7d4ec58da1d8 3:d25e14956c65
29 lcd.init(); 29 lcd.init();
30 lcd.backlight(); 30 lcd.backlight();
31 lcd.print("Hello world!"); 31 lcd.print("Hello world!");
32 } 32 }
33 33
34 // Based on http://playground.arduino.cc/Main/Fscale
35 float
36 fscale(float originalMin, float originalMax, float newBegin,
37 float newEnd, float inputValue, float curve) {
38 float OriginalRange = 0;
39 float NewRange = 0;
40 float zeroRefCurVal = 0;
41 float normalizedCurVal = 0;
42 float rangedValue = 0;
43 boolean invFlag = 0;
44
45 // condition curve parameter
46 // limit range
47 if (curve > 10) curve = 10;
48 if (curve < -10) curve = -10;
49
50 curve = (curve * -.1) ; // - invert and scale - this seems more intuitive - postive numbers give more weight to high end on output
51 curve = pow(10, curve); // convert linear scale into lograthimic exponent for other pow function
52
53 /*
54 Serial.println(curve * 100, DEC); // multply by 100 to preserve resolution
55 Serial.println();
56 */
57
58 // Check for out of range inputValues
59 if (inputValue < originalMin) {
60 inputValue = originalMin;
61 }
62 if (inputValue > originalMax) {
63 inputValue = originalMax;
64 }
65
66 // Zero Reference the values
67 OriginalRange = originalMax - originalMin;
68
69 if (newEnd > newBegin) {
70 NewRange = newEnd - newBegin;
71 } else {
72 NewRange = newBegin - newEnd;
73 invFlag = 1;
74 }
75
76 zeroRefCurVal = inputValue - originalMin;
77 normalizedCurVal = zeroRefCurVal / OriginalRange; // normalize to 0 - 1 float
78
79 /*
80 Serial.print(OriginalRange, DEC);
81 Serial.print(" ");
82 Serial.print(NewRange, DEC);
83 Serial.print(" ");
84 Serial.println(zeroRefCurVal, DEC);
85 Serial.println();
86 */
87
88 // Check for originalMin > originalMax - the math for all other cases i.e. negative numbers seems to work out fine
89 if (originalMin > originalMax) {
90 return 0;
91 }
92
93 if (invFlag == 0) {
94 rangedValue = (pow(normalizedCurVal, curve) * NewRange) + newBegin;
95
96 } else {
97 // invert the ranges
98 rangedValue = newBegin - (pow(normalizedCurVal, curve) * NewRange);
99 }
100
101 return rangedValue;
102 }
103
34 void loop() { 104 void loop() {
35 int joy1X, joy1Y, joy2X, joy2Y, but1, but2; 105 int joy1X, joy1Y, joy2X, joy2Y, but1, but2;
36 int lint, rint, vint, ldir, rdir, vdir, ethrot; 106 int lint, rint, vint, ldir, rdir, vdir;
37 float k, l, r, v, x, y; 107 float k, l, r, v, x, y, tmp;
38 char lcdbuf[16 + 1]; // Buffer 1 line 108 char lcdbuf[16 + 1]; // Buffer 1 line
39 109
40 k = 1.5; 110 k = 1.5;
41 111
42 joy1X = analogRead(A0); 112 joy1X = analogRead(A0);
62 if (joy2X > 500 && joy2X < 510) // Sits at 505 132 if (joy2X > 500 && joy2X < 510) // Sits at 505
63 joy2X = 512; 133 joy2X = 512;
64 if (joy2Y > 520 && joy2Y < 530) // Sits at 525 134 if (joy2Y > 520 && joy2Y < 530) // Sits at 525
65 joy2Y = 512; 135 joy2Y = 512;
66 136
137 // Convert each axis to -1..1
138 x = ((float)joy1X - 512.0) / 512.0;
139 y = ((float)joy1Y - 512.0) / 512.0;
140 v = ((float)joy2Y - 512.0) / 512.0;
141
67 // Mix 'joystick' input to left/right thrust (elevon mixing) 142 // Mix 'joystick' input to left/right thrust (elevon mixing)
68 // http://eastbay-rc.blogspot.com.au/2011/05/elevon-v-tail-mixing-calculations.html 143 // http://eastbay-rc.blogspot.com.au/2011/05/elevon-v-tail-mixing-calculations.html
69 x = (joy1X - 512.0) / 512.0; // More positive = right
70 y = (joy1Y - 512.0) / 512.0; // More positive = forward, convert to -1..1
71
72 l = 1 * (x / 2 + y / 2); 144 l = 1 * (x / 2 + y / 2);
73 r = -1 * (x / 2 - y / 2); 145 r = -1 * (x / 2 - y / 2);
74 146
75 v = ((float)(joy2Y - 512) / 512.0); 147 // Apply some gain so full forward is a max speed
148 l *= 2.0;
149 r *= 2.0;
76 150
77 // Apply some gain 151 // Apply transfer function and limit
78 l *= 2.2; 152 tmp = fscale(0, 1, 0, 1, abs(l), -1.5);
79 r *= 2.2; 153 #if 0
80 154 Serial.print("L: "); Serial.print(l); Serial.print(" Tmp: "); Serial.print(tmp); Serial.println();
81 // Limit 155 #endif
82 if (l > 1) 156 l = tmp * (l < 0 ? -1 : 1);
83 l = 1; 157 tmp = fscale(0, 1, 0, 1, abs(r), -1.5);
84 if (l < -1) 158 r = tmp * (r < 0 ? -1 : 1);
85 l = -1; 159 tmp = fscale(0, 1, 0, 1, abs(v), -1.5);
86 if (r > 1) 160 v = tmp * (v < 0 ? -1 : 1);
87 r = 1;
88 if (r < -1)
89 r = -1;
90 if (v < -1)
91 v = -1;
92 if (v > 1)
93 v = 1;
94 161
95 // Map values to -255..255 162 // Map values to -255..255
96 lint = l * 255; 163 lint = l * 255;
97 rint = r * 255; 164 rint = r * 255;
98 vint = v * 255; 165 vint = v * 255;
99 166
100 // Apply log transfer function
101 ldir = lint < 0 ? 0 : 1; // 0 = reverse, 1 = forward
102 lint = pgm_read_byte_near(log_lookup + abs(lint));
103 rdir = rint < 0 ? 0 : 1;
104 rint = pgm_read_byte_near(log_lookup + abs(rint));
105 vdir = vint < 0 ? 0 : 1;
106 vint = pgm_read_byte_near(log_lookup + abs(vint));
107
108 Serial.print("L:"); 167 Serial.print("L:");
109 Serial.print(lint * (ldir == 0 ? -1 : 1)); 168 Serial.print(lint);
110 Serial.print(" R:"); 169 Serial.print(" R:");
111 Serial.print(rint * (rdir == 0 ? -1 : 1)); 170 Serial.print(rint);
112 Serial.print(" V:"); 171 Serial.print(" V:");
113 Serial.print(vint * (vdir == 0 ? -1 : 1)); 172 Serial.print(vint);
114 Serial.println(); 173 Serial.println();
115 174
116 snprintf(lcdbuf, sizeof(lcdbuf) - 1, "L%c%3dR%c%3dV%c%3d", ldir == 0 ? '-' : '+', lint, 175 snprintf(lcdbuf, sizeof(lcdbuf) - 1, "L%3dR%3dV%3d", lint, rint, vint);
117 rdir == 0 ? '-' : '+', rint, vdir == 0 ? '-' : '+', vint);
118 lcd.setCursor(0, 0); 176 lcd.setCursor(0, 0);
119 lcd.print(lcdbuf); 177 lcd.print(lcdbuf);
120 snprintf(lcdbuf, sizeof(lcdbuf) - 1, " B1: %d B2: %d ", but1, but2); 178 snprintf(lcdbuf, sizeof(lcdbuf) - 1, " B1: %d B2: %d ", but1, but2);
121 lcd.setCursor(0, 1); 179 lcd.setCursor(0, 1);
122 lcd.print(lcdbuf); 180 lcd.print(lcdbuf);