int main()
{
std::cout << "just work";
XBee xB;
std::cout << "created xB";
xB.setup();
std::cout << "after setup";
/* UDP udp;
udp.setup();
for (int i = 0;i < 20;i++) {
udp.sendCommand(1);
}
usleep(3000000);
for (int i = 0; i < 20;i++) {
udp.sendCommand(7);
}*/
return 0;
}
/* Transmission Code */
void Transmit_Packet(void)
{
/* Create Xbee object */
XBee xbee = XBee();
/* Packet to be transmitted */
uint8_t payload[MAX_SIZE];
/* Obtain address of receiving end */
XBeeAddress64 addr64 = XBeeAddress64(0x0, 0x0);
ZBTxStatusResponse txStatus = ZBTxStatusResponse();
/* Clear the payload */
memset(payload, '\0', sizeof(payload));
/* Debug Packet */
//payload[0] = '1';
//payload[1] = '5';
/* Transfer information into payload */
memcpy(payload, &G_packet, sizeof(payload));
for(int i = 0; i < MAX_SIZE; i++)
{ Serial.write(payload[i]);}
ZBTxRequest zbTx = ZBTxRequest(addr64, payload, sizeof(payload));
/* Transfer the payload */
xbee.send(zbTx); //!!Prints packet to serial monitor
/* delay */
delay(1000);
}
void Monitor::GetPacket(char* data, size_t len)
{
int i;
XBee x;
for(i=0;i<len;i++)
{
switch( state )
{
case WAIT_FOR_PACKET_START:
if( data[i] == XBEE_START_CHAR )
{
state = WAIT_FOR_LEN1;
//cmdBox->AppendText(_("Got Packet Start\n"));
}
break;
case WAIT_FOR_LEN1:
state = WAIT_FOR_LEN2;
packetsize = data[i];
packetsize <<= 8;
packetsize &= 0x0ff00;
break;
case WAIT_FOR_LEN2:
state = RECEIVE_DATA;
packetsize |= (0x0ff & data[i]);
packetcount = 0;
printf("Got Size: %02x\n", packetsize );
break;
case RECEIVE_DATA:
if(data[i] == XBEE_ESC_CHAR)
state = GOT_ESCAPE;
else
{
packet[packetcount++] = data[i];
if( packetcount >= packetsize ) state = WAIT_FOR_CHECKSUM;
}
//printf("Data: %02x\n", data[i]);
//cmd.Clear();
//cmd << data[i];
//cmdBox->AppendText(cmd);
break;
case GOT_ESCAPE:
packet[packetcount++] = data[i] ^ 0x20;
if( packetcount >= packetsize ) state = WAIT_FOR_CHECKSUM;
else state = RECEIVE_DATA;
break;
case WAIT_FOR_CHECKSUM:
if( x.Checksum(packet, packetcount) == data[i] & 0x0ff )
{
printf("Got good checksum\n");
Process();
}
state = WAIT_FOR_PACKET_START;
break;
}
}
printf("exit getpacket\n");
}
// Flex sensor task
void flexTask(void* p) {
Serial.println("IN FLEX TASK");
while (1) {
if (deviceRdy) {
int readRaw = analogRead(flexpin);
//Serial.println(readRaw);
int frameLen = 0;
unsigned char outFrame[Q_SIZE];
unsigned char outMsg[2];
memcpy(outMsg, "4", 1);
if (readRaw < 490 && handStatus == open) {
if (counterForFlex[0] >= 5) {
// send data to rpi to inform that hand has been closed for 2 cycles
handStatus = close;
memcpy(&outMsg[1], "1", 1);
frameLen = xbee.Send(outMsg, 2, outFrame, RPI_ADDR);
Serial1.write(outFrame, frameLen);
Serial.println("ON");
counterForFlex[0] = 0; //reset counter
}
else {
counterForFlex[0]++; //increment counter
counterForFlex[1] = 0;
}
} else if (readRaw >= 500 && handStatus == close) {
if (counterForFlex[1] >= 5) {
// send data to rpi to inform that hand has been opened for 2 cycles;
handStatus = open;
memcpy(&outMsg[1], "0", 1);
frameLen = xbee.Send(outMsg, 2, outFrame, RPI_ADDR);
Serial1.write(outFrame, frameLen);
Serial.println("OFF");
counterForFlex[1] = 0; //reset counter
}
else {
counterForFlex[1]++; //reset counters
counterForFlex[0] = 0;
}
}
else {
counterForFlex[0] = 0;
counterForFlex[1] = 0;
}
}
vTaskDelay(200);
}
}
int testAT()
{
XBee x;
int i, j, assert=1;
char cmd[8];
char *known = "\x7e\x00\x04\x08\x41\x53\x48\x1b";
i = x.LocalAT((char*)"SH", (char*)NULL, 2, cmd);
for(j=0;j<i;j++)
if( cmd[j] != known[j] ) assert = 0;
return assert;
}
void XBeeStrategy::run(){
xbee.readPacket();
if (xbee.getResponse().isAvailable()) {
// got something
if (xbee.getResponse().getApiId() == RX_16_RESPONSE) {
// got a rx packet
Rx16Response rx16 = Rx16Response();
xbee.getResponse().getRx16Response(rx16);
Serial.println( "**** Incoming transmission ***");
char destination = rx16.getData(0);
if(destination == 'a'){
Serial.println( "Triggering us...");
char mode = rx16.getData(1);
if(mode == 'm'){
Serial.println( "Changing direction");
int direction = rx16.getData(2);
int on = rx16.getData(3);
switch(direction){
case 'w':
sputnik.stopMoving();
sputnik.forward();
break;
case 'a':
sputnik.stopMoving();
sputnik.turn(1000, 0);
break;
case 's':
sputnik.stopMoving();
sputnik.backward();
break;
case 'd':
sputnik.stopMoving();
sputnik.turn(1000, 1);
break;
default:
break;
}
}
}
}
}
}
/* Initialize objects */
void setup()
{
/* Variable Initialization */
count = 0;
/* Create XBee Object */
XBee xbee = XBee();
/* Initialization */
init1(); //initialize sensors
Serial.begin(9600);
xbee.begin(Serial);
//Serial.println("Main"); //debug main
// initHealthSamples();
}
void setup() {
Serial.begin(9600);
xbee.setSerial(Serial);
centerX = analogRead(JX);
centerY = analogRead(JY);
pinMode(SPIN, INPUT);
}
//discover target node
void discover() {
//mySerial.println("discover");
//if we don't get a address we can't fire
while (true) {
//send node discovery
xbee.send(atRequest);
//default value is that responding XBEE can wait up to six seconds before answering
//so spamming it with node discoverys might be a bad thing, but waiting so long is booring so
//we we'll try it and see if it works...
//knight rider on the diodes let's the users know we're looking
for (int i=0; i<CHANNELS; i++) {
clearLeds();
digitalWrite(channels[i % CHANNELS].led_pin,HIGH);
if (checkNDResponse()) {
return;
}
}
for (int i=CHANNELS-1; i>=0; i--) {
clearLeds();
digitalWrite(channels[i % CHANNELS].led_pin,HIGH);
if (checkNDResponse()) {
return;
}
}
}
}
boolean checkNDResponse() {
//mySerial.println("checkNDResponse");
// wait a small bit so the animation looks good
if (xbee.readPacket(ND_WAIT_TIME / 6)) {
// got a response!
// should be an AT command response
if (xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE) {
xbee.getResponse().getAtCommandResponse(atResponse);
if (atResponse.isOk()) {
if (atResponse.getCommand()[0] == atCmd[0] && atResponse.getCommand()[1] == atCmd[1] && atResponse.getValueLength() > 3) {
//mySerial.println(pack(atResponse.getValue()[2],atResponse.getValue()[3],atResponse.getValue()[4],atResponse.getValue()[5]));
//mySerial.println(pack(atResponse.getValue()[6],atResponse.getValue()[7],atResponse.getValue()[8],atResponse.getValue()[9]));
addr64 = XBeeAddress64( pack(atResponse.getValue()[2],atResponse.getValue()[3],atResponse.getValue()[4],atResponse.getValue()[5]),pack(atResponse.getValue()[6],atResponse.getValue()[7],atResponse.getValue()[8],atResponse.getValue()[9]) );
return true;
}
}
else {
//nss.print("Command return error code: ");
//nss.println(atResponse.getStatus(), HEX);
nr(1);
}
} else {
//nss.print("Expected AT response but got ");
//nss.print(xbee.getResponse().getApiId(), HEX);
nr(2);
}
} else {
// at command failed
if (xbee.getResponse().isError()) {
//nss.print("Error reading packet. Error code: ");
//nss.println(xbee.getResponse().getErrorCode());
nr(3);
}
else {
//nss.print("No response from radio");
nr(4);
}
}
return false;
}
EXPORT libxbee::Con::Con(XBee &parent, std::string type, struct xbee_conAddress *address) : parent(parent) {
xbee_err ret;
if ((xbee = parent.getHnd()) == NULL) throw(XBEE_EINVAL);
if ((ret = xbee_conNew(xbee, &con, type.c_str(), address)) != XBEE_ENONE) throw(ret);
if ((ret = xbee_conDataSet(con, (void*)this, NULL)) != XBEE_ENONE) {
xbee_conEnd(con);
throw(ret);
}
try {
parent.conRegister(this);
} catch (xbee_err ret) {
xbee_conEnd(con);
throw(ret);
}
}
void checkSerialAPI(){
xbee.readPacket();
if (xbee.getResponse().isAvailable()) {
if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
xbee.getResponse().getZBRxResponse(rx);
if (rx.getOption() == ZB_PACKET_ACKNOWLEDGED) {
} else {}
int len = rx.getDataLength();
char m[len+1];
for(int i=0;i<len;i++){
m[i]=rx.getData(i);
}
m[len]='\0';
splitString(m, CMD,DEL,BRI, R, G ,B, TOUT);
t=millis() + TOUT * 1000;
} else if (xbee.getResponse().getApiId() == MODEM_STATUS_RESPONSE) {
xbee.getResponse().getModemStatusResponse(msr);
if (msr.getStatus() == ASSOCIATED) {}
else if (msr.getStatus() == DISASSOCIATED) {}
else {}
} else {}
} else if (xbee.getResponse().isError()) {}
}
void setup() {
pixels.begin();
t = millis() + TOUT * 1000;
CMD[0] = 'F';
//Change to Serial1 if using micro
Serial.begin(9600);
xbee.begin(Serial);
}
void setupold() {
//mySerial.begin(4800);
//mySerial.println("Hello world");
for (int i= 0; i<CHANNELS; i++) {
pinMode(channels[i].button_pin,INPUT);
digitalWrite(channels[i].button_pin,HIGH); //enable internal 20K pullup
pinMode(channels[i].led_pin,OUTPUT);
//blink leds a bit
digitalWrite(channels[i].led_pin,HIGH);
delay(200);
digitalWrite(channels[i].led_pin,LOW);
}
//debug led
//pinMode(13,OUTPUT);
//digitalWrite(13,HIGH);
//delay(500);
//digitalWrite(13,LOW);
xbee.begin(XBEE_BAUD);
//discover the other XBEE's address
discover();
zbTx = ZBTxRequest(addr64, payload, sizeof(payload));
//send a no-op packet so that the xbees can do their magic and find each other
payload[0] = 254;
xbee.send(zbTx);
//Flash all leds once so the user knows
flashAll(500);
//mySerial.println("Discovered address");
//mySerial.print("MSB: ");
//mySerial.println(addr64.getMsb());
//mySerial.println(addr64.getMsb()==0x0013a200?"Yes!":"NO");
//mySerial.print("LSB: ");
//mySerial.println(addr64.getLsb());
//mySerial.println(addr64.getLsb()==0x403141DA?"Yes!":"NO");
}
void loopold() {
int val;
int m;
for (uint8_t i= 0; i<CHANNELS; i++) {
m = millis();
if (channels[i].state == 0 || channels[i].state == 2) {
val = digitalRead(channels[i].button_pin);
if (channels[i].state == 0 && val == LOW) {
//a press!, fire!
uint8_t cc = i;
//special case, we can fire all channels by firing the first button repeatably
if (i == 0) {
cc = channel_count;
channel_count = (channel_count + 1) % CHANNELS;
}
//fire!
payload[0] = cc;
xbee.send(zbTx);
//set as fired
fired_channels |= (1 << cc);
digitalWrite(channels[cc].led_pin,HIGH);
//check if all is fired
if (fired_channels == B00111111) {
//wait a bit
delay(500);
//reset all
channel_count = 0;
fired_channels = 0;
for (int j = 0; j<CHANNELS; j++) {
channels[j].state = 0;
digitalWrite(channels[j].led_pin,LOW);
delay(300);
}
break;
}
}
if ((channels[i].state == 0 && val == LOW) || (channels[i].state == 2 && val == HIGH)) {
channels[i].state = (channels[i].state + 1) % 4; //change state
channels[i].time = m;
}
} else if (m - channels[i].time > THRESHHOLD) {
channels[i].state = (channels[i].state + 1) % 4; //change state
}
}
}
void XBeeActive::sendRemoteAtCommand()
{
DEBAG.println("\nSending command sendRemoteAtCommand to the XBee");
xbee.send(remoteAtRequest);
// wait up to 5 seconds for the status response
if (xbee.readPacket(5000))
{
// got a response!
// should be an AT command response
if (xbee.getResponse().getApiId() == REMOTE_AT_COMMAND_RESPONSE)
{
xbee.getResponse().getRemoteAtCommandResponse(remoteAtResponse);
if (remoteAtResponse.isOk()) {
DEBAG.print("Command [");
DEBAG.print(remoteAtResponse.getCommand()[0]);
DEBAG.print(remoteAtResponse.getCommand()[1]);
DEBAG.println("] was successful!");
if (remoteAtResponse.getValueLength() > 0) {
DEBAG.print("Command value length is ");
DEBAG.println(remoteAtResponse.getValueLength(), DEC);
DEBAG.print("Command value: ");
for (int i = 0; i < remoteAtResponse.getValueLength(); i++) {
DEBAG.print(remoteAtResponse.getValue()[i], HEX);
DEBAG.print(" ");
}
DEBAG.println("");
}
}
else {
DEBAG.print("Command returned error code: ");
DEBAG.println(remoteAtResponse.getStatus(), HEX);
}
}
else
{
DEBAG.print("Expected Remote AT response but got ");
DEBAG.print(xbee.getResponse().getApiId(), HEX);
}
}
else if (xbee.getResponse().isError())
{
DEBAG.print("Error reading packet. Error code: ");
DEBAG.println(xbee.getResponse().getErrorCode());
}
else
{
DEBAG.print("No response from radio3");
}
}
void sendWeather(string temperature)
{
char buffer[20];
strcpy(buffer, "WT:");
strcat(buffer, temperature.c_str());
strcat(buffer, "\n");
if (!xBeePort.empty())
{
tx16 = Tx16Request(destXbeeID, (uint8_t *)buffer, strlen(buffer));
xbee.send(tx16);
}
if (!serialPort.empty())
{
gmBuffSerial->WriteData((uint8_t *)buffer, strlen(buffer));
}
}
void sendTime()
{
char buffer[20];
strcpy(buffer, "TI:");
DateTime::GetTime(buffer + strlen(buffer), 20 - strlen(buffer), true, false);
strcat(buffer, "\n");
if (!xBeePort.empty())
{
tx16 = Tx16Request(destXbeeID, (uint8_t *)buffer, strlen(buffer));
xbee.send(tx16);
}
if (!serialPort.empty())
{
gmBuffSerial->WriteData((uint8_t *)buffer, strlen(buffer));
}
}
void loop() {
String cmd = cal();
for(int i=0;i<cmd.length();i++){
payload[i] = cmd[i];
}
payload[cmd.length()] = ',';
if(lastx!=x||lasty!=y||lasts!=s){
xbee.send(zbTx);
lastx=x;
lasty=y;
lasts=s;
}
delay(10);
}
void XBeeActive::sendAtCommand()
{
int i10;
xbee.send(atRequest);
if (xbee.readPacket(5000)) // подождите до 5 секунд для ответа состояния
{
if (xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE) // Должна быть команда отклика AT
{
xbee.getResponse().getAtCommandResponse(atResponse);
if (atResponse.isOk())
{
//DEBAG.print("Command [");
//DEBAG.print(atResponse.getCommand()[0]);
//DEBAG.print(atResponse.getCommand()[1]);
//DEBAG.println("] was successful!");
if (atResponse.getValueLength() > 0)
{
Len_XBee = atResponse.getValueLength();
//DEBAG.print("\nCommand value length is - ");
//DEBAG.println(atResponse.getValueLength(), DEC);
//DEBAG.print("Command value: ");
int i11 = Len_XBee - 1;
info_XBee_data1[0] = 0;
info_XBee_data1[1] = 0;
info_XBee_data1[2] = 0;
info_XBee_data1[3] = 0;
for (i10 = 0; i10 < atResponse.getValueLength(); i10++)
{
info_XBee_data[i10] = atResponse.getValue()[i10];
//DEBAG.print(info_XBee_data[i10], HEX);
//DEBAG.print(" ");
info_XBee_data1[i11] = info_XBee_data[i10];
i11--;
}
//DEBAG.println("");
}
}
else
{
//DEBAG.print("Command return error code: ");
//DEBAG.println(atResponse.getStatus(), HEX);
}
}
else
{
//DEBAG.print("Expected AT response but got ");
//DEBAG.println(xbee.getResponse().getApiId(), HEX);
}
}
else
{
if (xbee.getResponse().isError()) // at command failed
{
//DEBAG.print("Error reading packet. Error code: ");
//DEBAG.println(xbee.getResponse().getErrorCode());
}
else
{
//DEBAG.println("No response from radio1");
}
}
delay(200);
}
int main(int argc, char *argv[])
{
int bytesAvail;
int byteRead;
int bufferIndex;
if (readIni("SensorRelay.ini") != 0)
{
printf("Ini file SensorRelay.ini not found! See ya.\n");
exit(1);
}
//!!!!!!!!!!!!!!!TEST CODE - REMOVE!!!!!!!!!!!!!!!!!!!!!!!!
//SITE testSite = sites["XivelyHome"];
//GMXively *test = new GMXively(testSite.apiKey.c_str());
//test->SendData(testSite.feedID, "TempOffice2", 23.45);
nextUploadCheck = DateTime::seconds() + 60;
nextWeatherUpdateTime = DateTime::seconds() + 10; // get outside temperature right away
nextSendTime = DateTime::seconds() + 10; // send time to sensor right away
GMUtils::Initialize(logFileName.c_str(), debugMode);
GMUtils::Log("SensorRelay v053116 starting...");
if (!wunderAPI.empty())
{
weather = new GMWunderground(wunderAPI, wunderCountry, wunderCity, 0);
}
if (!xBeePort.empty())
{
// NOTE - the original code I used didn't disable flow control so I'm leaving it enabled. Should work without it, though
gmSerial = new GMSerial(xBeePort.c_str(), 9600, true);
if (gmSerial->Open() != 0)
{
GMUtils::Log("Unable to open serial port %s. See ya!", xBeePort.c_str());
exit(1);
}
xbee.setSerial(*gmSerial);
}
if (!serialPort.empty())
{
gmBuffSerial = new GMBufferedSerial(serialPort.c_str(), 9600, false);
if (gmBuffSerial->Open_USB() != 0)
{
GMUtils::Log("Unable to open serial port %s. See ya!", serialPort.c_str());
exit(1);
}
}
while (1)
{
if (!xBeePort.empty())
{
xbee.readPacket();
if (xbee.getResponse().isAvailable())
{
// got something
uint8_t apiID = xbee.getResponse().getApiId();
if (apiID == RX_16_RESPONSE || apiID == RX_64_RESPONSE)
{
// got a rx packet
if (apiID == RX_16_RESPONSE)
{
xbee.getResponse().getRx16Response(rx16);
//option = rx16.getOption();
//data = rx16.getData(0);
//memcpy(buffer, rx16.getData(), rx16.getDataLength());
string data = string((char *)rx16.getData(), rx16.getDataLength());
//buffer[rx16.getDataLength() + 1] = 0x00;
if (debugMode)
{
GMUtils::Write("Remote address %d, RSSI %d, data length %d", rx16.getRemoteAddress16(), rx16.getRssi(), rx16.getDataLength());
GMUtils::Write("Data %s", data.c_str());
}
processSensorData(data);
}
//else
//{
//xbee.getResponse().getRx64Response(rx64);
//option = rx64.getOption();
//data = rx64.getData(0);
//}
// TODO check option, rssi bytes
//flashLed(statusLed, 1, 10);
//
//// set dataLed PWM to value of the first byte in the data
//analogWrite(dataLed, data);
}
else if (xbee.getResponse().getApiId() == RX_16_IO_RESPONSE)
{
xbee.getResponse().getRx16IoSampleResponse(ioSample);
uint16_t address = ioSample.getRemoteAddress16();
if (debugMode)
{
for (int k = 0; k < ioSample.getSampleSize(); k++)
{
printf("Sample %d: ", k + 1);
// assume that any analog sample will be in the first 5 pins
for (int i = 0; i <= 5; i++)
{
if (ioSample.isAnalogEnabled(i))
{
printf("A%d = %d", i, ioSample.getAnalog(i, k));
}
}
printf("\n");
}
}
// assume that any analog sample will be in the first 5 pins
for (int i = 0; i <= 5; i++)
{
if (ioSample.isAnalogEnabled(i))
{
char tempbuffer[20];
sprintf(tempbuffer, "%d.%d", address, i);
string sensorID = string(tempbuffer);
double values = 0;
for (int k = 0; k < ioSample.getSampleSize(); k++)
{
values += ioSample.getAnalog(i, k);
}
storeSensorValue(sensorID, values / ioSample.getSampleSize());
}
}
}
else
{
// not something we were expecting
//flashLed(errorLed, 1, 25);
GMUtils::Log("xbee getResponse getApiId returned %d\n", apiID);
}
}
else if (xbee.getResponse().isError())
{
GMUtils::Log("Error reading packet. Error code %d\n ", xbee.getResponse().getErrorCode());
//nss.println(xbee.getResponse().getErrorCode());
// or flash error led
}
}
if (!serialPort.empty())
{
// serial port directly connected to sensor source
gmBuffSerial->FillBuffer();
if (gmBuffSerial->ReadAvail() > 0)
{
if (debugMode)
{
GMUtils::Write("serial port buffer contains %d bytes", gmBuffSerial->ReadAvail());
}
uint16_t bytesRead = gmBuffSerial->ReadUpToChar(buffer, 1000, 0x0a);
if (bytesRead > 0)
{
if (debugMode)
{
GMUtils::Write("Serial port returned %d: %s", byteRead, buffer);
}
processSensorData(GMUtils::UnsignedBytesToString(buffer));
}
}
// bytesAvail = gmBuffSerial->ReadAvail();
//
// if (bytesAvail > 0)
// {
// if (bytesAvail >= sizeof(buffer))
// {
// bytesAvail = sizeof(buffer) - 1; // allow for ending null
// }
// bufferIndex = 0;
// for (int i = 0; i < bytesAvail; i++)
// {
// buffer[bufferIndex++] = gmBuffSerial->Read();
// //printf("Read %d\n", byteRead);
// }
// buffer[bufferIndex] = 0;
// if (debugMode)
// {
// printf("Read %d: %s\n", bytesAvail, buffer);
// }
// //processSensorData(string(reinterpret_cast< char const* >(buffer)));
// processSensorData(GMUtils::UnsignedBytesToString(buffer));
// }
}
if (nextUploadCheck <= DateTime::seconds())
{
checkForNextUpload();
nextUploadCheck = DateTime::seconds() + 60;
}
if (weatherEnabled && nextWeatherUpdateTime <= DateTime::seconds())
{
if (getWeather() == 0)
{
sendWeather(temperatureC);
}
// NOTE - to avoid exceeding WeatherUnderground rate limit, don't immediately retry if call fails
nextWeatherUpdateTime = DateTime::seconds() + WEATHER_MEASUREMENT_INTERVAL;
}
if (sendTimeEnabled && nextSendTime <= DateTime::seconds())
{
sendTime();
nextSendTime = DateTime::seconds() + 60;
}
usleep(1000000);
}
//gmSerial->serialClose();
return 0;
}
void XBeeActive::search_XBee()
{
d0Value[0] = 0xFE;
commandValueLength = 0x1;
atRequest.setCommand(NTCmd);
sendAtCommand();
delay(250);
arRequestMod.clearCommandValue();
delay(100);
atRequest.setCommand(NDCmd);
int search_list = 0;
int i10;
Serial.println("\nSending command search XBee");
xbee.send(atRequest);
while (true)
{
if (xbee.readPacket(5000)) // подождите до 5 секунд для ответа состояния
{
if (xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE) // Должна быть команда отклика AT
{
xbee.getResponse().getAtCommandResponse(atResponse);
if (atResponse.isOk())
{
Serial.print("Command [");
Serial.print(atResponse.getCommand()[0]);
Serial.print(atResponse.getCommand()[1]);
Serial.println("] was successful!");
if (atResponse.getValueLength() > 0)
{
Len_XBee = atResponse.getValueLength();
Serial.print("Command value length is - ");
Serial.println(atResponse.getValueLength(), DEC);
Serial.print("Command value: ");
for (i10 = 0; i10 < atResponse.getValueLength(); i10++)
{
info_XBee_data[i10] = atResponse.getValue()[i10];
Serial.print(info_XBee_data[i10], HEX);
Serial.print(" ");
}
Serial.println("");
search_list++;
}
}
else
{
Serial.print("Command return error code: ");
Serial.println(atResponse.getStatus(), HEX);
}
}
else
{
Serial.print("Expected AT response but got ");
Serial.println(xbee.getResponse().getApiId(), HEX);
}
}
else
{
if (xbee.getResponse().isError()) // at command failed
{
Serial.print("Error reading packet. Error code: ");
Serial.println(xbee.getResponse().getErrorCode());
}
else
{
Serial.println("\nNo response from radio1");
return;
}
}
}
}
// Task to receive data from RPi
void xbeeTask(void* p) {
Serial.println("IN XBEE TASK");
while (1) {
int queueLen = 0;
int delPos = 0;
while (Serial1.available() > 0) {
unsigned char in = (unsigned char)Serial1.read();
Serial.println(in, HEX);
if (!RxQ.Enqueue(in)) {
break;
}
}
queueLen = RxQ.Size();
for (int i=0;i<queueLen;i++) {
if (RxQ.Peek(i) == 0x7E) {
unsigned char checkBuff[Q_SIZE];
unsigned char msgBuff[Q_SIZE];
int checkLen = 0;
int msgLen = 0;
checkLen = RxQ.Copy(checkBuff, i);
msgLen = xbee.Receive(checkBuff, checkLen, msgBuff);
if (msgLen > 0) {
unsigned char outMsg[MAX_RPI_MSG_SIZE];
unsigned char outFrame[Q_SIZE];
int frameLen = 0;
int packageID = (char)msgBuff[PKG_INDEX] - '0';
int ack_len = 0;
switch(packageID) {
case DEVICE_READY:
memcpy(outMsg, "ACK", 3);
deviceRdy = ack;
ack_len = 3;
break;
case NAVI_READY:
memcpy(outMsg, "ACK", 3);
naviRdy = true;
ack_len = 3;
break;
case NAVI_END:
memcpy(outMsg, "ACK", 3);
naviRdy = false;
deviceRdy = not_ack;
ack_len = 3;
break;
case OBSTACLE_DETECTED:
memcpy(outMsg, "ACK", 3);
// activate servos
if(msgBuff[9] == 'L')
Serial.println("left");
else
Serial.println("right");
// str = msgBuff[10];
//writeToActuator(left,right);
ack_len = 3;
break;
default:
// raise error ?
break;
}
frameLen = xbee.Send(outMsg, ack_len, outFrame, RPI_ADDR);
Serial1.write(outFrame, frameLen);
i += msgLen;
delPos = i;
}
else {
if (i > 0) {
delPos = i-1;
}
}
}
}
RxQ.Clear(delPos);
vTaskDelay(200);
}
}
XBeeStrategy::XBeeStrategy(Sputnik _sputnik) : ControllerStrategy( _sputnik){
xbee.begin(9600);
Serial.println( "XbeeStrategy Initialized");
}
void XBeeActive::sendAtCommand_ar()
{
int i10;
Serial.println("\nSending command sendAtCommand_ar to the XBee");
xbee.send(arRequestMod); // send the command
// wait up to 5 seconds for the status response
if (xbee.readPacket(5000))
{
// should be an AT command response
if (xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE)
{
xbee.getResponse().getAtCommandResponse(atResponse);
if (atResponse.isOk())
{
//myGLCD.setColor(255 , 0, 0);
// Serial.print("Command [");
// Serial.print(atResponse.getCommand()[0]);
// Serial.print(atResponse.getCommand()[1]);
// Serial.println("] was successful!");
if (atResponse.getValueLength() > 0)
{
Len_XBee = atResponse.getValueLength();
// Serial.print("Command value length is ");
// Serial.println(atResponse.getValueLength(), DEC);
// Serial.print("Command value: ");
int i11 = Len_XBee - 1;
info_XBee_data1[0] = 0;
info_XBee_data1[1] = 0;
info_XBee_data1[2] = 0;
info_XBee_data1[3] = 0;
for (i10 = 0; i10 < atResponse.getValueLength(); i10++)
{
info_XBee_data[i10] = atResponse.getValue()[i10];
Serial.print(info_XBee_data[i10], HEX);
info_XBee_data1[i11] = info_XBee_data[i10];
i11--;
}
// Serial.println("");
}
}
else
{
Serial.print("Command return error code: ");
Serial.println(atResponse.getStatus(), HEX);
//myGLCD.setColor(255, 0, 0);
//myGLCD.fillRoundRect(278, 92, 318, 132);
//myGLCD.setColor(255, 255, 255);
//myGLCD.drawRoundRect(278, 92, 318, 132);
//myGLCD.setBackColor(0, 0, 0);
//delay(200);
////XBee_alarm();
//delay(1000);
//myGLCD.setColor(0, 0, 0);
//myGLCD.fillRoundRect(278, 92, 318, 132);
//myGLCD.setColor(0, 0, 0);
//myGLCD.drawRoundRect(278, 92, 318, 132);
// mcp_Out1.digitalWrite(Beep, LOW); //
delay(300);
}
}
else
{
Serial.print("Expected AT response but got ");
Serial.println(xbee.getResponse().getApiId(), HEX);
}
}
else
{
// at command failed
if (xbee.getResponse().isError())
{
Serial.print("Error reading packet. Error code: ");
Serial.println(xbee.getResponse().getErrorCode());
}
else
{
Serial.println("No response from radio2");
}
}
}
namespace FireNest {
struct Channel {
int button_pin;
int led_pin;
int state;
int time;
};
Channel channels[] = {
{2,8,0,0},
{3,9,0,0},
{4,A3,0,0},
{5,A0,0,0},
{6,A1,0,0},
{7,A2,0,0}
};
//prototypes
void discover();
boolean checkNDResponse();
void flashAll(int ms);
void clearLeds();
void nr(uint8_t nr);
uint32_t pack(uint32_t c1, uint32_t c2, uint32_t c3, uint32_t c4);
//NewSoftSerial mySerial(12, 13);
//used when using first button to fire all channels
uint8_t channel_count = 0;
uint8_t fired_channels = 0;
XBee xbee = XBee();
uint8_t payload[] = { 0 };
// SH + SL Address of receiving XBee
uint32_t sh = 0;
uint32_t sl = 0;
//XBeeAddress64 addr64; //XBeeAddress64(0x0013a200, 0x403141DA);
XBeeAddress64 addr64; //XBeeAddress64(0x0013a200, 0x403141DA);
ZBTxRequest zbTx;
ZBTxStatusResponse txStatus = ZBTxStatusResponse();
//for device discovery
uint8_t atCmd[] = {'N','D'};
AtCommandRequest atRequest = AtCommandRequest(atCmd);
AtCommandResponse atResponse = AtCommandResponse();
void setupold() {
//mySerial.begin(4800);
//mySerial.println("Hello world");
for (int i= 0; i<CHANNELS; i++) {
pinMode(channels[i].button_pin,INPUT);
digitalWrite(channels[i].button_pin,HIGH); //enable internal 20K pullup
pinMode(channels[i].led_pin,OUTPUT);
//blink leds a bit
digitalWrite(channels[i].led_pin,HIGH);
delay(200);
digitalWrite(channels[i].led_pin,LOW);
}
//debug led
//pinMode(13,OUTPUT);
//digitalWrite(13,HIGH);
//delay(500);
//digitalWrite(13,LOW);
xbee.begin(XBEE_BAUD);
//discover the other XBEE's address
discover();
zbTx = ZBTxRequest(addr64, payload, sizeof(payload));
//send a no-op packet so that the xbees can do their magic and find each other
payload[0] = 254;
xbee.send(zbTx);
//Flash all leds once so the user knows
flashAll(500);
//mySerial.println("Discovered address");
//mySerial.print("MSB: ");
//mySerial.println(addr64.getMsb());
//mySerial.println(addr64.getMsb()==0x0013a200?"Yes!":"NO");
//mySerial.print("LSB: ");
//mySerial.println(addr64.getLsb());
//mySerial.println(addr64.getLsb()==0x403141DA?"Yes!":"NO");
}
//State 0 == not pressed, waiting for press
//State 1 == pressed, debouncing time not up
//Fire on press
//State 2 == pressed, waiting for release
//State 3 == release, debouncing time not up
void loopold() {
int val;
int m;
for (uint8_t i= 0; i<CHANNELS; i++) {
m = millis();
if (channels[i].state == 0 || channels[i].state == 2) {
val = digitalRead(channels[i].button_pin);
if (channels[i].state == 0 && val == LOW) {
//a press!, fire!
uint8_t cc = i;
//special case, we can fire all channels by firing the first button repeatably
if (i == 0) {
cc = channel_count;
channel_count = (channel_count + 1) % CHANNELS;
}
//fire!
payload[0] = cc;
xbee.send(zbTx);
//set as fired
fired_channels |= (1 << cc);
digitalWrite(channels[cc].led_pin,HIGH);
//check if all is fired
if (fired_channels == B00111111) {
//wait a bit
delay(500);
//reset all
channel_count = 0;
fired_channels = 0;
for (int j = 0; j<CHANNELS; j++) {
channels[j].state = 0;
digitalWrite(channels[j].led_pin,LOW);
delay(300);
}
break;
}
}
if ((channels[i].state == 0 && val == LOW) || (channels[i].state == 2 && val == HIGH)) {
channels[i].state = (channels[i].state + 1) % 4; //change state
channels[i].time = m;
}
} else if (m - channels[i].time > THRESHHOLD) {
channels[i].state = (channels[i].state + 1) % 4; //change state
}
}
}
//discover target node
void discover() {
//mySerial.println("discover");
//if we don't get a address we can't fire
while (true) {
//send node discovery
xbee.send(atRequest);
//default value is that responding XBEE can wait up to six seconds before answering
//so spamming it with node discoverys might be a bad thing, but waiting so long is booring so
//we we'll try it and see if it works...
//knight rider on the diodes let's the users know we're looking
for (int i=0; i<CHANNELS; i++) {
clearLeds();
digitalWrite(channels[i % CHANNELS].led_pin,HIGH);
if (checkNDResponse()) {
return;
}
}
for (int i=CHANNELS-1; i>=0; i--) {
clearLeds();
digitalWrite(channels[i % CHANNELS].led_pin,HIGH);
if (checkNDResponse()) {
return;
}
}
}
}
boolean checkNDResponse() {
//mySerial.println("checkNDResponse");
// wait a small bit so the animation looks good
if (xbee.readPacket(ND_WAIT_TIME / 6)) {
// got a response!
// should be an AT command response
if (xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE) {
xbee.getResponse().getAtCommandResponse(atResponse);
if (atResponse.isOk()) {
if (atResponse.getCommand()[0] == atCmd[0] && atResponse.getCommand()[1] == atCmd[1] && atResponse.getValueLength() > 3) {
//mySerial.println(pack(atResponse.getValue()[2],atResponse.getValue()[3],atResponse.getValue()[4],atResponse.getValue()[5]));
//mySerial.println(pack(atResponse.getValue()[6],atResponse.getValue()[7],atResponse.getValue()[8],atResponse.getValue()[9]));
addr64 = XBeeAddress64( pack(atResponse.getValue()[2],atResponse.getValue()[3],atResponse.getValue()[4],atResponse.getValue()[5]),pack(atResponse.getValue()[6],atResponse.getValue()[7],atResponse.getValue()[8],atResponse.getValue()[9]) );
return true;
}
}
else {
//nss.print("Command return error code: ");
//nss.println(atResponse.getStatus(), HEX);
nr(1);
}
} else {
//nss.print("Expected AT response but got ");
//nss.print(xbee.getResponse().getApiId(), HEX);
nr(2);
}
} else {
// at command failed
if (xbee.getResponse().isError()) {
//nss.print("Error reading packet. Error code: ");
//nss.println(xbee.getResponse().getErrorCode());
nr(3);
}
else {
//nss.print("No response from radio");
nr(4);
}
}
return false;
}
//flash leds once, variable time
void flashAll(int ms) {
for (int i=0;i<CHANNELS; i++) {
digitalWrite(channels[i].led_pin,HIGH);
}
delay(ms);
clearLeds();
}
//clear all leds
void clearLeds() {
for (int i=0;i<CHANNELS; i++) {
digitalWrite(channels[i].led_pin,LOW);
}
}
//light up a nr, binary code
void nr(uint8_t nr) {
//TODO: smarter code...
if (nr & B00000001) {
digitalWrite(8,HIGH);
}
if (nr & B00000010) {
digitalWrite(9,HIGH);
}
if (nr & B00000100) {
digitalWrite(A3,HIGH);
}
if (nr & B00001000) {
digitalWrite(A0,HIGH);
}
if (nr & B00010000) {
digitalWrite(A1,HIGH);
}
if (nr & B00100000) {
digitalWrite(A2,HIGH);
}
}
uint32_t pack(uint32_t c1, uint32_t c2, uint32_t c3, uint32_t c4) {
return (c1 << 24) | (c2 << 16) | (c3 << 8) | (c4);
}
}
void XBeeActive::setup()
{
xbee.begin(Serial1);
DEBAG.println("\nXBee setup");
}