From 2d573211a5fa1667921fa3b7e8f57b3ee8c10a3d Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Sat, 21 Nov 2020 16:55:56 +0100 Subject: [PATCH] port ball reading code to 32u4 --- utility/ball_read/ball_read/ball_read.ino | 436 +++++++++++----------- 1 file changed, 218 insertions(+), 218 deletions(-) mode change 100755 => 100644 utility/ball_read/ball_read/ball_read.ino diff --git a/utility/ball_read/ball_read/ball_read.ino b/utility/ball_read/ball_read/ball_read.ino old mode 100755 new mode 100644 index 203d359..f3629cc --- a/utility/ball_read/ball_read/ball_read.ino +++ b/utility/ball_read/ball_read/ball_read.ino @@ -1,218 +1,218 @@ -/** - Sensor Mapping - Sensor Angle Pin Port - S0 0 A3 C3 - S1 A2 C2 - S2 A1 C1 - S3 A0 C0 - S4 90 13 B5 - S5 12 B4 - S6 11 B3 - S7 10 B2 - S8 180 9 B1 - S9 8 B0 - S10 7 D7 - S11 6 D6 - S12 270 5 D5 - S13 4 D4 - S14 3 D3 - S15 2 D2 - loop cycle duration: 3.2 millis -**/ - -#define S9 ((PINB & 1)) -#define S8 ((PINB & 2) >> 1) -#define S7 ((PINB & 4) >> 2) -#define S6 ((PINB & 8) >> 3) -#define S5 ((PINB & 16) >> 4) -#define S4 ((PINB & 32) >> 5) - -#define S3 ((PINC & 1)) -#define S2 ((PINC & 2) >> 1) -#define S1 ((PINC & 4) >> 2) -#define S0 ((PINC & 8) >> 3) - -#define S15 ((PIND & 4) >> 2) -#define S14 ((PIND & 8) >> 3) -#define S13 ((PIND & 16) >> 4) -#define S12 ((PIND & 32) >> 5) -#define S11 ((PIND & 64) >> 6) -#define S10 ((PIND & 128) >> 7) - -#define NCYCLES 350 -#define BROKEN 300 -#define TOO_LOW 60 - -int counter[16]; -int pins[] = {A3, A2, A1, A0, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2}; -int distance; -int nmax = 0; -int sensor = 0; - -int oldIndex = 0; -int oldDistance = 0; - -byte ballInfo = 0; - -float xs[16]; -float ys[16]; - -float angle = 0, dist = 0; -boolean sending = false; -byte sendAngle = 0, sendDistance = 0; -byte sendByte = 0; - -unsigned long t = 0; - -void setup() { - delay(1000); - - Serial.begin(57600); - - pinMode(2, INPUT); - pinMode(3, INPUT); - pinMode(4, INPUT); - pinMode(5, INPUT); - pinMode(6, INPUT); - pinMode(7, INPUT); - pinMode(8, INPUT); - pinMode(9, INPUT); - pinMode(10, INPUT); - pinMode(11, INPUT); - pinMode(12, INPUT); - pinMode(13, INPUT); - pinMode(A0, INPUT); - pinMode(A1, INPUT); - pinMode(A2, INPUT); - pinMode(A3, INPUT); - - pinMode(A4, OUTPUT); - - for (int i = 0; i < 16; i++) { - xs[i] = cos((22.5 * PI / 180) * i); - ys[i] = sin((22.5 * PI / 180) * i); - } -} - -void loop() { - readBallInterpolation(); - sendDataInterpolation(); -} - -/**--- READ BALL USING SENSORS ANGLE INTERPOLATION ---**/ - -void readBallInterpolation() { - for (int i = 0; i < 16; i++) { - counter[i] = 0; - } - - //reads from the register - for (int i = 0; i < NCYCLES; i++) { - counter[0] += !S0; - counter[1] += !S1; - counter[2] += !S2; - counter[3] += !S3; - counter[4] += !S4; - counter[5] += !S5; - counter[6] += !S6; - counter[7] += !S7; - counter[8] += !S8; - counter[9] += !S9; - counter[10] += !S10; - counter[11] += !S11; - counter[12] += !S12; - counter[13] += !S13; - counter[14] += !S14; - counter[15] += !S15; - } - - float x = 0, y = 0; - for (int i = 0; i < 16; i++) { - if (counter[i] > BROKEN || counter[i] < TOO_LOW) counter[i] = 0; - - x += xs[i] * counter[i]; - y += ys[i] * counter[i]; - } - - angle = atan2(y, x) * 180 / PI; - angle = ((int)(angle + 360)) % 360; - - //distance is 0 when not seeing ball - //dist = hypot(x, y); - - nmax = 0; - //saves max value and sensor - for (int i = 0; i < 16; i++) { - if (counter[i] > nmax) { - nmax = counter[i]; - } - } - - dist = nmax; - - //turn led on - if (dist == 0) { - digitalWrite(A4, LOW); - } else { - digitalWrite(A4, HIGH); - } -} - -void sendDataInterpolation() { - if(sending){ - sendAngle = ((byte) (angle / 2)) & 0b11111110; - Serial.write(sendAngle); - }else{ - sendDistance = dist; - if(sendDistance > 254) sendDistance = 254; - sendDistance = sendDistance |= 0b00000001; - Serial.write(sendDistance); - } - sending = !sending; -} - -void test() { - readBallInterpolation(); - - Serial.print(S0); - Serial.print(" | "); - Serial.print(S1); - Serial.print(" | "); - Serial.print(S2); - Serial.print(" | "); - Serial.print(S3); - Serial.print(" | "); - Serial.print(S4); - Serial.print(" | "); - Serial.print(S5); - Serial.print(" | "); - Serial.print(S6); - Serial.print(" | "); - Serial.print(S7); - Serial.print(" | "); - Serial.print(S8); - Serial.print(" | "); - Serial.print(S9); - Serial.print(" | "); - Serial.print(S10); - Serial.print(" | "); - Serial.print(S11); - Serial.print(" | "); - Serial.print(S12); - Serial.print(" | "); - Serial.print(S13); - Serial.print(" | "); - Serial.print(S14); - Serial.print(" | "); - Serial.print(S15); - Serial.print(" () "); - Serial.println(sensor); -} - -void printCounter() { - for (int i = 0; i < 16; i++) { - Serial.print(counter[i]); - Serial.print(" | "); - } - Serial.println(); -} +/** + Sensor Mapping + Sensor Angle Pin Port + S1 0 26 PD6 + S2 25 PD4 + S3 19 PD1 + S4 18 PD0 + S5 90 1 PE6 + S6 40 PF1 + S7 39 PF4 + S8 38 PF5 + S9 180 37 PF6 + S10 36 PF7 + S11 32 PC7 + S12 31 PC6 + S13 270 30 PB6 + S14 29 PB5 + S15 28 PB4 + S16 27 PD7 + loop cycle duration: 3.2 millis +**/ + +#define S5 ((PORTE & 64 ) >> 6) + +#define S11 ((PORTC & 128 ) >> 7) +#define S12 ((PORTC & 64 ) >> 6) + +#define S13 ((PORTB & 64 ) >> 6) +#define S14 ((PORTB & 32 ) >> 5) +#define S15 ((PORTB & 16) >> 4) + +#define S1 ((PORTD & 64 ) >> 6) +#define S2 ((PORTD & 16 ) >> 4) +#define S3 ((PORTD & 2 ) >> 1) +#define S4 (PORTD & 1 ) +#define S16 ((PORTD & 128) >> 7) + +#define S6 ((PORTF & 2 ) >> 1) +#define S7 ((PORTF & 16 ) >> 4) +#define S8 ((PORTF & 32 ) >> 5) +#define S9 ((PORTF & 64 ) >> 6) +#define S10 ((PORTF & 128 ) >> 7) + +#define NCYCLES 350 +#define BROKEN 300 +#define TOO_LOW 60 + +int counter[16]; +int distance; +int nmax = 0; +int sensor = 0; + +int oldIndex = 0; +int oldDistance = 0; + +byte ballInfo = 0; + +float xs[16]; +float ys[16]; + +float angle = 0, dist = 0; +boolean sending = false; +byte sendAngle = 0, sendDistance = 0; +byte sendByte = 0; + +unsigned long t = 0; +t +void setup() { + delay(1000); + + Serial.begin(57600); + + pinMode(26, INPUT); //S1 + pinMode(25, INPUT); //S2 + pinMode(19, INPUT); //S3 + pinMode(18, INPUT); //S4 + pinMode(1, INPUT); //S5 + pinMode(40, INPUT); //S6 + pinMode(39, INPUT); //S7 + pinMode(38, INPUT); //S8 + pinMode(37, INPUT); //S9 + pinMode(36, INPUT); //S10 + pinMode(32, INPUT); //S11 + pinMode(31, INPUT); //S12 + pinMode(30, INPUT); //S13 + pinMode(29, INPUT); //S14 + pinMode(28, INPUT); //S15 + pinMode(27, INPUT); //S16 + + pinMode(22, OUTPUT); //LED1 + + for (int i = 0; i < 16; i++) { + xs[i] = cos((22.5 * PI / 180) * i); + ys[i] = sin((22.5 * PI / 180) * i); + } +} + +void loop() { + readBallInterpolation(); + sendDataInterpolation(); +} + +/**--- READ BALL USING SENSORS ANGLE INTERPOLATION ---**/ + +void readBallInterpolation() { + for (int i = 0; i < 16; i++) { + counter[i] = 0; + } + + //reads from the register + for (int i = 0; i < NCYCLES; i++) { + counter[0] += !S1; + counter[1] += !S2; + counter[2] += !S3; + counter[3] += !S4; + counter[4] += !S5; + counter[5] += !S6; + counter[6] += !S7; + counter[7] += !S8; + counter[8] += !S9; + counter[9] += !S10; + counter[10] += !S11; + counter[11] += !S12; + counter[12] += !S13; + counter[13] += !S14; + counter[14] += !S15; + counter[15] += !S16; + } + + float x = 0, y = 0; + for (int i = 0; i < 16; i++) { + if (counter[i] > BROKEN || counter[i] < TOO_LOW) counter[i] = 0; + + x += xs[i] * counter[i]; + y += ys[i] * counter[i]; + } + + angle = atan2(y, x) * 180 / PI; + angle = ((int)(angle + 360)) % 360; + + //distance is 0 when not seeing ball + //dist = hypot(x, y); + + nmax = 0; + //saves max value and sensor + for (int i = 0; i < 16; i++) { + if (counter[i] > nmax) { + nmax = counter[i]; + } + } + + dist = nmax; + + //turn led on + if (dist == 0) { + digitalWrite(22, LOW); + } else { + digitalWrite(22, HIGH); + } +} + +void sendDataInterpolation() { + if(sending){ + sendAngle = ((byte) (angle / 2)) & 0b11111110; + Serial.write(sendAngle); + }else{ + sendDistance = map(sendDistance, 0, NCYCLES, 254, 0); + sendDistance = sendDistance |= 0b00000001; + Serial.write(sendDistance); + } + sending = !sending; +} + +void test() { + readBallInterpolation(); + + Serial.print(S1); + Serial.print(" | "); + Serial.print(S2); + Serial.print(" | "); + Serial.print(S3); + Serial.print(" | "); + Serial.print(S4); + Serial.print(" | "); + Serial.print(S5); + Serial.print(" | "); + Serial.print(S6); + Serial.print(" | "); + Serial.print(S7); + Serial.print(" | "); + Serial.print(S8); + Serial.print(" | "); + Serial.print(S9); + Serial.print(" | "); + Serial.print(S10); + Serial.print(" | "); + Serial.print(S11); + Serial.print(" | "); + Serial.print(S12); + Serial.print(" | "); + Serial.print(S13); + Serial.print(" | "); + Serial.print(S14); + Serial.print(" | "); + Serial.print(S15); + Serial.print(" | "); + Serial.print(S16); + Serial.print(" () "); + Serial.println(sensor); +} + +void printCounter() { + for (int i = 0; i < 16; i++) { + Serial.print(counter[i]); + Serial.print(" | "); + } + Serial.println(); +}