Mercurial > ~darius > hgwebdir.cgi > splitbrain

comparison shore/shore.ino @ 0:fe0a2f223e10

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Initial commit of split brain code for the Sea Dragon.
author Daniel O'Connor <darius@dons.net.au>
date Sat, 11 Jul 2015 15:20:05 +0930
parents
children 7d4ec58da1d8
comparison
equal deleted inserted replaced
-1:000000000000 0:fe0a2f223e10
1 #include <avr/pgmspace.h>
2
3 #include <Servo.h>
4 #include <Wire.h>
5 #include <LiquidCrystal_I2C.h>
6 #include "log_lookup.h"
7
8 // DFRobot I/O Expansion Shield V7 - http://www.dfrobot.com/wiki/index.php/IO_Expansion_Shield_for_Arduino_V7_SKU:DFR0265
9 // 2xPS3 joysticks connected like so
10 // Joy 1 Horizontal - Left/right - AD0
11 // Joy 1 Vertical - Forward/back - AD1
12 // Joy 1 Button - Unused - D2
13 // Joy 2 Horizontal - Unused - AD2
14 // Joy 2 Vertical - Up/down - AD3
15 // Joy 2 Button - Unused - D3
16
17 // LCD panel
18 LiquidCrystal_I2C lcd(0x27, 16, 2); // I/O expander is at 0x27, LCD is 16x2
19
20 void setup() {
21 int byte;
22
23 Serial.begin(115200);
24
25 // Set joystick button pins as inputs
26 pinMode(7, INPUT);
27 pinMode(8, INPUT);
28
29 lcd.init();
30 lcd.backlight();
31 lcd.print("Hello world!");
32 }
33
34 void loop() {
35 int joy1X, joy1Y, joy2X, joy2Y, but1, but2;
36 int lint, rint, vint, ldir, rdir, vdir, ethrot;
37 float k, l, r, v, x, y;
38 char lcdbuf[16 + 1]; // Buffer 1 line
39
40 k = 1.5;
41
42 joy1X = analogRead(A0);
43 joy1Y = analogRead(A1);
44 but1 = !digitalRead(7); // Buttons are active low
45 joy2X = analogRead(A2);
46 joy2Y = analogRead(A3);
47 but2 = !digitalRead(8);
48
49 #if 0
50 Serial.print("Joy 1 X: "); Serial.print(joy1X); Serial.print(" ");
51 Serial.print("Y: "); Serial.print(joy1Y); Serial.print(" Button: ");
52 Serial.println(but1);
53 Serial.print("Joy 2 X: "); Serial.print(joy2X); Serial.print(" ");
54 Serial.print("Y: "); Serial.print(joy2Y); Serial.print(" Button: ");
55 Serial.println(but2);
56 #endif
57 // Create deadband in the centre because they don't sit at 512
58 if (joy1X > 500 && joy1X < 510) // Sits at 505
59 joy1X = 512;
60 if (joy1Y > 516 && joy1Y < 526) // Sits at 521
61 joy1Y = 512;
62 if (joy2X > 500 && joy2X < 510) // Sits at 505
63 joy2X = 512;
64 if (joy2Y > 520 && joy2Y < 530) // Sits at 525
65 joy2Y = 512;
66
67 // Mix 'joystick' input to left/right thrust (elevon mixing)
68 // 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);
73 r = -1 * (x / 2 - y / 2);
74
75 v = ((float)(joy2Y - 512) / 512.0);
76
77 // Apply some gain
78 l *= 2.2;
79 r *= 2.2;
80
81 // Limit
82 if (l > 1)
83 l = 1;
84 if (l < -1)
85 l = -1;
86 if (r > 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
95 // Map values to -255..255
96 lint = l * 255;
97 rint = r * 255;
98 vint = v * 255;
99
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 #if 0
109 Serial.print("l = "); Serial.print(l); Serial.print(" "); Serial.print(lint); Serial.print(" "); Serial.print(ldir);
110 Serial.print(" r = "); Serial.print(r); Serial.print(" "); Serial.print(rint); Serial.print(" "); Serial.print(r);
111 Serial.print(" v = "); Serial.print(vint); Serial.print(" "); Serial.println(vdir);
112 #else
113 Serial.print("L:");
114 Serial.print(lint * (ldir == 0 ? -1 : 1));
115 Serial.print(" R:");
116 Serial.print(rint * (rdir == 0 ? -1 : 1));
117 Serial.print(" V:");
118 Serial.print(vint * (vdir == 0 ? -1 : 1));
119 Serial.println();
120 #endif
121
122 snprintf(lcdbuf, sizeof(lcdbuf) - 1, "L%c%3dR%c%3dV%c%3d", ldir == 0 ? '-' : '+', lint,
123 rdir == 0 ? '-' : '+', rint, vdir == 0 ? '-' : '+', vint);
124 lcd.setCursor(0, 0);
125 lcd.print(lcdbuf);
126 snprintf(lcdbuf, sizeof(lcdbuf) - 1, " B1: %d B2: %d ", but1, but2);
127 lcd.setCursor(0, 1);
128 lcd.print(lcdbuf);
129 delay(200);
130 }
131
132 /*
133 * Local variables:
134 * mode: c++
135 * End:
136 */