45 lines
1.2 KiB
Arduino
45 lines
1.2 KiB
Arduino
|
#define BTN0 A3
|
||
|
|
#define BTN1 A4
|
||
|
|
#define BTN2 A5
|
||
|
|
#define LED_Y 41
|
||
|
|
#define LED_R 22
|
||
|
|
|
||
|
|
void setup (){
|
||
|
|
Serial1.begin(19200);
|
||
|
|
|
||
|
|
pinMode(LED_Y,OUTPUT);
|
||
|
|
pinMode(LED_R,OUTPUT);
|
||
|
|
pinMode(BTN0,INPUT);
|
||
|
|
pinMode(BTN1,INPUT);
|
||
|
|
pinMode(BTN2,INPUT);
|
||
|
|
}
|
||
|
|
|
||
|
|
byte b, oldB = 0;
|
||
|
|
|
||
|
|
void loop () {
|
||
|
|
//We have three config. buttons, making a packet out of a single byte with a start header, so teensy can use it as it wants
|
||
|
|
//The header is signed by the two most important bits put high, so 128+64 in OR with other bits shifted by the needed ammount
|
||
|
|
//This approach only admits 5 configuration buttons, it should be enough
|
||
|
|
b = 0b11000111;
|
||
|
|
//b |= digitalRead(BTN0);
|
||
|
|
//b |= digitalRead(BTN1) << 1;
|
||
|
|
//b |= digitalRead(BTN2) << 2;
|
||
|
|
if(oldB != b) Serial1.write(b);
|
||
|
|
oldB = b;
|
||
|
|
|
||
|
|
while(Serial1.available() > 0){
|
||
|
|
int a = Serial1.read();
|
||
|
|
if((a & 0b11000000) == 0b11000000){
|
||
|
|
digitalWrite(LED_Y, !(a & 0b00000001));
|
||
|
|
digitalWrite(LED_R, !(a & 0b00000010));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
int angle = (int) (atan2(3,2) * 180 / 3.14);
|
||
|
|
angle = angle / 2 + angle % 2;
|
||
|
|
int dist = 256;
|
||
|
|
Serial1.write((char) 105); //Start packet
|
||
|
|
Serial1.write((char) angle); //Start packet
|
||
|
|
Serial1.write((char) dist); //Start packet
|
||
|
|
Serial1.write((char) 115); //Start packet
|
||
|
|
}
|