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@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?

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 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