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@0:6af643b2bf72, 2013-01-18 (annotated)

Committer:
FalconOnishi
Date:
Fri Jan 18 17:01:02 2013 +0000
Revision:
0:6af643b2bf72
Child:
1:c2f68b0adabb
First commit

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 0:6af643b2bf72 46 Serial pc(USBTX, USBRX);
FalconOnishi 0:6af643b2bf72 47
FalconOnishi 0:6af643b2bf72 48 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 49 // XBee WiFi
FalconOnishi 0:6af643b2bf72 50 XBeeWiFi xbee(p13, p14, p12, P0_22); // TX, RX, CTS, RTS
FalconOnishi 0:6af643b2bf72 51 //XBeeWiFi xbee(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 0:6af643b2bf72 56 SPI spi(p5, p6, p7); // mosi, miso, sclk
FalconOnishi 0:6af643b2bf72 57 //MCP2515 can(spi, p15);
FalconOnishi 0:6af643b2bf72 58 MCP2515 can(spi, p8);
FalconOnishi 0:6af643b2bf72 59
FalconOnishi 0:6af643b2bf72 60 bool received[N_JOINTS][2];
FalconOnishi 0:6af643b2bf72 61 float __quat__[N_JOINTS+1][4];
FalconOnishi 0:6af643b2bf72 62 floatAndByte tmpQuat[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 0:6af643b2bf72 67 CAN mbedCan0(p9, p10);
FalconOnishi 0:6af643b2bf72 68 CAN mbedCan1(p30, p29);
FalconOnishi 0:6af643b2bf72 69
FalconOnishi 0:6af643b2bf72 70 CANMessage mbedCanMessage[N_MBED_CAN];
FalconOnishi 0:6af643b2bf72 71
FalconOnishi 0:6af643b2bf72 72 //--------------------------------------------------------------
FalconOnishi 0:6af643b2bf72 73 Timer timer;
FalconOnishi 0:6af643b2bf72 74 long frame = 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 0:6af643b2bf72 88 pc.printf("Init XBee WiFi with %d baudrate\r\n", baud);
FalconOnishi 0:6af643b2bf72 89 xbee.begin(baud);
FalconOnishi 0:6af643b2bf72 90
FalconOnishi 0:6af643b2bf72 91 // set XBee baudrate as 921600!!(1Mbps)
FalconOnishi 0:6af643b2bf72 92 // then we cann't comunicate with XbeeWiFi using pc
FalconOnishi 0:6af643b2bf72 93 // so we should set baudrate under 115200 when we want to comunicate with it
FalconOnishi 0:6af643b2bf72 94 xbee.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 0:6af643b2bf72 99 pc.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 0:6af643b2bf72 107 pc.baud(115200);
FalconOnishi 0:6af643b2bf72 108 pc.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 0:6af643b2bf72 115 xbee.getAddress(ipaddr, netmask, gateway, nameserver);
FalconOnishi 0:6af643b2bf72 116 pc.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 0:6af643b2bf72 118 pc.printf("not configure\r\n");
FalconOnishi 0:6af643b2bf72 119 error("");
FalconOnishi 0:6af643b2bf72 120 }
FalconOnishi 0:6af643b2bf72 121
FalconOnishi 0:6af643b2bf72 122 // CAN
FalconOnishi 0:6af643b2bf72 123 //spi.format( 8, 1 ); //Arduino's SPI Mode = 1
FalconOnishi 0:6af643b2bf72 124 spi.frequency( 8000000 ); //8MHz
FalconOnishi 0:6af643b2bf72 125
FalconOnishi 0:6af643b2bf72 126 pc.printf("CAN reset\r\n");
FalconOnishi 0:6af643b2bf72 127 can.reset();
FalconOnishi 0:6af643b2bf72 128 pc.printf("CAN baudConfig\r\n");
FalconOnishi 0:6af643b2bf72 129 can.baudConfig(BUS_SPEED);
FalconOnishi 0:6af643b2bf72 130 //can.configRate(CAN_500KBPS_8MHZ);
FalconOnishi 0:6af643b2bf72 131 pc.printf("CAN setMask\r\n");
FalconOnishi 0:6af643b2bf72 132 //can.setMask(MASK_SID_CPL_MATCH);//test no mask
FalconOnishi 0:6af643b2bf72 133 //can.setMask(MASK_SID_ALL_HIT);//test no mask
FalconOnishi 0:6af643b2bf72 134
FalconOnishi 0:6af643b2bf72 135 //enable RX1 and RX0 buffers and rollover
FalconOnishi 0:6af643b2bf72 136 can.bitModify(RXB0CTRL, RXB_RX_MASK | RXB_BUKT_MASK, RXB_RX_STDEXT | RXB_BUKT_MASK );
FalconOnishi 0:6af643b2bf72 137 can.bitModify(RXB1CTRL, RXB_RX_MASK, RXB_RX_STDEXT);
FalconOnishi 0:6af643b2bf72 138
FalconOnishi 0:6af643b2bf72 139 //can.setRegister(MCP_RXM0SIDL, 0x11);
FalconOnishi 0:6af643b2bf72 140 //can.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 0:6af643b2bf72 145 pc.printf("readRegister\r\n");
FalconOnishi 0:6af643b2bf72 146 can.readRegister(RXM0SIDH, &addrHi);
FalconOnishi 0:6af643b2bf72 147 pc.printf("readRegister\r\n");
FalconOnishi 0:6af643b2bf72 148 can.readRegister(RXM0SIDL, &addrLo);
FalconOnishi 0:6af643b2bf72 149
FalconOnishi 0:6af643b2bf72 150 pc.printf("CAN address");
FalconOnishi 0:6af643b2bf72 151 pc.printf("%02x",addrHi);
FalconOnishi 0:6af643b2bf72 152 pc.printf("%02x",addrLo);
FalconOnishi 0:6af643b2bf72 153 pc.printf("\r\n");
FalconOnishi 0:6af643b2bf72 154
FalconOnishi 0:6af643b2bf72 155 //can.setMode(LOOPBACK);
FalconOnishi 0:6af643b2bf72 156 can.setMode(NORMAL);
FalconOnishi 0:6af643b2bf72 157
FalconOnishi 0:6af643b2bf72 158 //
FalconOnishi 0:6af643b2bf72 159 mbedCan0.frequency(MBED_CAN_BUS_SPEED);
FalconOnishi 0:6af643b2bf72 160 mbedCan1.frequency(MBED_CAN_BUS_SPEED);
FalconOnishi 0:6af643b2bf72 161
FalconOnishi 0:6af643b2bf72 162 wait(1.0f);
FalconOnishi 0:6af643b2bf72 163
FalconOnishi 0:6af643b2bf72 164 timer .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 0:6af643b2bf72 199 can.load_ff_0(length,frame_id,frame_data);
FalconOnishi 0:6af643b2bf72 200 can.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 0:6af643b2bf72 214 mbedCan0.write(msg);
FalconOnishi 0:6af643b2bf72 215 else
FalconOnishi 0:6af643b2bf72 216 mbedCan1.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 0:6af643b2bf72 224 const float canBegin = timer.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 0:6af643b2bf72 230 while (timer.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 0:6af643b2bf72 240 rx_status = can.readStatus();
FalconOnishi 0:6af643b2bf72 241
FalconOnishi 0:6af643b2bf72 242 if ((rx_status & 0x01) == 0x01) {
FalconOnishi 0:6af643b2bf72 243 can.readDATA_ff_0(&length,frame_data,&frame_id);
FalconOnishi 0:6af643b2bf72 244 //pc.printf("0");
FalconOnishi 0:6af643b2bf72 245 } else if ((rx_status & 0x02) == 0x02) {
FalconOnishi 0:6af643b2bf72 246 can.readDATA_ff_1(&length,frame_data,&frame_id);
FalconOnishi 0:6af643b2bf72 247 //pc.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 0:6af643b2bf72 256 //if (frame % 120 == 0)
FalconOnishi 0:6af643b2bf72 257 // pc.printf("%d %d, ", frame_id, nodeId);
FalconOnishi 0:6af643b2bf72 258
FalconOnishi 0:6af643b2bf72 259 received[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 0:6af643b2bf72 263 //pc.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 0:6af643b2bf72 268 tmpQuat[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 0:6af643b2bf72 281 can = &mbedCan0;
FalconOnishi 0:6af643b2bf72 282 else
FalconOnishi 0:6af643b2bf72 283 can = &mbedCan1;
FalconOnishi 0:6af643b2bf72 284
FalconOnishi 0:6af643b2bf72 285 if (can->read(mbedCanMessage[canId])) {
FalconOnishi 0:6af643b2bf72 286
FalconOnishi 0:6af643b2bf72 287 CANMessage &msg = mbedCanMessage[canId];
FalconOnishi 0:6af643b2bf72 288
FalconOnishi 0:6af643b2bf72 289 nodeId = msg.id >> 3;
FalconOnishi 0:6af643b2bf72 290 uint8_t slot = msg.id & 0x01;
FalconOnishi 0:6af643b2bf72 291 received[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 0:6af643b2bf72 295 //pc.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 0:6af643b2bf72 300 tmpQuat[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 0:6af643b2bf72 310 //if (frame % 120 == 0)
FalconOnishi 0:6af643b2bf72 311 // pc.printf("\n\r");
FalconOnishi 0:6af643b2bf72 312
FalconOnishi 0:6af643b2bf72 313 if (frame % 240 == 0) {
FalconOnishi 0:6af643b2bf72 314 //pc.printf("\r\n");
FalconOnishi 0:6af643b2bf72 315 const float elapsed = timer.read()-canBegin;
FalconOnishi 0:6af643b2bf72 316 pc.printf("c%f/%d\r\n", elapsed, nCanUpdate);
FalconOnishi 0:6af643b2bf72 317 for (int i=0; i<N_JOINTS; i++)
FalconOnishi 0:6af643b2bf72 318 if (received[i][0] || received[i][1])
FalconOnishi 0:6af643b2bf72 319 pc.printf("%d:%d%d, ", i, received[i][0], received[i][1]);
FalconOnishi 0:6af643b2bf72 320 //pc.printf("%x: %f, %f, %f, %f\n\r", i, ___quat___[i][0].f, ____quat____[i][1].f, ____quat____[i][2].f, ___quat___[i][3].f);
FalconOnishi 0:6af643b2bf72 321 pc.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 0:6af643b2bf72 330 while(pc.readable()) {
FalconOnishi 0:6af643b2bf72 331 char c = pc.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 0:6af643b2bf72 355 if (received[i][0] && received[i][1]) {
FalconOnishi 0:6af643b2bf72 356 for (int j=0; j<4; j++) {
FalconOnishi 0:6af643b2bf72 357 __quat__[i][j] = tmpQuat[i][j].f;
FalconOnishi 0:6af643b2bf72 358 }
FalconOnishi 0:6af643b2bf72 359 }
FalconOnishi 0:6af643b2bf72 360 }
FalconOnishi 0:6af643b2bf72 361
FalconOnishi 0:6af643b2bf72 362 uint8_t *buf = (uint8_t *)__quat__;
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 0:6af643b2bf72 379 data.setFrameId(xbee.getNextFrameId());//0
FalconOnishi 0:6af643b2bf72 380 xbee.send(data);
FalconOnishi 0:6af643b2bf72 381
FalconOnishi 0:6af643b2bf72 382 //pc.printf("len:%d\r\n",strlen(buf));
FalconOnishi 0:6af643b2bf72 383 const int r = xbee.getWiResponse(TX_STATUS_RESPONSE, data.getFrameId());
FalconOnishi 0:6af643b2bf72 384
FalconOnishi 0:6af643b2bf72 385 /// error
FalconOnishi 0:6af643b2bf72 386 if (r==-1) {
FalconOnishi 0:6af643b2bf72 387 pc.printf("Fatal error! XBee WiFi didn't responce!\r\nTrying to reset modem again.");
FalconOnishi 0:6af643b2bf72 388 if (initXBeeWiFi(20)) {
FalconOnishi 0:6af643b2bf72 389 pc.printf("Failure...");
FalconOnishi 0:6af643b2bf72 390 error("");
FalconOnishi 0:6af643b2bf72 391 } else {
FalconOnishi 0:6af643b2bf72 392 pc.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 0:6af643b2bf72 400 const float transBegin = timer.read();
FalconOnishi 0:6af643b2bf72 401
FalconOnishi 0:6af643b2bf72 402 updateSerial();
FalconOnishi 0:6af643b2bf72 403
FalconOnishi 0:6af643b2bf72 404 updateXBeeWiFi();
FalconOnishi 0:6af643b2bf72 405
FalconOnishi 0:6af643b2bf72 406 const float elapsed = timer.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 0:6af643b2bf72 410 if (frame % 120 == 0)
FalconOnishi 0:6af643b2bf72 411 pc.printf("x%f\r\n", elapsed);
FalconOnishi 0:6af643b2bf72 412
FalconOnishi 0:6af643b2bf72 413 frame++;
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 0:6af643b2bf72 417 received[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 0:6af643b2bf72 428 pc.printf("Reset XBee Wi-Fi\r\n");
FalconOnishi 0:6af643b2bf72 429 r = xbee.reset();
FalconOnishi 0:6af643b2bf72 430 if (r < 0) {
FalconOnishi 0:6af643b2bf72 431 pc.printf("Error reset %d\r\n", r);
FalconOnishi 0:6af643b2bf72 432 return -1;
FalconOnishi 0:6af643b2bf72 433 }
FalconOnishi 0:6af643b2bf72 434
FalconOnishi 0:6af643b2bf72 435 pc.printf("Get Responce from XBee Wi-Fi\r\n");
FalconOnishi 0:6af643b2bf72 436 xbee.getWiResponse(MODEM_STATUS_RESPONSE, 5000);
FalconOnishi 0:6af643b2bf72 437
FalconOnishi 0:6af643b2bf72 438 pc.printf("Setup XBee Wi-Fi %d\r\n", r);
FalconOnishi 0:6af643b2bf72 439 r = xbee.setup(SECURITY, SSID, PASSPHRASE);
FalconOnishi 0:6af643b2bf72 440 //r = xbee.setup(SSID);
FalconOnishi 0:6af643b2bf72 441 if (r < 0) {
FalconOnishi 0:6af643b2bf72 442 pc.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 0:6af643b2bf72 448 r = xbee.getStatus();
FalconOnishi 0:6af643b2bf72 449 pc.printf("Status %02x: ", r);
FalconOnishi 0:6af643b2bf72 450 switch (r) {
FalconOnishi 0:6af643b2bf72 451 case JOINED_AP:
FalconOnishi 0:6af643b2bf72 452 pc.printf("Successfully joined an access point.\r\n");
FalconOnishi 0:6af643b2bf72 453 return 0;
FalconOnishi 0:6af643b2bf72 454 case INITIALIZATION:
FalconOnishi 0:6af643b2bf72 455 pc.printf("WiFi initialization in progress.\r\n");
FalconOnishi 0:6af643b2bf72 456 break;
FalconOnishi 0:6af643b2bf72 457 case SSID_NOT_FOUND:
FalconOnishi 0:6af643b2bf72 458 pc.printf("SSID not found.\r\n");
FalconOnishi 0:6af643b2bf72 459 return -1;
FalconOnishi 0:6af643b2bf72 460 case SSID_NOT_CONFIGURED:
FalconOnishi 0:6af643b2bf72 461 pc.printf("SSID not configured.\r\n");
FalconOnishi 0:6af643b2bf72 462 return -1;
FalconOnishi 0:6af643b2bf72 463 case JOIN_FAILED:
FalconOnishi 0:6af643b2bf72 464 pc.printf("SSID join failed.\r\n");
FalconOnishi 0:6af643b2bf72 465 return -1;
FalconOnishi 0:6af643b2bf72 466 case WAITING_IPADDRESS:
FalconOnishi 0:6af643b2bf72 467 pc.printf("Waiting for IP configuration.\r\n");
FalconOnishi 0:6af643b2bf72 468 break;
FalconOnishi 0:6af643b2bf72 469 case WAITING_SOCKETS:
FalconOnishi 0:6af643b2bf72 470 pc.printf("Listening sockets are being set up.\r\n");
FalconOnishi 0:6af643b2bf72 471 break;
FalconOnishi 0:6af643b2bf72 472 case SCANNING_SSID:
FalconOnishi 0:6af643b2bf72 473 pc.printf("Currently scanning for SSID.\r\n");
FalconOnishi 0:6af643b2bf72 474 break;
FalconOnishi 0:6af643b2bf72 475 default:
FalconOnishi 0:6af643b2bf72 476 pc.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 }