diff --git a/Receiver/Receiver.ino b/Receiver/Receiver.ino index b014b8b..ee5a621 100644 --- a/Receiver/Receiver.ino +++ b/Receiver/Receiver.ino @@ -8,7 +8,8 @@ //TODO Füge alle möglichen bewegungen hinzu //TODO Mache es möglich Motoren geschwindigkeit mit analog zu kontolieren RF24 radio(7, 8); // (CE, CSN) - +int pwmx; +int pwmy; const byte address[6] = "1RF24"; struct dataStruct { int Xposition; @@ -18,13 +19,15 @@ void setup() { Serial.begin(9600); radio.begin(); radio.setPALevel(RF24_PA_MAX); - radio.setChannel(125); + radio.setChannel(50); radio.openReadingPipe(0, address); radio.startListening(); pinMode(2, OUTPUT); pinMode(3, OUTPUT); pinMode(4, OUTPUT); pinMode(5, OUTPUT); + pinMode(10, OUTPUT); + pinMode(11, OUTPUT); pinMode(A2, OUTPUT); pinMode(A3, OUTPUT); pinMode(A4, OUTPUT); @@ -40,6 +43,8 @@ void loop() { Serial.println(myData.Yposition); Serial.print("X Pos"); Serial.println(myData.Xposition); + pwmy=255-(myData.Yposition/4); + pwmx=255-(myData.Xposition/4); if (myData.Yposition >= 700) { digitalWrite(3, 1); @@ -50,6 +55,9 @@ void loop() { digitalWrite(A2, 0); digitalWrite(A4, 0); digitalWrite(A5, 1); + + analogWrite(10,pwmy); + analogWrite(11,pwmx); } if (myData.Yposition <= 200) { @@ -61,6 +69,8 @@ void loop() { digitalWrite(A3, 0); digitalWrite(A5, 0); digitalWrite(A4, 1); + analogWrite(10,pwmy); + analogWrite(11,pwmx); } if (myData.Xposition >= 700){ digitalWrite(3, 1); @@ -71,6 +81,8 @@ void loop() { digitalWrite(A2, 0); digitalWrite(A4, 1); digitalWrite(A5, 0); + analogWrite(10,pwmy); + analogWrite(11,pwmx); } if (myData.Xposition <= 200){ digitalWrite(3, 0); @@ -81,6 +93,8 @@ void loop() { digitalWrite(A2, 1); digitalWrite(A4, 0); digitalWrite(A5, 1); + analogWrite(10,pwmy); + analogWrite(11,pwmx); } if (myData.Yposition>200 && myData.Yposition<700 && myData.Xposition>200 && myData.Xposition<700) { diff --git a/Transceiver/Transceiver.ino b/Transceiver/Transceiver.ino index b575b91..bf2a6ac 100644 --- a/Transceiver/Transceiver.ino +++ b/Transceiver/Transceiver.ino @@ -12,7 +12,7 @@ struct dataStruct { void setup() { radio.begin(); radio.setPALevel(RF24_PA_MAX); - radio.setChannel(125); + radio.setChannel(50); radio.openWritingPipe(address); radio.stopListening(); Serial.begin(9600);