EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

port ball reading code to 32u4

pull/1/head
EmaMaker 2020-11-21 16:55:56 +01:00
parent 41061931fc
commit 2d573211a5
1 changed files with 218 additions and 218 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

436
utility/ball_read/ball_read/ball_read.ino Executable file → Normal file
View File

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