139 lines
3.0 KiB
C++
139 lines
3.0 KiB
C++
#include <SoftwareSerial.h>
|
|
#include <Time.h>
|
|
#include <RF24.h>
|
|
#include <nRF24L01.h>
|
|
#define RADIO_CHANNEL 42
|
|
#define RADIO_RATE RF24_250KBPS //RF24_1MBPS
|
|
#define RADIO_POWER RF24_PA_MAX
|
|
#define RADIO_CE 9
|
|
#define RADIO_CSN 10
|
|
|
|
const bool DEBUG = false;
|
|
|
|
const int gLedPin = 13;
|
|
const int gRxPin = 5;
|
|
const int gTxPin = 6;
|
|
|
|
const uint8_t pipe[6] = "Foxy1"; // radio address
|
|
RF24 radio(RADIO_CE, RADIO_CSN);
|
|
|
|
SoftwareSerial BTSerial(gRxPin, gTxPin);
|
|
|
|
bool input_lock = 0;
|
|
byte drive = 0;
|
|
|
|
struct Package {
|
|
byte steering; // 0 - off, 1 - turn left, 2 - turn right
|
|
byte drive; // 0 - full, 1 - back, 2 - front | same type as uint8_t
|
|
bool direction; // false - forward, true - backward
|
|
uint8_t speed; // int number 0-255
|
|
};
|
|
|
|
bool send_package(Package* pkg, int size) {
|
|
radio.write(pkg, size);
|
|
return 1; // loook what is this
|
|
}
|
|
|
|
bool Broadcast(Package pkg) {
|
|
Package pack = pkg;
|
|
return send_package(&pack, sizeof(pack));
|
|
}
|
|
|
|
Package buildPack(uint8_t speed, byte steering, bool direction) {
|
|
Package pack;
|
|
pack.speed = speed;
|
|
pack.steering = steering;
|
|
pack.direction = direction;
|
|
pack.drive = drive;
|
|
return pack;
|
|
}
|
|
|
|
void setup() {
|
|
BTSerial.begin(9600);
|
|
BTSerial.setTimeout(2);
|
|
if (DEBUG) {
|
|
Serial.begin(9600);
|
|
while (!Serial) {
|
|
;
|
|
}
|
|
Serial.println("Serial is listening...");
|
|
}
|
|
|
|
radio.begin();
|
|
radio.setChannel(RADIO_CHANNEL);
|
|
radio.setDataRate(RADIO_RATE);
|
|
radio.setPALevel(RADIO_POWER);
|
|
radio.openWritingPipe(pipe);
|
|
radio.setAutoAck(false);
|
|
radio.powerUp();
|
|
radio.stopListening();
|
|
}
|
|
|
|
void loop() {
|
|
if (BTSerial.available()) {
|
|
String stickInfo = BTSerial.readStringUntil('\n');
|
|
Serial.println(stickInfo);
|
|
if (stickInfo.length() != 8) {
|
|
if (DEBUG) {
|
|
Serial.print("Error: invalid bluetooth input: ");
|
|
Serial.println(stickInfo);
|
|
}
|
|
return;
|
|
}
|
|
int angle = stickInfo.substring(0,3).toInt();
|
|
uint8_t speed = map(stickInfo.substring(3,6).toInt(), 0, 100, 0, 255);
|
|
uint8_t button = stickInfo.substring(6).toInt(); // 1 - X, 2 - O, 3 - triangle, 4 - square
|
|
|
|
if (button == 1) {
|
|
input_lock = !input_lock;
|
|
if (DEBUG) {
|
|
Serial.print("Input Lock status: ");
|
|
Serial.println(input_lock);
|
|
}
|
|
}
|
|
|
|
if (input_lock) {
|
|
if (DEBUG) {
|
|
Serial.println("Input Lock!");
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (button == 3) {
|
|
drive++;
|
|
if (drive > 2) {
|
|
drive = 0;
|
|
}
|
|
if (DEBUG) {
|
|
Serial.print("Drive mode set to ");
|
|
Serial.println(drive);
|
|
}
|
|
}
|
|
|
|
byte steering = 0;
|
|
if (button != 1 && button != 3) { // 0 is default position that should set steering to 0
|
|
if (button == 2) {
|
|
steering = 2;
|
|
} else if (button == 4) {
|
|
steering = 1;
|
|
}
|
|
}
|
|
|
|
if (DEBUG) {
|
|
Serial.println(steering);
|
|
}
|
|
|
|
bool direction = 0;
|
|
if (angle > 180) {
|
|
direction = 1;
|
|
}
|
|
|
|
Package pack = buildPack(speed, steering, direction);
|
|
bool sent = Broadcast(pack);
|
|
if (DEBUG && sent) {
|
|
Serial.print("Package is sent: ");
|
|
Serial.println(stickInfo);
|
|
}
|
|
}
|
|
}
|