bool SmartyBase::setupRadio(uint8_t hwType, uint8_t smartyID) {
Serial.begin(115200);
//Serial.println("serial ok");
if (!configRadio(hwType)) {
sendDebug("Error initializing nrf24l01");
return false;
}
else
sendDebug("Success initializing nrf24l01");
m_radio.openWritingPipe(m_destAddress); // address of combrick
if (smartyID != 0) {
m_config.m_smartyID = smartyID;
m_localAddress[4] = m_config.m_smartyID;
}
sendDebug("I am smarty # %d", m_config.m_smartyID);
m_radio.openReadingPipe(MAIN_RX_PIPE, m_localAddress); // smarty address
//m_radio.openReadingPipe(BROADCAST_RX_PIPE, m_broadcastAddress); // broadcast address
/*if (m_isComBrick) {
m_radio.openWritingPipe(addresses[1]);
m_radio.openReadingPipe(1,addresses[0]);
} else {
m_radio.openWritingPipe(addresses[0]);
m_radio.openReadingPipe(1,addresses[1]);
}*/
m_radio.startListening();
return true;
}
void Smarty::setupSmarty() {
Serial.begin(115200);
if (!setupRadio())
Serial.println("Error initializing nrf24l01");
else
sendDebug("Success initializing nrf24l01");
sendDebug("I am smarty # %d", m_config.m_smartyID);
m_radio.openWritingPipe(m_destAddress); // address of combrick
m_radio.openReadingPipe(SMARTY_RX_PIPE, m_localAddress); // smarty address
//m_radio.openReadingPipe(BROADCAST_RX_PIPE, m_broadcastAddress); // broadcast address
m_radio.startListening();
}
void ofxModbusTcpClient::sendNextCommand(){
if (commandToSend.size()>0){
waitingForReply = true;
//Set last function code
lastFunctionCode = commandToSend[0].msg[7];
//set transaction id
WORD tx = getTransactionID();
commandToSend[0].msg[0] = HIGHBYTE(tx); //Transaction High
commandToSend[0].msg[1] = LOWBYTE(tx); //Transaction Low
//load vector to local uint8_t
uint8_t localByteArray[commandToSend[0].msg.size()];
for (int i=0; i<commandToSend[0].msg.size(); i++){
localByteArray[i] = commandToSend[0].msg[i];
}
//send command
unsigned char * t = localByteArray;
tcpClient.sendRawBytes((const char*)t, 6+commandToSend[0].length);
//Log it
sendDebug("Sent: "+commandToSend[0].debugString);
//erase last command
commandToSend.erase(commandToSend.begin());
}
}
void ComBrick::checkPcMessage() {
if (Serial.available() > 0) { // received a message from serial port (from PC)
Serial.readBytes((char*)(&m_txMessage), CONSTANT_PAYLOAD_SIZE);
sendDebug("Msg for smarty: %d", m_txMessage.m_smartyID);
sendMessageToID(m_txMessage.m_smartyID);
/*
// First, stop listening so we can talk.
m_radio.stopListening();
// TODO: check the network map if we need more hops
m_destAddress[4] = m_txMessage.m_smartyID;
m_radio.openWritingPipe(m_destAddress);
// send the whole message to smarty
if (!m_radio.write(&m_txMessage, CONSTANT_PAYLOAD_SIZE)){
sendDebug("failed");
}
// Now, continue listening
m_radio.startListening();
*/
}
}
/* This method handles lowest-level transmission of a message to a destination
The message is assumed to be complete and contained in the m_txMessage buffer
The method checks if it is necessary to update the m_destAddress variable */
bool SmartyBase::sendMessageToAddr(uint8_t addr) {
// First, stop listening so we can talk
m_radio.stopListening();
// Check if we have to update the destination address
//if (m_destAddress[4] != addr) {
m_destAddress[4] = addr;
m_radio.openWritingPipe(m_destAddress);
//}
delay(1);
// Send the message
//sendDebug("2.sending message to %d", addr);
int res = m_radio.write(&m_txMessage, CONSTANT_PAYLOAD_SIZE);
if (res < 0) sendDebug("failed to send message to %d: res = %d", addr, res);
else sendDebug("success send message to %d", addr);
// Now, resume listening so we catch the next packets.
m_radio.startListening();
return res;
}
void Smarty::setHardPin(uint8_t pinId, uint8_t pinHwId, PIN_TYPE type) {
// check if id is within the bounds
if (pinId >= NUM_HW_PINS) return;
// check if the pin is free
if (pinHwId == 10 || pinHwId == 11 || pinHwId == 12 || pinHwId == CE_PIN || pinHwId == CS_PIN) return;
m_pins[pinId].m_pinHwId = pinHwId;
m_pins[pinId].m_type = type;
pinMode(pinHwId, (type == ANALOG_OUT || type == DIGITAL_OUT) ? OUTPUT : INPUT);
sendDebug("Hard pin set");
}
// service a client request; test the opcode and then do appropriate servicing
void CPicoServSession::DispatchMessageL(const RMessage &aMessage)
{
switch (aMessage.Function()) {
case PicoMsgLoadState:
if(!rom_data) User::Leave(-1); // no ROM
User::LeaveIfError(saveLoadGame(1));
gamestate = PGS_Running;
return;
case PicoMsgSaveState:
if(!rom_data) User::Leave(-1);
User::LeaveIfError(saveLoadGame(0));
gamestate = PGS_Running;
return;
case PicoMsgLoadROM:
loadROM();
return;
case PicoMsgResume:
if(rom_data) gamestate = PGS_Running;
return;
case PicoMsgReset:
if(rom_data) {
PicoReset(0);
pico_was_reset = 1;
gamestate = PGS_Running;
}
return;
case PicoMsgKeys:
gamestate = PGS_KeyConfig;
return;
case PicoMsgPause:
gamestate = PGS_Paused;
return;
case PicoMsgQuit:
DEBUGPRINT(_L("got quit msg."));
gamestate = PGS_Quit;
return;
// config change
case PicoMsgConfigChange: // launcher -> emu
changeConfig();
return;
case PicoMsgRetrieveConfig: // emu -> launcher
sendConfig();
return;
case PicoMsgRetrieveDebugStr: // emu -> launcher
sendDebug();
return;
// requests we don't understand at all are a different thing,
// so panic the client here, this function also completes the message
default:
PanicClient(EBadRequest);
return;
}
}
//Main Update
void ofxModbusTcpClient::update(ofEventArgs & args) {
connected = weConnected;
int transactionID = 0;
int lengthPacket = 0;
if (enabled) {
if (weConnected) {
if (!waitingForReply){
sendNextCommand();
}
uint8_t headerReply[2000];
int totalBytes = 0;
if (tcpClient.receiveRawBytes((char *)headerReply, 2000) > 0) { //Grab Header
int transID = convertToWord(headerReply[0], headerReply[1]);
int protocolID = convertToWord(headerReply[3], headerReply[4]);
//Ignore if not last transaction ID
if (transID != lastTransactionID) {
ofLogError("ofxModbusTCP IP:"+ip)<<"Transaction ID Mismatch, discarding Reply. Got: "<<ofToHex(transID)<<" expected:"<<ofToHex(lastTransactionID);
return;
}
//Ignore if not correct protocol
if (headerReply[3] != 0x00 || headerReply[4] != 0x00) {
ofLogError("ofxModbusTCP IP:"+ip)<<"Invalid Protocol ID, discarding reply. Got: "<<ofToHex(headerReply[3])<<" "<<ofToHex(headerReply[4]);
return;
}
int functionCode = headerReply[7];
//Ignore if not correct last function code
if (functionCode != lastFunctionCode) {
ofLogError("ofxModbusTCP IP:"+ip)<<"Function Code Mismatch, discarding reply.";
}
int originatingID = headerReply[6];
int lengthData = convertToWord(headerReply[4], headerReply[5]);
switch (functionCode) {
case 1: {
sendDebug("Reply Received, Setting Multiple Coils - not supported yet");
} break;
case 3: {
sendDebug("Reply Received, Setting Multiple Registers");
int byteCount = headerReply[8];
int currentByte = 9;
for (int i=0; i<(byteCount/2); i++) {
int newVal = convertToWord(headerReply[currentByte], headerReply[currentByte+1]);
slaves.at(originatingID-1)->setRegister(i+1, newVal);
currentByte = currentByte + 2;
}
} break;
case 5: {
sendDebug("Reply Received, Setting A Single Coul");
int address = convertToWord(headerReply[8], headerReply[9])+1;
bool t;
if (headerReply[10] == 0xff) {t=true;} else {t=false;}
slaves.at(originatingID-1)->setCoil(address+1, t);
} break;
case 6: {
sendDebug("Reply Received, Setting A Single Register");
int address = convertToWord(headerReply[8], headerReply[9]);
slaves.at(originatingID-1)->setRegister(address+1, convertToWord(headerReply[10], headerReply[11]));
} break;
case 15: {
sendDebug("Reply Received, Setting Multiple Coils - not supported yet");
} break;
case 16: {
sendDebug("Reply Received, Setting Multiple Registers - not supported yet");
} break;
}
waitingForReply = false;
}
if(!active) {
active = true;
PreviousActivityTime = ofGetElapsedTimeMillis();
}
if(ofGetElapsedTimeMillis() > (PreviousActivityTime + 60000)) {
if(active) {
active = false;
}
}
} else if (!tcpClient.isConnected()){
//if we are not connected lets try and reconnect every 5 seconds
deltaTime = ofGetElapsedTimeMillis() - connectTime;
if( deltaTime > 5000 ){
weConnected = tcpClient.setup(ip, port);
connectTime = ofGetElapsedTimeMillis();
}
}
} else {
if (tcpClient.isConnected()) {
tcpClient.close();
}
}
}
void Smarty::poll() {
if (m_buttonPin < 255)
if (digitalRead(m_buttonPin) == LOW && m_prevButtonState == HIGH) {
m_prevButtonState = LOW;
// fire a message to the combrick
}
else if (digitalRead(m_buttonPin) == HIGH && m_prevButtonState == LOW) {
m_prevButtonState = HIGH;
}
// if ( m_radio.available(&pipe_num) ) {
// m_radio.read(&m_rxMessage, CONSTANT_PAYLOAD_SIZE);
if ((pipe_num = checkRx()) >= 0) {
sendDebug("Received message from pipe %d", pipe_num);
sendDebug("Opcode is %d", m_rxMessage.m_opcode);
if (pipe_num == SMARTY_RX_PIPE && m_rxMessage.m_smartyID == m_config.m_smartyID) {
//Serial.println("This command is for me");
switch (m_rxMessage.m_opcode) {
case OPCODE_GENERIC_CMD:
if (mp_genericCmdCb != NULL)
mp_genericCmdCb(m_rxMessage.m_params[0], m_rxMessage.m_params + 1);
break;
case OPCODE_SET_ANALOG_CMD:
if (mp_setAnalogCmdCb != NULL) {
memcpy(pinCmds, m_rxMessage.m_params, 6);
mp_setAnalogCmdCb(pinCmds);
}
else {
sendDebug("set analog command for me");
memcpy(pinCmds, m_rxMessage.m_params, 6);
for (int i = 0; i < NUM_HW_PINS; i++) {
if (pinCmds[i].m_pinId < NUM_HW_PINS && m_pins[pinCmds[i].m_pinId].m_type == ANALOG_OUT)
{
analogWrite(m_pins[pinCmds[i].m_pinId].m_pinHwId, pinCmds[i].m_value);
sendDebug("Found pin");
}
}
}
break;
case OPCODE_SET_DIGITAL_CMD:
if (mp_setDigitalCmdCb != NULL) {
memcpy(pinCmds, m_rxMessage.m_params, 6);
mp_setDigitalCmdCb(pinCmds);
}
else {
memcpy(pinCmds, m_rxMessage.m_params, 6);
for (int i = 0; i < NUM_HW_PINS; i++) {
if (pinCmds[i].m_pinId < NUM_HW_PINS && m_pins[pinCmds[i].m_pinId].m_type == ANALOG_OUT)
digitalWrite(m_pins[pinCmds[i].m_pinId].m_pinHwId, pinCmds[i].m_value == 0 ? LOW : HIGH);
}
}
break;
case OPCODE_DC_MOTOR_CMD:
if (mp_dcMotorCmdCb != NULL)
mp_dcMotorCmdCb(m_rxMessage.m_params[1], m_rxMessage.m_params[2], m_rxMessage.m_params[3], m_rxMessage.m_params[4]);
break;
default:
if (mp_defaultCmdCb != NULL)
mp_defaultCmdCb(&m_rxMessage.m_opcode);
break;
}
}
else if (pipe_num == SMARTY_RX_PIPE) {
// the message is intended for another smarty - transfer it (relay mode)
// this should depend on a network map saved in the eeprom
// design should allow unlimited hops
transferMessage();
}
else if (pipe_num == BROADCAST_RX_PIPE) {
}
}
}
