Yoshito Onishi / Mbed 2 deprecated MotionerController

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   EthernetNetIf MCP2515 XBee mbed

main.cpp@1:c2f68b0adabb, 2013-01-18 (annotated)

Committer:
FalconOnishi
Date:
Fri Jan 18 17:07:53 2013 +0000
Revision:
1:c2f68b0adabb
Parent:
0:6af643b2bf72
Child:
2:0939d8f69886
Update variable names. Not tested yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
FalconOnishi 0:6af643b2bf72 1 #include "mbed.h"
FalconOnishi 0:6af643b2bf72 2 #include "XBeeWiFi.h"
FalconOnishi 0:6af643b2bf72 3 #include "EthernetNetIf.h"
FalconOnishi 0:6af643b2bf72 4 #include "TCPSocket.h"
FalconOnishi 0:6af643b2bf72 5 #include "MCP2515.h"
FalconOnishi 0:6af643b2bf72 6 #include "Utility.h"
FalconOnishi 0:6af643b2bf72 7
FalconOnishi 0:6af643b2bf72 8 #define DEBUG (1)
FalconOnishi 0:6af643b2bf72 9 #define PC_SERIAL (1)
FalconOnishi 0:6af643b2bf72 10
FalconOnishi 0:6af643b2bf72 11 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 12 //settings
FalconOnishi 0:6af643b2bf72 13 //#define SECURITY SECURITY_OPEN
FalconOnishi 0:6af643b2bf72 14 #define SECURITY SECURITY_WPA2
FalconOnishi 0:6af643b2bf72 15 #define SSID "f"
FalconOnishi 0:6af643b2bf72 16 #define PASSPHRASE "1q2w3e4r5t6y7u8i9o0p"
FalconOnishi 0:6af643b2bf72 17
FalconOnishi 0:6af643b2bf72 18 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 19 //transmit
FalconOnishi 0:6af643b2bf72 20 #define N_JOINTS (18)
FalconOnishi 0:6af643b2bf72 21 #define DATA_LEN (4*4*N_JOINTS+3*4+4) //18*quaternion*float bytes + position vector*flot bytes + delimiter
FalconOnishi 0:6af643b2bf72 22
FalconOnishi 0:6af643b2bf72 23 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 24 // functions
FalconOnishi 0:6af643b2bf72 25 bool initXBee(int baud);
FalconOnishi 0:6af643b2bf72 26 int initXBeeWiFi(int timeout);
FalconOnishi 0:6af643b2bf72 27 void setup();
FalconOnishi 0:6af643b2bf72 28 void loop();
FalconOnishi 0:6af643b2bf72 29 void updateCAN(float time);
FalconOnishi 0:6af643b2bf72 30 void sendCanMessage(uint8_t cmd);
FalconOnishi 0:6af643b2bf72 31 void sendMbedCanMessage(int id, uint8_t cmd);
FalconOnishi 0:6af643b2bf72 32
FalconOnishi 0:6af643b2bf72 33 void beginTransmitQuaternion();
FalconOnishi 0:6af643b2bf72 34 void endTransmitQuaternion();
FalconOnishi 0:6af643b2bf72 35
FalconOnishi 0:6af643b2bf72 36 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 37 // unions
FalconOnishi 0:6af643b2bf72 38 union floatAndByte {
FalconOnishi 0:6af643b2bf72 39 float f;
FalconOnishi 0:6af643b2bf72 40 byte b[4];
FalconOnishi 0:6af643b2bf72 41 };
FalconOnishi 0:6af643b2bf72 42
FalconOnishi 0:6af643b2bf72 43 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 44 // objects
FalconOnishi 0:6af643b2bf72 45 // Serial
FalconOnishi 1:c2f68b0adabb 46 Serial gPc(USBTX, USBRX);
FalconOnishi 0:6af643b2bf72 47
FalconOnishi 0:6af643b2bf72 48 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 49 // XBee WiFi
FalconOnishi 1:c2f68b0adabb 50 XBeeWiFi gXBee(p13, p14, p12, P0_22); // TX, RX, CTS, RTS
FalconOnishi 1:c2f68b0adabb 51 //XBeeWiFi gXBee(p13, p14, p12, p11); // TX, RX, CTS, RTS
FalconOnishi 0:6af643b2bf72 52
FalconOnishi 0:6af643b2bf72 53 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 54 //CAN
FalconOnishi 0:6af643b2bf72 55 #define BUS_SPEED (1000)
FalconOnishi 1:c2f68b0adabb 56 SPI gSpi(p5, p6, p7); // mosi, miso, sclk
FalconOnishi 1:c2f68b0adabb 57 //MCP2515 gCan(gSpi, p15);
FalconOnishi 1:c2f68b0adabb 58 MCP2515 gCan(gSpi, p8);
FalconOnishi 0:6af643b2bf72 59
FalconOnishi 1:c2f68b0adabb 60 bool gReceived[N_JOINTS][2];
FalconOnishi 1:c2f68b0adabb 61 float __gQuat__[N_JOINTS+1][4];
FalconOnishi 1:c2f68b0adabb 62 floatAndByte gTmpQuat[N_JOINTS+1][4];
FalconOnishi 0:6af643b2bf72 63
FalconOnishi 0:6af643b2bf72 64 //mbed CAN
FalconOnishi 0:6af643b2bf72 65 #define MBED_CAN_BUS_SPEED (1000000)
FalconOnishi 0:6af643b2bf72 66 #define N_MBED_CAN (2)
FalconOnishi 1:c2f68b0adabb 67 CAN gMbedCan0(p9, p10);
FalconOnishi 1:c2f68b0adabb 68 CAN gMbedCan1(p30, p29);
FalconOnishi 0:6af643b2bf72 69
FalconOnishi 1:c2f68b0adabb 70 CANMessage gMbedCanMessage[N_MBED_CAN];
FalconOnishi 0:6af643b2bf72 71
FalconOnishi 0:6af643b2bf72 72 //--------------------------------------------------------------
FalconOnishi 1:c2f68b0adabb 73 Timer gTimer;
FalconOnishi 1:c2f68b0adabb 74 long gFrame = 0;
FalconOnishi 0:6af643b2bf72 75
FalconOnishi 0:6af643b2bf72 76 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 77 // main()
FalconOnishi 0:6af643b2bf72 78 int main()
FalconOnishi 0:6af643b2bf72 79 {
FalconOnishi 0:6af643b2bf72 80 setup();
FalconOnishi 0:6af643b2bf72 81 for (;;)
FalconOnishi 0:6af643b2bf72 82 loop();
FalconOnishi 0:6af643b2bf72 83 }
FalconOnishi 0:6af643b2bf72 84
FalconOnishi 0:6af643b2bf72 85 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 86 bool initXBee(int baud)
FalconOnishi 0:6af643b2bf72 87 {
FalconOnishi 1:c2f68b0adabb 88 gPc.printf("Init XBee WiFi with %d baudrate\r\n", baud);
FalconOnishi 1:c2f68b0adabb 89 gXBee.begin(baud);
FalconOnishi 0:6af643b2bf72 90
FalconOnishi 0:6af643b2bf72 91 // set XBee baudrate as 921600!!(1Mbps)
FalconOnishi 1:c2f68b0adabb 92 // then we cann't comunicate with XbeeWiFi using gPc
FalconOnishi 0:6af643b2bf72 93 // so we should set baudrate under 115200 when we want to comunicate with it
FalconOnishi 1:c2f68b0adabb 94 gXBee.baud(921600);
FalconOnishi 0:6af643b2bf72 95
FalconOnishi 0:6af643b2bf72 96 if (initXBeeWiFi(20)) {
FalconOnishi 0:6af643b2bf72 97 return false;
FalconOnishi 0:6af643b2bf72 98 } else {
FalconOnishi 1:c2f68b0adabb 99 gPc.printf("succeed\r\n");
FalconOnishi 0:6af643b2bf72 100 return true;
FalconOnishi 0:6af643b2bf72 101 }
FalconOnishi 0:6af643b2bf72 102 }
FalconOnishi 0:6af643b2bf72 103
FalconOnishi 0:6af643b2bf72 104 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 105 void setup()
FalconOnishi 0:6af643b2bf72 106 {
FalconOnishi 1:c2f68b0adabb 107 gPc.baud(115200);
FalconOnishi 1:c2f68b0adabb 108 gPc.printf("[ram::Motioner]\r\n");
FalconOnishi 0:6af643b2bf72 109
FalconOnishi 0:6af643b2bf72 110 if (!initXBee(115200))
FalconOnishi 0:6af643b2bf72 111 if (!initXBee(921600))
FalconOnishi 0:6af643b2bf72 112 error("XBee init failure\r\n");
FalconOnishi 0:6af643b2bf72 113
FalconOnishi 0:6af643b2bf72 114 IpAddr ipaddr, netmask, gateway, nameserver;
FalconOnishi 1:c2f68b0adabb 115 gXBee.getAddress(ipaddr, netmask, gateway, nameserver);
FalconOnishi 1:c2f68b0adabb 116 gPc.printf("IP address %d.%d.%d.%d\r\n", ipaddr[0], ipaddr[1], ipaddr[2], ipaddr[3]);
FalconOnishi 0:6af643b2bf72 117 if (ipaddr == 0) {
FalconOnishi 1:c2f68b0adabb 118 gPc.printf("not configure\r\n");
FalconOnishi 0:6af643b2bf72 119 error("");
FalconOnishi 0:6af643b2bf72 120 }
FalconOnishi 0:6af643b2bf72 121
FalconOnishi 0:6af643b2bf72 122 // CAN
FalconOnishi 1:c2f68b0adabb 123 //gSpi.format( 8, 1 ); //Arduino's SPI Mode = 1
FalconOnishi 1:c2f68b0adabb 124 gSpi.frequency( 8000000 ); //8MHz
FalconOnishi 0:6af643b2bf72 125
FalconOnishi 1:c2f68b0adabb 126 gPc.printf("CAN reset\r\n");
FalconOnishi 1:c2f68b0adabb 127 gCan.reset();
FalconOnishi 1:c2f68b0adabb 128 gPc.printf("CAN baudConfig\r\n");
FalconOnishi 1:c2f68b0adabb 129 gCan.baudConfig(BUS_SPEED);
FalconOnishi 1:c2f68b0adabb 130 //gCan.configRate(CAN_500KBPS_8MHZ);
FalconOnishi 1:c2f68b0adabb 131 gPc.printf("CAN setMask\r\n");
FalconOnishi 1:c2f68b0adabb 132 //gCan.setMask(MASK_SID_CPL_MATCH);//test no mask
FalconOnishi 1:c2f68b0adabb 133 //gCan.setMask(MASK_SID_ALL_HIT);//test no mask
FalconOnishi 0:6af643b2bf72 134
FalconOnishi 0:6af643b2bf72 135 //enable RX1 and RX0 buffers and rollover
FalconOnishi 1:c2f68b0adabb 136 gCan.bitModify(RXB0CTRL, RXB_RX_MASK | RXB_BUKT_MASK, RXB_RX_STDEXT | RXB_BUKT_MASK );
FalconOnishi 1:c2f68b0adabb 137 gCan.bitModify(RXB1CTRL, RXB_RX_MASK, RXB_RX_STDEXT);
FalconOnishi 0:6af643b2bf72 138
FalconOnishi 1:c2f68b0adabb 139 //gCan.setRegister(MCP_RXM0SIDL, 0x11);
FalconOnishi 1:c2f68b0adabb 140 //gCan.setRegister(MCP_RXM0SIDH, 0x32);
FalconOnishi 0:6af643b2bf72 141
FalconOnishi 0:6af643b2bf72 142 byte addrHi;
FalconOnishi 0:6af643b2bf72 143 byte addrLo;
FalconOnishi 0:6af643b2bf72 144
FalconOnishi 1:c2f68b0adabb 145 gPc.printf("readRegister\r\n");
FalconOnishi 1:c2f68b0adabb 146 gCan.readRegister(RXM0SIDH, &addrHi);
FalconOnishi 1:c2f68b0adabb 147 gPc.printf("readRegister\r\n");
FalconOnishi 1:c2f68b0adabb 148 gCan.readRegister(RXM0SIDL, &addrLo);
FalconOnishi 0:6af643b2bf72 149
FalconOnishi 1:c2f68b0adabb 150 gPc.printf("CAN address");
FalconOnishi 1:c2f68b0adabb 151 gPc.printf("%02x",addrHi);
FalconOnishi 1:c2f68b0adabb 152 gPc.printf("%02x",addrLo);
FalconOnishi 1:c2f68b0adabb 153 gPc.printf("\r\n");
FalconOnishi 0:6af643b2bf72 154
FalconOnishi 1:c2f68b0adabb 155 //gCan.setMode(LOOPBACK);
FalconOnishi 1:c2f68b0adabb 156 gCan.setMode(NORMAL);
FalconOnishi 0:6af643b2bf72 157
FalconOnishi 0:6af643b2bf72 158 //
FalconOnishi 1:c2f68b0adabb 159 gMbedCan0.frequency(MBED_CAN_BUS_SPEED);
FalconOnishi 1:c2f68b0adabb 160 gMbedCan1.frequency(MBED_CAN_BUS_SPEED);
FalconOnishi 0:6af643b2bf72 161
FalconOnishi 0:6af643b2bf72 162 wait(1.0f);
FalconOnishi 0:6af643b2bf72 163
FalconOnishi 1:c2f68b0adabb 164 gTimer .start();
FalconOnishi 0:6af643b2bf72 165
FalconOnishi 0:6af643b2bf72 166 //freezeTimer.start();
FalconOnishi 0:6af643b2bf72 167 //freezeChecker.attach(&freezeCheck, 0.3f);
FalconOnishi 0:6af643b2bf72 168 }
FalconOnishi 0:6af643b2bf72 169
FalconOnishi 0:6af643b2bf72 170 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 171 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 172 void beginTransmitQuaternion()
FalconOnishi 0:6af643b2bf72 173 {
FalconOnishi 0:6af643b2bf72 174 sendCanMessage('b');
FalconOnishi 0:6af643b2bf72 175 for (int i=0; i<N_MBED_CAN; i++)
FalconOnishi 0:6af643b2bf72 176 sendMbedCanMessage(i, 'b');
FalconOnishi 0:6af643b2bf72 177 }
FalconOnishi 0:6af643b2bf72 178
FalconOnishi 0:6af643b2bf72 179 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 180 void endTransmitQuaternion()
FalconOnishi 0:6af643b2bf72 181 {
FalconOnishi 0:6af643b2bf72 182 sendCanMessage('e');
FalconOnishi 0:6af643b2bf72 183 for (int i=0; i<N_MBED_CAN; i++)
FalconOnishi 0:6af643b2bf72 184 sendMbedCanMessage(i, 'e');
FalconOnishi 0:6af643b2bf72 185 }
FalconOnishi 0:6af643b2bf72 186
FalconOnishi 0:6af643b2bf72 187 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 188 void sendCanMessage(uint8_t cmd)
FalconOnishi 0:6af643b2bf72 189 {
FalconOnishi 0:6af643b2bf72 190 uint8_t length = 3;
FalconOnishi 0:6af643b2bf72 191 uint8_t frame_data[length];
FalconOnishi 0:6af643b2bf72 192
FalconOnishi 0:6af643b2bf72 193 frame_data[0] = 'm';
FalconOnishi 0:6af643b2bf72 194 frame_data[1] = 't';
FalconOnishi 0:6af643b2bf72 195 frame_data[2] = cmd;
FalconOnishi 0:6af643b2bf72 196
FalconOnishi 0:6af643b2bf72 197 unsigned short frame_id = 0x0040;
FalconOnishi 0:6af643b2bf72 198
FalconOnishi 1:c2f68b0adabb 199 gCan.load_ff_0(length,frame_id,frame_data);
FalconOnishi 1:c2f68b0adabb 200 gCan.send_0();
FalconOnishi 0:6af643b2bf72 201 }
FalconOnishi 0:6af643b2bf72 202
FalconOnishi 0:6af643b2bf72 203 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 204 void sendMbedCanMessage(int id, uint8_t cmd)
FalconOnishi 0:6af643b2bf72 205 {
FalconOnishi 0:6af643b2bf72 206 CANMessage msg;
FalconOnishi 0:6af643b2bf72 207 msg.len = 3;
FalconOnishi 0:6af643b2bf72 208 msg.data[0] = 'm';
FalconOnishi 0:6af643b2bf72 209 msg.data[1] = 't';
FalconOnishi 0:6af643b2bf72 210 msg.data[2] = cmd;
FalconOnishi 0:6af643b2bf72 211 msg.id = 0x0040;
FalconOnishi 0:6af643b2bf72 212
FalconOnishi 0:6af643b2bf72 213 if (id==0)
FalconOnishi 1:c2f68b0adabb 214 gMbedCan0.write(msg);
FalconOnishi 0:6af643b2bf72 215 else
FalconOnishi 1:c2f68b0adabb 216 gMbedCan1.write(msg);
FalconOnishi 0:6af643b2bf72 217 }
FalconOnishi 0:6af643b2bf72 218
FalconOnishi 0:6af643b2bf72 219 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 220
FalconOnishi 0:6af643b2bf72 221 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 222 void updateCAN(float time)
FalconOnishi 0:6af643b2bf72 223 {
FalconOnishi 1:c2f68b0adabb 224 const float canBegin = gTimer.read();
FalconOnishi 0:6af643b2bf72 225 int nCanUpdate = 0;
FalconOnishi 0:6af643b2bf72 226
FalconOnishi 0:6af643b2bf72 227 beginTransmitQuaternion();
FalconOnishi 0:6af643b2bf72 228
FalconOnishi 0:6af643b2bf72 229 // loop for update can
FalconOnishi 1:c2f68b0adabb 230 while (gTimer.read()-canBegin < time) {
FalconOnishi 0:6af643b2bf72 231 //CAN
FalconOnishi 0:6af643b2bf72 232 byte nodeId = 0x00;
FalconOnishi 0:6af643b2bf72 233 byte length = 0x00, rx_status = 0x00;
FalconOnishi 0:6af643b2bf72 234 unsigned short frame_id = 0x0000;
FalconOnishi 0:6af643b2bf72 235 byte frame_data[8];
FalconOnishi 0:6af643b2bf72 236
FalconOnishi 0:6af643b2bf72 237 for (int i=0; i<8; i++)
FalconOnishi 0:6af643b2bf72 238 frame_data[i] = 0x00;
FalconOnishi 0:6af643b2bf72 239
FalconOnishi 1:c2f68b0adabb 240 rx_status = gCan.readStatus();
FalconOnishi 0:6af643b2bf72 241
FalconOnishi 0:6af643b2bf72 242 if ((rx_status & 0x01) == 0x01) {
FalconOnishi 1:c2f68b0adabb 243 gCan.readDATA_ff_0(&length,frame_data,&frame_id);
FalconOnishi 1:c2f68b0adabb 244 //gPc.printf("0");
FalconOnishi 0:6af643b2bf72 245 } else if ((rx_status & 0x02) == 0x02) {
FalconOnishi 1:c2f68b0adabb 246 gCan.readDATA_ff_1(&length,frame_data,&frame_id);
FalconOnishi 1:c2f68b0adabb 247 //gPc.printf("1");
FalconOnishi 0:6af643b2bf72 248 }
FalconOnishi 0:6af643b2bf72 249
FalconOnishi 0:6af643b2bf72 250 if (((rx_status & 0x01) == 0x01) || ((rx_status & 0x02) == 0x02)) {
FalconOnishi 0:6af643b2bf72 251
FalconOnishi 0:6af643b2bf72 252 // 11bit address
FalconOnishi 0:6af643b2bf72 253 nodeId = frame_id >> 3;
FalconOnishi 0:6af643b2bf72 254 uint8_t slot = frame_id & 0x01;
FalconOnishi 0:6af643b2bf72 255
FalconOnishi 1:c2f68b0adabb 256 //if (gFrame % 120 == 0)
FalconOnishi 1:c2f68b0adabb 257 // gPc.printf("%d %d, ", frame_id, nodeId);
FalconOnishi 0:6af643b2bf72 258
FalconOnishi 1:c2f68b0adabb 259 gReceived[nodeId][slot] = true;
FalconOnishi 0:6af643b2bf72 260
FalconOnishi 0:6af643b2bf72 261 // 0to18 Joints and RX buffer*2(quaternion xy or zw) and 8 byte CAN data
FalconOnishi 0:6af643b2bf72 262 if (nodeId < N_JOINTS && slot < 2 && length == 8) {
FalconOnishi 1:c2f68b0adabb 263 //gPc.printf("%x, %x\n\r", nodeId, slot);
FalconOnishi 0:6af643b2bf72 264
FalconOnishi 0:6af643b2bf72 265 for (int q=0; q<2; q++) { //quaternion x, y or z, w
FalconOnishi 0:6af643b2bf72 266 for (int f=0; f<4; f++) { //float byte 0 - 4
FalconOnishi 0:6af643b2bf72 267 //[node id][quaternion x, y or z, w][float bytes 0 - 4]
FalconOnishi 1:c2f68b0adabb 268 gTmpQuat[nodeId][q+slot*2].b[f] = frame_data[f+q*4];
FalconOnishi 0:6af643b2bf72 269 }
FalconOnishi 0:6af643b2bf72 270 }
FalconOnishi 0:6af643b2bf72 271 }
FalconOnishi 0:6af643b2bf72 272 }
FalconOnishi 0:6af643b2bf72 273
FalconOnishi 0:6af643b2bf72 274 //mbed CAN
FalconOnishi 0:6af643b2bf72 275 for (int canId=0; canId<N_MBED_CAN; canId++) {
FalconOnishi 0:6af643b2bf72 276
FalconOnishi 0:6af643b2bf72 277 CAN *can = NULL;
FalconOnishi 0:6af643b2bf72 278 byte nodeId = 0x00;
FalconOnishi 0:6af643b2bf72 279
FalconOnishi 0:6af643b2bf72 280 if (canId == 0)
FalconOnishi 1:c2f68b0adabb 281 can = &gMbedCan0;
FalconOnishi 0:6af643b2bf72 282 else
FalconOnishi 1:c2f68b0adabb 283 can = &gMbedCan1;
FalconOnishi 0:6af643b2bf72 284
FalconOnishi 1:c2f68b0adabb 285 if (can->read(gMbedCanMessage[canId])) {
FalconOnishi 0:6af643b2bf72 286
FalconOnishi 1:c2f68b0adabb 287 CANMessage &msg = gMbedCanMessage[canId];
FalconOnishi 0:6af643b2bf72 288
FalconOnishi 0:6af643b2bf72 289 nodeId = msg.id >> 3;
FalconOnishi 0:6af643b2bf72 290 uint8_t slot = msg.id & 0x01;
FalconOnishi 1:c2f68b0adabb 291 gReceived[nodeId][slot] = true;
FalconOnishi 0:6af643b2bf72 292
FalconOnishi 0:6af643b2bf72 293 // 0to18 Joints and RX buffer*2(quaternion xy or zw) and 8 byte CAN data
FalconOnishi 0:6af643b2bf72 294 if (nodeId < N_JOINTS && slot < 2 && msg.len == 8) {
FalconOnishi 1:c2f68b0adabb 295 //gPc.printf("%x, %x\n\r", nodeId, slot);
FalconOnishi 0:6af643b2bf72 296
FalconOnishi 0:6af643b2bf72 297 for (int q=0; q<2; q++) { //quaternion x, y or z, w
FalconOnishi 0:6af643b2bf72 298 for (int f=0; f<4; f++) { //float byte 0 - 4
FalconOnishi 0:6af643b2bf72 299 //[node id][quaternion x, y or z, w][float bytes 0 - 4]
FalconOnishi 1:c2f68b0adabb 300 gTmpQuat[nodeId][q+slot*2].b[f] = msg.data[f+q*4];
FalconOnishi 0:6af643b2bf72 301 }
FalconOnishi 0:6af643b2bf72 302 }
FalconOnishi 0:6af643b2bf72 303 }
FalconOnishi 0:6af643b2bf72 304 }
FalconOnishi 0:6af643b2bf72 305 }
FalconOnishi 0:6af643b2bf72 306
FalconOnishi 0:6af643b2bf72 307 nCanUpdate++; //how many times could we update can?
FalconOnishi 0:6af643b2bf72 308 }// loop for update can
FalconOnishi 0:6af643b2bf72 309
FalconOnishi 1:c2f68b0adabb 310 //if (gFrame % 120 == 0)
FalconOnishi 1:c2f68b0adabb 311 // gPc.printf("\n\r");
FalconOnishi 0:6af643b2bf72 312
FalconOnishi 1:c2f68b0adabb 313 if (gFrame % 240 == 0) {
FalconOnishi 1:c2f68b0adabb 314 //gPc.printf("\r\n");
FalconOnishi 1:c2f68b0adabb 315 const float elapsed = gTimer.read()-canBegin;
FalconOnishi 1:c2f68b0adabb 316 gPc.printf("c%f/%d\r\n", elapsed, nCanUpdate);
FalconOnishi 0:6af643b2bf72 317 for (int i=0; i<N_JOINTS; i++)
FalconOnishi 1:c2f68b0adabb 318 if (gReceived[i][0] || gReceived[i][1])
FalconOnishi 1:c2f68b0adabb 319 gPc.printf("%d:%d%d, ", i, gReceived[i][0], gReceived[i][1]);
FalconOnishi 1:c2f68b0adabb 320 //gPc.printf("%x: %f, %f, %f, %f\n\r", i, ___gQuat___[i][0].f, ____gQuat____[i][1].f, ____gQuat____[i][2].f, ___gQuat___[i][3].f);
FalconOnishi 1:c2f68b0adabb 321 gPc.printf("\r\n");
FalconOnishi 0:6af643b2bf72 322 }
FalconOnishi 0:6af643b2bf72 323
FalconOnishi 0:6af643b2bf72 324 endTransmitQuaternion();
FalconOnishi 0:6af643b2bf72 325
FalconOnishi 0:6af643b2bf72 326 }
FalconOnishi 0:6af643b2bf72 327 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 328 void updateSerial()
FalconOnishi 0:6af643b2bf72 329 {
FalconOnishi 1:c2f68b0adabb 330 while(gPc.readable()) {
FalconOnishi 1:c2f68b0adabb 331 char c = gPc.getc();
FalconOnishi 0:6af643b2bf72 332 switch (c) {
FalconOnishi 0:6af643b2bf72 333
FalconOnishi 0:6af643b2bf72 334 };
FalconOnishi 0:6af643b2bf72 335 }
FalconOnishi 0:6af643b2bf72 336 }
FalconOnishi 0:6af643b2bf72 337
FalconOnishi 0:6af643b2bf72 338 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 339 void updateXBeeWiFi()
FalconOnishi 0:6af643b2bf72 340 {
FalconOnishi 0:6af643b2bf72 341 IPv4TransmitRequest data;
FalconOnishi 0:6af643b2bf72 342
FalconOnishi 0:6af643b2bf72 343 //XBee WiFi Transmit
FalconOnishi 0:6af643b2bf72 344 //IPv4TransmitRequest data;
FalconOnishi 0:6af643b2bf72 345 //int len = 2*4*18+2*3; //18*quaternion+3*vector q // short
FalconOnishi 0:6af643b2bf72 346 //int len = 4*4*18+4*3; //18*quaternion+3*vector // float
FalconOnishi 0:6af643b2bf72 347 //int len = 8; //18*quaternion+3*vector // float
FalconOnishi 0:6af643b2bf72 348 //char buf[len];
FalconOnishi 0:6af643b2bf72 349
FalconOnishi 0:6af643b2bf72 350 // debug
FalconOnishi 0:6af643b2bf72 351 //for (int i=0; i<len; i++)
FalconOnishi 0:6af643b2bf72 352 //buf[i] = (char)('0'+i%70);
FalconOnishi 0:6af643b2bf72 353
FalconOnishi 0:6af643b2bf72 354 for (int i=0; i<N_JOINTS; i++) {
FalconOnishi 1:c2f68b0adabb 355 if (gReceived[i][0] && gReceived[i][1]) {
FalconOnishi 0:6af643b2bf72 356 for (int j=0; j<4; j++) {
FalconOnishi 1:c2f68b0adabb 357 __gQuat__[i][j] = gTmpQuat[i][j].f;
FalconOnishi 0:6af643b2bf72 358 }
FalconOnishi 0:6af643b2bf72 359 }
FalconOnishi 0:6af643b2bf72 360 }
FalconOnishi 0:6af643b2bf72 361
FalconOnishi 1:c2f68b0adabb 362 uint8_t *buf = (uint8_t *)__gQuat__;
FalconOnishi 0:6af643b2bf72 363
FalconOnishi 0:6af643b2bf72 364 buf[DATA_LEN-4] = (char)'a';
FalconOnishi 0:6af643b2bf72 365 buf[DATA_LEN-3] = (char)'b';
FalconOnishi 0:6af643b2bf72 366 buf[DATA_LEN-2] = (char)'c';
FalconOnishi 0:6af643b2bf72 367 buf[DATA_LEN-1] = (char)'d';
FalconOnishi 0:6af643b2bf72 368
FalconOnishi 0:6af643b2bf72 369
FalconOnishi 0:6af643b2bf72 370 IpAddr addr(192, 168, 2, 1);
FalconOnishi 0:6af643b2bf72 371 data.setAddress(addr);
FalconOnishi 0:6af643b2bf72 372 data.setDstPort(9750);
FalconOnishi 0:6af643b2bf72 373 data.setSrcPort(9750);
FalconOnishi 0:6af643b2bf72 374 data.setProtocol(PROTOCOL_UDP);
FalconOnishi 0:6af643b2bf72 375 //data.setPayload((uint8_t*)buf);
FalconOnishi 0:6af643b2bf72 376 //data.setPayloadLength(strlen(buf));
FalconOnishi 0:6af643b2bf72 377 data.setPayload(buf);
FalconOnishi 0:6af643b2bf72 378 data.setPayloadLength(DATA_LEN);
FalconOnishi 1:c2f68b0adabb 379 data.setFrameId(gXBee.getNextFrameId());//0
FalconOnishi 1:c2f68b0adabb 380 gXBee.send(data);
FalconOnishi 0:6af643b2bf72 381
FalconOnishi 1:c2f68b0adabb 382 //gPc.printf("len:%d\r\n",strlen(buf));
FalconOnishi 1:c2f68b0adabb 383 const int r = gXBee.getWiResponse(TX_STATUS_RESPONSE, data.getFrameId());
FalconOnishi 0:6af643b2bf72 384
FalconOnishi 0:6af643b2bf72 385 /// error
FalconOnishi 0:6af643b2bf72 386 if (r==-1) {
FalconOnishi 1:c2f68b0adabb 387 gPc.printf("Fatal error! XBee WiFi didn't responce!\r\nTrying to reset modem again.");
FalconOnishi 0:6af643b2bf72 388 if (initXBeeWiFi(20)) {
FalconOnishi 1:c2f68b0adabb 389 gPc.printf("Failure...");
FalconOnishi 0:6af643b2bf72 390 error("");
FalconOnishi 0:6af643b2bf72 391 } else {
FalconOnishi 1:c2f68b0adabb 392 gPc.printf("succeed\r\n");
FalconOnishi 0:6af643b2bf72 393 }
FalconOnishi 0:6af643b2bf72 394 }
FalconOnishi 0:6af643b2bf72 395 }
FalconOnishi 0:6af643b2bf72 396
FalconOnishi 0:6af643b2bf72 397 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 398 void loop()
FalconOnishi 0:6af643b2bf72 399 {
FalconOnishi 1:c2f68b0adabb 400 const float transBegin = gTimer.read();
FalconOnishi 0:6af643b2bf72 401
FalconOnishi 0:6af643b2bf72 402 updateSerial();
FalconOnishi 0:6af643b2bf72 403
FalconOnishi 0:6af643b2bf72 404 updateXBeeWiFi();
FalconOnishi 0:6af643b2bf72 405
FalconOnishi 1:c2f68b0adabb 406 const float elapsed = gTimer.read() - transBegin;
FalconOnishi 0:6af643b2bf72 407 const float frameTime = 1.0f/60.0f;
FalconOnishi 0:6af643b2bf72 408 const float t = frameTime-elapsed;
FalconOnishi 0:6af643b2bf72 409
FalconOnishi 1:c2f68b0adabb 410 if (gFrame % 120 == 0)
FalconOnishi 1:c2f68b0adabb 411 gPc.printf("x%f\r\n", elapsed);
FalconOnishi 0:6af643b2bf72 412
FalconOnishi 1:c2f68b0adabb 413 gFrame++;
FalconOnishi 0:6af643b2bf72 414
FalconOnishi 0:6af643b2bf72 415 for (int i=0; i<N_JOINTS; i++)
FalconOnishi 0:6af643b2bf72 416 for (int j=0; j<2; j++)
FalconOnishi 1:c2f68b0adabb 417 gReceived[i][j] = false;
FalconOnishi 0:6af643b2bf72 418
FalconOnishi 0:6af643b2bf72 419 updateCAN(t);
FalconOnishi 0:6af643b2bf72 420 }
FalconOnishi 0:6af643b2bf72 421
FalconOnishi 0:6af643b2bf72 422 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 423 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 424 int initXBeeWiFi(int timeout)
FalconOnishi 0:6af643b2bf72 425 {
FalconOnishi 0:6af643b2bf72 426 int i, r;
FalconOnishi 0:6af643b2bf72 427
FalconOnishi 1:c2f68b0adabb 428 gPc.printf("Reset XBee Wi-Fi\r\n");
FalconOnishi 1:c2f68b0adabb 429 r = gXBee.reset();
FalconOnishi 0:6af643b2bf72 430 if (r < 0) {
FalconOnishi 1:c2f68b0adabb 431 gPc.printf("Error reset %d\r\n", r);
FalconOnishi 0:6af643b2bf72 432 return -1;
FalconOnishi 0:6af643b2bf72 433 }
FalconOnishi 0:6af643b2bf72 434
FalconOnishi 1:c2f68b0adabb 435 gPc.printf("Get Responce from XBee Wi-Fi\r\n");
FalconOnishi 1:c2f68b0adabb 436 gXBee.getWiResponse(MODEM_STATUS_RESPONSE, 5000);
FalconOnishi 0:6af643b2bf72 437
FalconOnishi 1:c2f68b0adabb 438 gPc.printf("Setup XBee Wi-Fi %d\r\n", r);
FalconOnishi 1:c2f68b0adabb 439 r = gXBee.setup(SECURITY, SSID, PASSPHRASE);
FalconOnishi 1:c2f68b0adabb 440 //r = gXBee.setup(SSID);
FalconOnishi 0:6af643b2bf72 441 if (r < 0) {
FalconOnishi 1:c2f68b0adabb 442 gPc.printf("Error setup %d\r\n", r);
FalconOnishi 0:6af643b2bf72 443 return -1;
FalconOnishi 0:6af643b2bf72 444 }
FalconOnishi 0:6af643b2bf72 445
FalconOnishi 0:6af643b2bf72 446 for (i = 0; i < timeout; i ++) {
FalconOnishi 0:6af643b2bf72 447 wait(1.0f);
FalconOnishi 1:c2f68b0adabb 448 r = gXBee.getStatus();
FalconOnishi 1:c2f68b0adabb 449 gPc.printf("Status %02x: ", r);
FalconOnishi 0:6af643b2bf72 450 switch (r) {
FalconOnishi 0:6af643b2bf72 451 case JOINED_AP:
FalconOnishi 1:c2f68b0adabb 452 gPc.printf("Successfully joined an access point.\r\n");
FalconOnishi 0:6af643b2bf72 453 return 0;
FalconOnishi 0:6af643b2bf72 454 case INITIALIZATION:
FalconOnishi 1:c2f68b0adabb 455 gPc.printf("WiFi initialization in progress.\r\n");
FalconOnishi 0:6af643b2bf72 456 break;
FalconOnishi 0:6af643b2bf72 457 case SSID_NOT_FOUND:
FalconOnishi 1:c2f68b0adabb 458 gPc.printf("SSID not found.\r\n");
FalconOnishi 0:6af643b2bf72 459 return -1;
FalconOnishi 0:6af643b2bf72 460 case SSID_NOT_CONFIGURED:
FalconOnishi 1:c2f68b0adabb 461 gPc.printf("SSID not configured.\r\n");
FalconOnishi 0:6af643b2bf72 462 return -1;
FalconOnishi 0:6af643b2bf72 463 case JOIN_FAILED:
FalconOnishi 1:c2f68b0adabb 464 gPc.printf("SSID join failed.\r\n");
FalconOnishi 0:6af643b2bf72 465 return -1;
FalconOnishi 0:6af643b2bf72 466 case WAITING_IPADDRESS:
FalconOnishi 1:c2f68b0adabb 467 gPc.printf("Waiting for IP configuration.\r\n");
FalconOnishi 0:6af643b2bf72 468 break;
FalconOnishi 0:6af643b2bf72 469 case WAITING_SOCKETS:
FalconOnishi 1:c2f68b0adabb 470 gPc.printf("Listening sockets are being set up.\r\n");
FalconOnishi 0:6af643b2bf72 471 break;
FalconOnishi 0:6af643b2bf72 472 case SCANNING_SSID:
FalconOnishi 1:c2f68b0adabb 473 gPc.printf("Currently scanning for SSID.\r\n");
FalconOnishi 0:6af643b2bf72 474 break;
FalconOnishi 0:6af643b2bf72 475 default:
FalconOnishi 1:c2f68b0adabb 476 gPc.printf("\r\n");
FalconOnishi 0:6af643b2bf72 477 break;
FalconOnishi 0:6af643b2bf72 478 }
FalconOnishi 0:6af643b2bf72 479 }
FalconOnishi 0:6af643b2bf72 480 return -1;
FalconOnishi 0:6af643b2bf72 481 }