Commit 9234adaa authored by Ondřej Kučera's avatar Ondřej Kučera
Browse files

add ACKs, link detection, LED light

parent 646ccaf6
/*
* BLDC driver example
*/
#include <Servo.h>
Servo ESC;
int val;
void setup() {
ESC.attach(8, 1000, 2000);
Serial.begin(9600);
}
void loop() {
val = analogRead(A0);
val = map(val, 0, 1023, 0, 180);
ESC.write(val);
Serial.println(val);
}
#include <RFM69.h>
#include <SPI.h>
#include <Servo.h>
//radio
#define NETWORKID 5 // Must be the same for all nodes (0 to 255)
#define MYNODEID 2 // My node ID (0 to 255)
#define TONODEID 1 // Destination node ID (0 to 254, 255 = broadcast)
#define FREQUENCY RF69_433MHZ // Frequency set up
#define FREQUENCYSPECIFIC 433000000 // Should be value in Hz
#define CHIP_SELECT_PIN 8 //radio chip select
#define INTERUP_PIN 3 // itr for receiving
#define IS_RFM69HW_HW //high power version
//pins
#define TURN_A 5
#define TURN_B 10
#define GO 9
#define LED 6
RFM69 radio(CHIP_SELECT_PIN, INTERUP_PIN, true); //radio
Servo ESC;
int SPEED = 0;
int STEER = 0;
int BUTTON = 0;
bool btn_last = false;
long unsigned int last_received;
bool gotSignal = false;
int16_t rssi;
// IN struct
typedef struct {
int speed;
int steer;
bool button;
} messageIn;
void setup() {
//pin setup
ESC.attach(GO, 1000, 2000);
ESC.write(SPEED);
pinMode(TURN_A, OUTPUT);
pinMode(TURN_B, OUTPUT);
pinMode(LED, OUTPUT);
analogWrite(LED, 0);
digitalWrite(TURN_A, LOW);
digitalWrite(TURN_B, LOW);
//serial
Serial.begin(115200);
//radio
if(!radio.initialize(FREQUENCY, MYNODEID, NETWORKID)) {
Serial.println("RFM69HW initialization failed!");
} else {
radio.setFrequency(FREQUENCYSPECIFIC);
radio.setHighPower(true); // RFM69HW
radio.setPowerLevel(31); // max power
}
Serial.println("SETUP finished");
}
void loop() {
// process received data, reply an ACK if required
if (radio.receiveDone()) {
messageIn incoming = *(messageIn*)radio.DATA;
SPEED = incoming.speed;
STEER = incoming.steer;
bool _button = incoming.button;
if (_button && btn_last != _button) {
if (BUTTON < 4) {BUTTON++;}
else {BUTTON = 0;}
}
btn_last = _button;
if (radio.ACKRequested()) { //send ACK
radio.sendACK();
}
last_received = millis(); // signal check
}
//rssi = radio.readRSSI();
// check for signal
if ((millis() - last_received) > 250) {
gotSignal = false;
} else { gotSignal = true;}
if (!gotSignal) {
SPEED = 0;
STEER = 0;
}
//main motor control
if (SPEED>=0) {
ESC.write(map(SPEED, 0, 255, 0, 180));
}
//steering control
if (STEER<-50) { // turn left
digitalWrite(TURN_A, LOW);
digitalWrite(TURN_B, HIGH);
}
if (STEER>50) {// turn right
digitalWrite(TURN_A, HIGH);
digitalWrite(TURN_B, LOW);
}
if (STEER>-50 && STEER<50) {
digitalWrite(TURN_A, LOW);
digitalWrite(TURN_B, LOW);
}
//light control
switch (BUTTON) {
case 0:
analogWrite(LED, 0);
break;
case 1:
analogWrite(LED, 16);
break;
case 2:
analogWrite(LED, 32);
break;
case 3:
analogWrite(LED, 64);
break;
case 4:
analogWrite(LED, 128);
break;
default:
analogWrite(LED, 0);
break;
}
//Serial.println((String)SPEED + " " + (String)STEER + " " +
//(String)BUTTON + " " + (String)rssi);
}
#include <RFM69.h>
#include <SPI.h>
//radio
#define NETWORKID 5 // Must be the same for all nodes (0 to 255)
#define MYNODEID 1 // My node ID (0 to 255)
#define TONODEID 2 // Destination node ID (0 to 254, 255 = broadcast)
#define FREQUENCY RF69_433MHZ // Frequency set up
#define FREQUENCYSPECIFIC 433000000 // Should be value in Hz
#define CHIP_SELECT_PIN 10 //radio chip select
#define INTERUP_PIN 2
#define IS_RFM69HW_HW
//pins
#define GO A1
#define TURN A2
#define BTN 9 //btn
#define ACK_LED 3
RFM69 radio(CHIP_SELECT_PIN, INTERUP_PIN, true); //radio
// main vars
int SPEED = 0;
int STEER = 0;
bool BUTTON = false;
// steering bounds
int STRAIGHT = 511;
int STALL = 511;
bool ACK = false;
// OUT struct
typedef struct {
int speed;
int steer;
bool button;
} messageOut;
void setup() {
//pins
pinMode(GO, INPUT);
pinMode(STEER, INPUT);
pinMode(BTN, INPUT);
pinMode(ACK_LED, OUTPUT);
digitalWrite(ACK_LED, LOW);
//auto calibration
STRAIGHT = analogRead(TURN);
STALL = analogRead(GO);
//serial
Serial.begin(115200);
//radio
if (!radio.initialize(FREQUENCY, MYNODEID, NETWORKID)) {
Serial.println("RFM69HW initialization failed!");
} else {
radio.setFrequency(FREQUENCYSPECIFIC);
radio.setHighPower(true); // RFM69HW
radio.setPowerLevel(31); // max power
}
Serial.println("SETUP finished");
}
void loop() {
readController();
sendData();
if (ACK) {digitalWrite(ACK_LED, HIGH);}
else {digitalWrite(ACK_LED, LOW);}
}
void readController() {
BUTTON = digitalRead(BTN);
int _speed = analogRead(GO);
int _steer = analogRead(TURN);
if ((_speed <= STALL+10) & (_speed >= STALL-10)) { // offset in the middle
SPEED = 0;
} else {
SPEED = map(_speed, 0, 1023, -255, 255); // convert to speed levels
}
if ((_steer <= STRAIGHT+10) & (_steer >= STRAIGHT-10)) { // offset in the middle
STEER = 0;
} else {
STEER = map(_steer, 0, 1023, -255, 255); // convert to steering levels
}
}
void sendData() {
// create a struct & fill it w/ data
messageOut data;
data.speed = SPEED;
data.steer = STEER;
data.button = BUTTON;
//send the struct
if(radio.sendWithRetry(TONODEID, (const void*)&data, sizeof(data))) {
ACK = true;
} else {ACK = false;}
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment