Yuki Suga / RTnoV4

RTno is communicating library and framework which allows you to make your embedded device capable of communicating with RT-middleware world. RT-middleware is a platform software to realize Robotic system. In RTM, robots are developed by constructing robotics technologies\' elements (components) named RT-component. Therefore, the RTno helps you to create your own RT-component with your mbed and arduino. To know how to use your RTno device, visit here: http://ysuga.net/robot_e/rtm_e/rtc_e/1065?lang=en To know about RT-middleware and RT-component, visit http://www.openrtm.org

Dependencies:   EthernetInterface mbed-rtos

RTno.cpp@0:5f7bc45bc2e8, 2013-06-24 (annotated)

Committer:
ysuga
Date:
Mon Jun 24 06:42:11 2013 +0000
Revision:
0:5f7bc45bc2e8
Child:
1:f74116b37bc9
[mbed] converted /RTnoV3/RTnoV3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ysuga 0:5f7bc45bc2e8 1 /*******************************************
ysuga 0:5f7bc45bc2e8 2 * RTno.cpp
ysuga 0:5f7bc45bc2e8 3 * @author Yuki Suga
ysuga 0:5f7bc45bc2e8 4 * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
ysuga 0:5f7bc45bc2e8 5 * @license LGPLv3
ysuga 0:5f7bc45bc2e8 6 *****************************************/
ysuga 0:5f7bc45bc2e8 7 #define RTNO_SUBMODULE_DEFINE
ysuga 0:5f7bc45bc2e8 8 #include <stdint.h>
ysuga 0:5f7bc45bc2e8 9 #include "mbed.h"
ysuga 0:5f7bc45bc2e8 10
ysuga 0:5f7bc45bc2e8 11 #include "RTno.h"
ysuga 0:5f7bc45bc2e8 12 #include "Packet.h"
ysuga 0:5f7bc45bc2e8 13
ysuga 0:5f7bc45bc2e8 14 #include "Transport.h"
ysuga 0:5f7bc45bc2e8 15 //#include "UART.h"
ysuga 0:5f7bc45bc2e8 16 #include "RTnoProfile.h"
ysuga 0:5f7bc45bc2e8 17
ysuga 0:5f7bc45bc2e8 18 using namespace RTno;
ysuga 0:5f7bc45bc2e8 19
ysuga 0:5f7bc45bc2e8 20 // global variables
ysuga 0:5f7bc45bc2e8 21 // module private variables.
ysuga 0:5f7bc45bc2e8 22 #define PRIVATE static
ysuga 0:5f7bc45bc2e8 23
ysuga 0:5f7bc45bc2e8 24 // static value declaration.
ysuga 0:5f7bc45bc2e8 25 void(*SerialDevice_putc)(const char c);
ysuga 0:5f7bc45bc2e8 26 uint8_t(*SerialDevice_available)();
ysuga 0:5f7bc45bc2e8 27 char(*SerialDevice_getc)();
ysuga 0:5f7bc45bc2e8 28
ysuga 0:5f7bc45bc2e8 29 PRIVATE int8_t m_pPacketBuffer[PACKET_BUFFER_SIZE];
ysuga 0:5f7bc45bc2e8 30
ysuga 0:5f7bc45bc2e8 31 /*
ysuga 0:5f7bc45bc2e8 32 * Send Profile Data
ysuga 0:5f7bc45bc2e8 33 */
ysuga 0:5f7bc45bc2e8 34 PRIVATE void _SendProfile();
ysuga 0:5f7bc45bc2e8 35
ysuga 0:5f7bc45bc2e8 36 /**
ysuga 0:5f7bc45bc2e8 37 * Packet Handler in Error State.
ysuga 0:5f7bc45bc2e8 38 */
ysuga 0:5f7bc45bc2e8 39 PRIVATE void _PacketHandlerOnError();
ysuga 0:5f7bc45bc2e8 40
ysuga 0:5f7bc45bc2e8 41 /**
ysuga 0:5f7bc45bc2e8 42 * Packet Handler in Inactive State.
ysuga 0:5f7bc45bc2e8 43 */
ysuga 0:5f7bc45bc2e8 44 PRIVATE void _PacketHandlerOnInactive();
ysuga 0:5f7bc45bc2e8 45
ysuga 0:5f7bc45bc2e8 46 /**
ysuga 0:5f7bc45bc2e8 47 * Packet Handler in Active State.
ysuga 0:5f7bc45bc2e8 48 */
ysuga 0:5f7bc45bc2e8 49 PRIVATE void _PacketHandlerOnActive();
ysuga 0:5f7bc45bc2e8 50
ysuga 0:5f7bc45bc2e8 51 void EC_setup(exec_cxt_str& exec_cxt);
ysuga 0:5f7bc45bc2e8 52 void Connection_setup(config_str& conf);
ysuga 0:5f7bc45bc2e8 53
ysuga 0:5f7bc45bc2e8 54 /**
ysuga 0:5f7bc45bc2e8 55 * Arduino Setup Routine.
ysuga 0:5f7bc45bc2e8 56 * This function is called when arduino device is turned on.
ysuga 0:5f7bc45bc2e8 57 */
ysuga 0:5f7bc45bc2e8 58 void setup() {
ysuga 0:5f7bc45bc2e8 59 RTnoProfile_init();
ysuga 0:5f7bc45bc2e8 60 // This function must be called first.
ysuga 0:5f7bc45bc2e8 61 exec_cxt_str* exec_cxt = (exec_cxt_str*)malloc(sizeof(exec_cxt_str));
ysuga 0:5f7bc45bc2e8 62 config_str* conf = (config_str*)malloc(sizeof(config_str));
ysuga 0:5f7bc45bc2e8 63 rtcconf(*conf, *exec_cxt);
ysuga 0:5f7bc45bc2e8 64 if(onInitialize() == RTC_OK) {
ysuga 0:5f7bc45bc2e8 65 EC_setup(*exec_cxt);
ysuga 0:5f7bc45bc2e8 66 Connection_setup(*conf);
ysuga 0:5f7bc45bc2e8 67 free(exec_cxt);
ysuga 0:5f7bc45bc2e8 68 free(conf);
ysuga 0:5f7bc45bc2e8 69 Transport_init();
ysuga 0:5f7bc45bc2e8 70 EC_start();
ysuga 0:5f7bc45bc2e8 71 }
ysuga 0:5f7bc45bc2e8 72 }
ysuga 0:5f7bc45bc2e8 73
ysuga 0:5f7bc45bc2e8 74
ysuga 0:5f7bc45bc2e8 75 /**
ysuga 0:5f7bc45bc2e8 76 * Arduino Loop routine.
ysuga 0:5f7bc45bc2e8 77 * This function is repeadedly called when arduino is turned on.
ysuga 0:5f7bc45bc2e8 78 */
ysuga 0:5f7bc45bc2e8 79 void loop() {
ysuga 0:5f7bc45bc2e8 80 int8_t ret;
ysuga 0:5f7bc45bc2e8 81 ret = Transport_ReceivePacket(m_pPacketBuffer);
ysuga 0:5f7bc45bc2e8 82 if(ret < 0) { // Timeout Error or Checksum Error
ysuga 0:5f7bc45bc2e8 83 Transport_SendPacket(PACKET_ERROR, 1, &ret);
ysuga 0:5f7bc45bc2e8 84 } else if (ret == 0) {
ysuga 0:5f7bc45bc2e8 85 } else if (ret > 0) { // Packet is successfully received
ysuga 0:5f7bc45bc2e8 86 if (m_pPacketBuffer[INTERFACE] == GET_PROFILE) {
ysuga 0:5f7bc45bc2e8 87 _SendProfile();
ysuga 0:5f7bc45bc2e8 88 } else if(m_pPacketBuffer[INTERFACE] == GET_STATUS) {
ysuga 0:5f7bc45bc2e8 89 int8_t state = EC_get_component_state();
ysuga 0:5f7bc45bc2e8 90 Transport_SendPacket(GET_STATUS, 1, &state);
ysuga 0:5f7bc45bc2e8 91 } else if(m_pPacketBuffer[INTERFACE] == GET_CONTEXT) {
ysuga 0:5f7bc45bc2e8 92 int8_t type = EC_get_type();
ysuga 0:5f7bc45bc2e8 93 Transport_SendPacket(GET_CONTEXT, 1, &type);
ysuga 0:5f7bc45bc2e8 94 } else {
ysuga 0:5f7bc45bc2e8 95 switch(EC_get_component_state()) {
ysuga 0:5f7bc45bc2e8 96 case RTC_STATE_ERROR:
ysuga 0:5f7bc45bc2e8 97 _PacketHandlerOnError();
ysuga 0:5f7bc45bc2e8 98 break;
ysuga 0:5f7bc45bc2e8 99 case RTC_STATE_INACTIVE:
ysuga 0:5f7bc45bc2e8 100 _PacketHandlerOnInactive();
ysuga 0:5f7bc45bc2e8 101 break;
ysuga 0:5f7bc45bc2e8 102 case RTC_STATE_ACTIVE:
ysuga 0:5f7bc45bc2e8 103 _PacketHandlerOnActive();
ysuga 0:5f7bc45bc2e8 104 break;
ysuga 0:5f7bc45bc2e8 105 case RTC_STATE_NONE:
ysuga 0:5f7bc45bc2e8 106 ret = RTNO_NONE;
ysuga 0:5f7bc45bc2e8 107 Transport_SendPacket(m_pPacketBuffer[INTERFACE], 1, &ret);
ysuga 0:5f7bc45bc2e8 108 break;
ysuga 0:5f7bc45bc2e8 109 default: // if m_Condition is unknown...
ysuga 0:5f7bc45bc2e8 110
ysuga 0:5f7bc45bc2e8 111 break;
ysuga 0:5f7bc45bc2e8 112 }
ysuga 0:5f7bc45bc2e8 113 }
ysuga 0:5f7bc45bc2e8 114 }
ysuga 0:5f7bc45bc2e8 115
ysuga 0:5f7bc45bc2e8 116
ysuga 0:5f7bc45bc2e8 117 int numOutPort = RTnoProfile_getNumOutPort();
ysuga 0:5f7bc45bc2e8 118 for(int i = 0;i < numOutPort;i++) {
ysuga 0:5f7bc45bc2e8 119 EC_suspend();
ysuga 0:5f7bc45bc2e8 120 PortBase* pOutPort = RTnoProfile_getOutPortByIndex(i);
ysuga 0:5f7bc45bc2e8 121 if(pOutPort->pPortBuffer->hasNext(pOutPort->pPortBuffer)) {
ysuga 0:5f7bc45bc2e8 122 char* name = pOutPort->pName;
ysuga 0:5f7bc45bc2e8 123 unsigned char nameLen = strlen(name);
ysuga 0:5f7bc45bc2e8 124 unsigned char dataLen = pOutPort->pPortBuffer->getNextDataSize(pOutPort->pPortBuffer);
ysuga 0:5f7bc45bc2e8 125
ysuga 0:5f7bc45bc2e8 126 m_pPacketBuffer[0] = nameLen;
ysuga 0:5f7bc45bc2e8 127 m_pPacketBuffer[1] = dataLen;
ysuga 0:5f7bc45bc2e8 128 memcpy(m_pPacketBuffer + 2, name, nameLen);
ysuga 0:5f7bc45bc2e8 129 pOutPort->pPortBuffer->pop(pOutPort->pPortBuffer, m_pPacketBuffer + 2 + nameLen, dataLen);
ysuga 0:5f7bc45bc2e8 130 Transport_SendPacket(RECEIVE_DATA, 2 + nameLen + dataLen, m_pPacketBuffer);
ysuga 0:5f7bc45bc2e8 131 }
ysuga 0:5f7bc45bc2e8 132 EC_resume();
ysuga 0:5f7bc45bc2e8 133 }
ysuga 0:5f7bc45bc2e8 134
ysuga 0:5f7bc45bc2e8 135 }
ysuga 0:5f7bc45bc2e8 136
ysuga 0:5f7bc45bc2e8 137 /**
ysuga 0:5f7bc45bc2e8 138 * add InPort data to Profile.
ysuga 0:5f7bc45bc2e8 139 */
ysuga 0:5f7bc45bc2e8 140 void addInPort(InPortBase& Port)
ysuga 0:5f7bc45bc2e8 141 {
ysuga 0:5f7bc45bc2e8 142 RTnoProfile_addInPort(&Port);
ysuga 0:5f7bc45bc2e8 143 }
ysuga 0:5f7bc45bc2e8 144
ysuga 0:5f7bc45bc2e8 145 /**
ysuga 0:5f7bc45bc2e8 146 * add OutPort data to Profile
ysuga 0:5f7bc45bc2e8 147 */
ysuga 0:5f7bc45bc2e8 148 void addOutPort(OutPortBase& Port)
ysuga 0:5f7bc45bc2e8 149 {
ysuga 0:5f7bc45bc2e8 150 RTnoProfile_addOutPort(&Port);
ysuga 0:5f7bc45bc2e8 151 }
ysuga 0:5f7bc45bc2e8 152
ysuga 0:5f7bc45bc2e8 153
ysuga 0:5f7bc45bc2e8 154 /**
ysuga 0:5f7bc45bc2e8 155 * Private Function Definitions
ysuga 0:5f7bc45bc2e8 156 *
ysuga 0:5f7bc45bc2e8 157 */
ysuga 0:5f7bc45bc2e8 158
ysuga 0:5f7bc45bc2e8 159
ysuga 0:5f7bc45bc2e8 160 /**
ysuga 0:5f7bc45bc2e8 161 * Send Profile Data
ysuga 0:5f7bc45bc2e8 162 */
ysuga 0:5f7bc45bc2e8 163 PRIVATE void _SendProfile() {
ysuga 0:5f7bc45bc2e8 164 int8_t ret = RTNO_OK;
ysuga 0:5f7bc45bc2e8 165 for(uint8_t i = 0;i < RTnoProfile_getNumInPort();i++) {
ysuga 0:5f7bc45bc2e8 166 PortBase* inPort = RTnoProfile_getInPortByIndex(i);
ysuga 0:5f7bc45bc2e8 167 uint8_t nameLen = strlen(inPort->pName);
ysuga 0:5f7bc45bc2e8 168 m_pPacketBuffer[0] = inPort->typeCode;
ysuga 0:5f7bc45bc2e8 169 memcpy(&(m_pPacketBuffer[1]), inPort->pName, nameLen);
ysuga 0:5f7bc45bc2e8 170 Transport_SendPacket(ADD_INPORT, 1+nameLen, m_pPacketBuffer);
ysuga 0:5f7bc45bc2e8 171 }
ysuga 0:5f7bc45bc2e8 172
ysuga 0:5f7bc45bc2e8 173 for(uint8_t i = 0;i < RTnoProfile_getNumOutPort();i++) {
ysuga 0:5f7bc45bc2e8 174 PortBase* outPort = RTnoProfile_getOutPortByIndex(i);
ysuga 0:5f7bc45bc2e8 175 uint8_t nameLen = strlen(outPort->pName);
ysuga 0:5f7bc45bc2e8 176 m_pPacketBuffer[0] = outPort->typeCode;
ysuga 0:5f7bc45bc2e8 177 memcpy(&(m_pPacketBuffer[1]), outPort->pName, nameLen);
ysuga 0:5f7bc45bc2e8 178 Transport_SendPacket(ADD_OUTPORT, 1+nameLen, m_pPacketBuffer);
ysuga 0:5f7bc45bc2e8 179 }
ysuga 0:5f7bc45bc2e8 180
ysuga 0:5f7bc45bc2e8 181 Transport_SendPacket(GET_PROFILE, 1, &ret);
ysuga 0:5f7bc45bc2e8 182 }
ysuga 0:5f7bc45bc2e8 183
ysuga 0:5f7bc45bc2e8 184
ysuga 0:5f7bc45bc2e8 185
ysuga 0:5f7bc45bc2e8 186 /**
ysuga 0:5f7bc45bc2e8 187 * Packet Handler in Error State
ysuga 0:5f7bc45bc2e8 188 */
ysuga 0:5f7bc45bc2e8 189 PRIVATE void _PacketHandlerOnError() {
ysuga 0:5f7bc45bc2e8 190 char intface;
ysuga 0:5f7bc45bc2e8 191 int8_t ret = RTNO_OK;
ysuga 0:5f7bc45bc2e8 192
ysuga 0:5f7bc45bc2e8 193 int8_t retval = EC_error();
ysuga 0:5f7bc45bc2e8 194 if(retval < 0) ret = RTNO_ERROR;
ysuga 0:5f7bc45bc2e8 195 Transport_SendPacket(intface, 1, &ret);
ysuga 0:5f7bc45bc2e8 196 }
ysuga 0:5f7bc45bc2e8 197
ysuga 0:5f7bc45bc2e8 198
ysuga 0:5f7bc45bc2e8 199 /**
ysuga 0:5f7bc45bc2e8 200 * Packet Handler in Inactive State
ysuga 0:5f7bc45bc2e8 201 */
ysuga 0:5f7bc45bc2e8 202 PRIVATE void _PacketHandlerOnInactive() {
ysuga 0:5f7bc45bc2e8 203
ysuga 0:5f7bc45bc2e8 204 int8_t ret = RTNO_OK;
ysuga 0:5f7bc45bc2e8 205 int8_t retval = EC_activate_component();
ysuga 0:5f7bc45bc2e8 206 if(retval < 0) ret = RTNO_ERROR;
ysuga 0:5f7bc45bc2e8 207 Transport_SendPacket(ACTIVATE, 1, &ret);
ysuga 0:5f7bc45bc2e8 208 }
ysuga 0:5f7bc45bc2e8 209
ysuga 0:5f7bc45bc2e8 210 /**
ysuga 0:5f7bc45bc2e8 211 * Packet Handler in Active State.
ysuga 0:5f7bc45bc2e8 212 */
ysuga 0:5f7bc45bc2e8 213 PRIVATE void _PacketHandlerOnActive() {
ysuga 0:5f7bc45bc2e8 214 int8_t ret = RTNO_OK;
ysuga 0:5f7bc45bc2e8 215 //char intface;
ysuga 0:5f7bc45bc2e8 216 int8_t retval;
ysuga 0:5f7bc45bc2e8 217 switch(m_pPacketBuffer[INTERFACE]) {
ysuga 0:5f7bc45bc2e8 218 case DEACTIVATE:
ysuga 0:5f7bc45bc2e8 219 retval = EC_deactivate_component();
ysuga 0:5f7bc45bc2e8 220 if(retval < 0) ret = RTNO_ERROR;
ysuga 0:5f7bc45bc2e8 221 Transport_SendPacket(DEACTIVATE, 1, &ret);
ysuga 0:5f7bc45bc2e8 222 break;
ysuga 0:5f7bc45bc2e8 223 case EXECUTE:
ysuga 0:5f7bc45bc2e8 224 retval = EC_execute();
ysuga 0:5f7bc45bc2e8 225 if(retval < 0) ret = RTNO_ERROR;
ysuga 0:5f7bc45bc2e8 226 Transport_SendPacket(EXECUTE, 1, &ret);
ysuga 0:5f7bc45bc2e8 227 break;
ysuga 0:5f7bc45bc2e8 228 case SEND_DATA: {
ysuga 0:5f7bc45bc2e8 229 PortBase* pInPort = RTnoProfile_getInPort((const char*)&(m_pPacketBuffer[DATA_START_ADDR+2]), m_pPacketBuffer[DATA_START_ADDR]);
ysuga 0:5f7bc45bc2e8 230 if(pInPort == NULL) {
ysuga 0:5f7bc45bc2e8 231
ysuga 0:5f7bc45bc2e8 232 } else {
ysuga 0:5f7bc45bc2e8 233 PortBuffer* pBuffer = pInPort->pPortBuffer;
ysuga 0:5f7bc45bc2e8 234 EC_suspend();
ysuga 0:5f7bc45bc2e8 235 pBuffer->push(pBuffer,&(m_pPacketBuffer[DATA_START_ADDR+2+m_pPacketBuffer[DATA_START_ADDR]]), m_pPacketBuffer[DATA_START_ADDR+1]);
ysuga 0:5f7bc45bc2e8 236 EC_resume();
ysuga 0:5f7bc45bc2e8 237 Transport_SendPacket(SEND_DATA, 1, &ret);
ysuga 0:5f7bc45bc2e8 238 }
ysuga 0:5f7bc45bc2e8 239 }
ysuga 0:5f7bc45bc2e8 240 break;
ysuga 0:5f7bc45bc2e8 241 default:
ysuga 0:5f7bc45bc2e8 242 break;
ysuga 0:5f7bc45bc2e8 243 }
ysuga 0:5f7bc45bc2e8 244 }
ysuga 0:5f7bc45bc2e8 245