nucho
/
RTno_MotorControl
RTno/RTno.cpp@1:7f0fc0d1f777, 2011-08-01 (annotated)
- Committer:
- nucho
- Date:
- Mon Aug 01 14:46:08 2011 +0000
- Revision:
- 1:7f0fc0d1f777
- Parent:
- 0:3c49891bc39d
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nucho | 0:3c49891bc39d | 1 | /******************************************* |
nucho | 0:3c49891bc39d | 2 | * RTno.cpp |
nucho | 0:3c49891bc39d | 3 | * @author Yuki Suga |
nucho | 0:3c49891bc39d | 4 | * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010. |
nucho | 0:3c49891bc39d | 5 | * @license LGPLv3 |
nucho | 0:3c49891bc39d | 6 | *****************************************/ |
nucho | 0:3c49891bc39d | 7 | /******************************************** |
nucho | 0:3c49891bc39d | 8 | It changed a little for mbed. |
nucho | 0:3c49891bc39d | 9 | The serial buffer is not used. |
nucho | 0:3c49891bc39d | 10 | 2011/7/27 @nucho |
nucho | 0:3c49891bc39d | 11 | ********************************************/ |
nucho | 0:3c49891bc39d | 12 | |
nucho | 0:3c49891bc39d | 13 | #include "RTno.h" |
nucho | 0:3c49891bc39d | 14 | //#include "WConstants.h" |
nucho | 0:3c49891bc39d | 15 | //#include "HardwareSerial.h" |
nucho | 0:3c49891bc39d | 16 | #include "ReceivePacket.h" |
nucho | 0:3c49891bc39d | 17 | #include "SendPacket.h" |
nucho | 0:3c49891bc39d | 18 | #include "Serial.h" |
nucho | 0:3c49891bc39d | 19 | #include "mbed.h" |
nucho | 0:3c49891bc39d | 20 | |
nucho | 0:3c49891bc39d | 21 | using namespace RTno; |
nucho | 0:3c49891bc39d | 22 | |
nucho | 0:3c49891bc39d | 23 | config_str conf; |
nucho | 0:3c49891bc39d | 24 | exec_cxt_str exec_cxt; |
nucho | 0:3c49891bc39d | 25 | |
nucho | 0:3c49891bc39d | 26 | enum { |
nucho | 0:3c49891bc39d | 27 | CREATED='C', |
nucho | 0:3c49891bc39d | 28 | INACTIVE='I', |
nucho | 0:3c49891bc39d | 29 | ACTIVE='A', |
nucho | 0:3c49891bc39d | 30 | ERROR='E', |
nucho | 0:3c49891bc39d | 31 | }; |
nucho | 0:3c49891bc39d | 32 | |
nucho | 0:3c49891bc39d | 33 | char condition = CREATED; |
nucho | 0:3c49891bc39d | 34 | static char packet_buffer[64]; |
nucho | 0:3c49891bc39d | 35 | |
nucho | 0:3c49891bc39d | 36 | void RTno::setup() { |
nucho | 0:3c49891bc39d | 37 | rtcconf(); |
nucho | 0:3c49891bc39d | 38 | pc.baud(conf._default.baudrate); |
nucho | 0:3c49891bc39d | 39 | } |
nucho | 0:3c49891bc39d | 40 | |
nucho | 0:3c49891bc39d | 41 | |
nucho | 0:3c49891bc39d | 42 | static const char rtc_ok = RTNO_OK; |
nucho | 0:3c49891bc39d | 43 | static const char rtc_error = RTNO_ERROR; |
nucho | 0:3c49891bc39d | 44 | #define LP_RTC_OK (&rtc_ok) |
nucho | 0:3c49891bc39d | 45 | #define LP_RTC_ERROR (&rtc_error) |
nucho | 0:3c49891bc39d | 46 | |
nucho | 0:3c49891bc39d | 47 | |
nucho | 0:3c49891bc39d | 48 | int main() { |
nucho | 0:3c49891bc39d | 49 | RTno::setup(); |
nucho | 0:3c49891bc39d | 50 | |
nucho | 0:3c49891bc39d | 51 | while(1){ |
nucho | 0:3c49891bc39d | 52 | RTno::loop(); |
nucho | 0:3c49891bc39d | 53 | } |
nucho | 0:3c49891bc39d | 54 | } |
nucho | 0:3c49891bc39d | 55 | |
nucho | 0:3c49891bc39d | 56 | void RTno::loop() { |
nucho | 0:3c49891bc39d | 57 | ReceivePacket(packet_buffer); |
nucho | 0:3c49891bc39d | 58 | |
nucho | 0:3c49891bc39d | 59 | switch (condition) { |
nucho | 0:3c49891bc39d | 60 | case CREATED: |
nucho | 0:3c49891bc39d | 61 | if (packet_buffer[INTERFACE] == INITIALIZE) { |
nucho | 0:3c49891bc39d | 62 | if (onInitialize() == RTC_OK) { |
nucho | 0:3c49891bc39d | 63 | condition = INACTIVE; |
nucho | 0:3c49891bc39d | 64 | SendPacket(INITIALIZE, 1, LP_RTC_OK); |
nucho | 0:3c49891bc39d | 65 | } else { |
nucho | 0:3c49891bc39d | 66 | condition = ERROR; |
nucho | 0:3c49891bc39d | 67 | SendPacket(INITIALIZE, 1, LP_RTC_ERROR); |
nucho | 0:3c49891bc39d | 68 | } |
nucho | 0:3c49891bc39d | 69 | } |
nucho | 0:3c49891bc39d | 70 | break; |
nucho | 0:3c49891bc39d | 71 | case ERROR: |
nucho | 0:3c49891bc39d | 72 | if (packet_buffer[INTERFACE] == ONERROR) { |
nucho | 0:3c49891bc39d | 73 | onError(); |
nucho | 0:3c49891bc39d | 74 | SendPacket(ONERROR, 1, LP_RTC_OK); |
nucho | 0:3c49891bc39d | 75 | } else if (packet_buffer[INTERFACE] == RESET) { |
nucho | 0:3c49891bc39d | 76 | if (onReset() == RTC_OK) { |
nucho | 0:3c49891bc39d | 77 | condition = INACTIVE; |
nucho | 0:3c49891bc39d | 78 | SendPacket(RESET, 1, LP_RTC_OK); |
nucho | 0:3c49891bc39d | 79 | } else { |
nucho | 0:3c49891bc39d | 80 | condition = ERROR; |
nucho | 0:3c49891bc39d | 81 | SendPacket(RESET, 1, LP_RTC_ERROR); |
nucho | 0:3c49891bc39d | 82 | } |
nucho | 0:3c49891bc39d | 83 | } |
nucho | 0:3c49891bc39d | 84 | break; |
nucho | 0:3c49891bc39d | 85 | case INACTIVE: |
nucho | 0:3c49891bc39d | 86 | if (packet_buffer[INTERFACE] == ACTIVATE) { |
nucho | 0:3c49891bc39d | 87 | if (onActivated() == RTC_OK) { |
nucho | 0:3c49891bc39d | 88 | condition = ACTIVE; |
nucho | 0:3c49891bc39d | 89 | SendPacket(ACTIVATE, 1, LP_RTC_OK); |
nucho | 0:3c49891bc39d | 90 | } else { |
nucho | 0:3c49891bc39d | 91 | condition = ERROR; |
nucho | 0:3c49891bc39d | 92 | SendPacket(ACTIVATE, 1, LP_RTC_ERROR); |
nucho | 0:3c49891bc39d | 93 | } |
nucho | 0:3c49891bc39d | 94 | } |
nucho | 0:3c49891bc39d | 95 | break; |
nucho | 0:3c49891bc39d | 96 | case ACTIVE: |
nucho | 0:3c49891bc39d | 97 | if (packet_buffer[INTERFACE] == DEACTIVATE) { |
nucho | 0:3c49891bc39d | 98 | onDeactivated(); |
nucho | 0:3c49891bc39d | 99 | condition = INACTIVE; |
nucho | 0:3c49891bc39d | 100 | SendPacket(DEACTIVATE, 1, LP_RTC_OK); |
nucho | 0:3c49891bc39d | 101 | } else if (packet_buffer[INTERFACE] == EXECUTE) { |
nucho | 0:3c49891bc39d | 102 | if (onExecute() == RTC_OK) { |
nucho | 0:3c49891bc39d | 103 | SendPacket(EXECUTE, 1, LP_RTC_OK); |
nucho | 0:3c49891bc39d | 104 | } else { |
nucho | 0:3c49891bc39d | 105 | condition = ERROR; |
nucho | 0:3c49891bc39d | 106 | SendPacket(EXECUTE, 1, LP_RTC_ERROR); |
nucho | 0:3c49891bc39d | 107 | } |
nucho | 0:3c49891bc39d | 108 | } |
nucho | 0:3c49891bc39d | 109 | break; |
nucho | 0:3c49891bc39d | 110 | default: |
nucho | 0:3c49891bc39d | 111 | condition = ERROR; |
nucho | 0:3c49891bc39d | 112 | break; |
nucho | 0:3c49891bc39d | 113 | } |
nucho | 0:3c49891bc39d | 114 | |
nucho | 0:3c49891bc39d | 115 | if (packet_buffer[INTERFACE] == GET_STATUS) { |
nucho | 0:3c49891bc39d | 116 | SendPacket(GET_STATUS, 1, &condition); |
nucho | 0:3c49891bc39d | 117 | } |
nucho | 0:3c49891bc39d | 118 | } |
nucho | 0:3c49891bc39d | 119 | |
nucho | 0:3c49891bc39d | 120 | |
nucho | 0:3c49891bc39d | 121 | /*NNN |
nucho | 0:3c49891bc39d | 122 | l:TimedLong |
nucho | 0:3c49891bc39d | 123 | d:TimedDouble |
nucho | 0:3c49891bc39d | 124 | f:TimedFloat |
nucho | 0:3c49891bc39d | 125 | L:TimedLongSeq |
nucho | 0:3c49891bc39d | 126 | D:TimedDoubleSeq |
nucho | 0:3c49891bc39d | 127 | F:TimedFloatSeq |
nucho | 0:3c49891bc39d | 128 | */ |
nucho | 0:3c49891bc39d | 129 | |
nucho | 0:3c49891bc39d | 130 | void addInPort(InPortBase& Port) { |
nucho | 0:3c49891bc39d | 131 | int len = strlen(Port.GetName()); |
nucho | 0:3c49891bc39d | 132 | char *data = (char*)malloc(len+1); |
nucho | 0:3c49891bc39d | 133 | data[0] = Port.GetTypeCode(); |
nucho | 0:3c49891bc39d | 134 | memcpy(&(data[1]), Port.GetName(), len); |
nucho | 0:3c49891bc39d | 135 | SendPacket(ADD_INPORT, len+1, data); |
nucho | 0:3c49891bc39d | 136 | ReceivePacket(packet_buffer); |
nucho | 0:3c49891bc39d | 137 | free(data); |
nucho | 0:3c49891bc39d | 138 | } |
nucho | 0:3c49891bc39d | 139 | |
nucho | 0:3c49891bc39d | 140 | void addOutPort(OutPortBase& Port) { |
nucho | 0:3c49891bc39d | 141 | int len = strlen(Port.GetName()); |
nucho | 0:3c49891bc39d | 142 | char *data = (char*)malloc(len+1); |
nucho | 0:3c49891bc39d | 143 | data[0] = Port.GetTypeCode(); |
nucho | 0:3c49891bc39d | 144 | memcpy(&(data[1]), Port.GetName(), len); |
nucho | 0:3c49891bc39d | 145 | SendPacket(ADD_OUTPORT, len+1, data); |
nucho | 0:3c49891bc39d | 146 | ReceivePacket(packet_buffer); |
nucho | 0:3c49891bc39d | 147 | free(data); |
nucho | 0:3c49891bc39d | 148 | } |