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