File content as of revision 1:c2f68b0adabb:

#include "mbed.h"
#include "XBeeWiFi.h"
#include "EthernetNetIf.h"
#include "TCPSocket.h"
#include "MCP2515.h"
#include "Utility.h"

#define DEBUG (1)
#define PC_SERIAL (1)

//--------------------------------------------------------------
//settings
//#define SECURITY SECURITY_OPEN
#define SECURITY SECURITY_WPA2
#define SSID "f"
#define PASSPHRASE "1q2w3e4r5t6y7u8i9o0p"

//--------------------------------------------------------------
//transmit
#define N_JOINTS (18)
#define DATA_LEN (4*4*N_JOINTS+3*4+4) //18*quaternion*float bytes + position vector*flot bytes + delimiter

//--------------------------------------------------------------
// functions
bool initXBee(int baud);
int initXBeeWiFi(int timeout);
void setup();
void loop();
void updateCAN(float time);
void sendCanMessage(uint8_t cmd);
void sendMbedCanMessage(int id, uint8_t cmd);

void beginTransmitQuaternion();
void endTransmitQuaternion();

//--------------------------------------------------------------
// unions
union floatAndByte {
    float f;
    byte b[4];
};

//--------------------------------------------------------------
// objects
// Serial
Serial gPc(USBTX, USBRX);

//--------------------------------------------------------------
// XBee WiFi
XBeeWiFi gXBee(p13, p14, p12, P0_22); // TX, RX, CTS, RTS
//XBeeWiFi gXBee(p13, p14, p12, p11); // TX, RX, CTS, RTS

//--------------------------------------------------------------
//CAN
#define BUS_SPEED (1000)
SPI gSpi(p5, p6, p7); // mosi, miso, sclk
//MCP2515 gCan(gSpi, p15);
MCP2515 gCan(gSpi, p8);

bool gReceived[N_JOINTS][2];
float __gQuat__[N_JOINTS+1][4];
floatAndByte gTmpQuat[N_JOINTS+1][4];

//mbed CAN
#define MBED_CAN_BUS_SPEED (1000000)
#define N_MBED_CAN (2)
CAN gMbedCan0(p9, p10);
CAN gMbedCan1(p30, p29);

CANMessage gMbedCanMessage[N_MBED_CAN];

//--------------------------------------------------------------
Timer gTimer;
long gFrame = 0;

//--------------------------------------------------------------
// main()
int main()
{
    setup();
    for (;;)
        loop();
}

//--------------------------------------------------------------
bool initXBee(int baud)
{
    gPc.printf("Init XBee WiFi with %d baudrate\r\n", baud);
    gXBee.begin(baud);

    // set XBee baudrate as 921600!!(1Mbps)
    // then we cann't comunicate with XbeeWiFi using gPc
    // so we should set baudrate under 115200 when we want to comunicate with it
    gXBee.baud(921600);

    if (initXBeeWiFi(20)) {
        return false;
    } else {
        gPc.printf("succeed\r\n");
        return true;
    }
}

//--------------------------------------------------------------
void setup()
{
    gPc.baud(115200);
    gPc.printf("[ram::Motioner]\r\n");

    if (!initXBee(115200))
        if (!initXBee(921600))
            error("XBee init failure\r\n");

    IpAddr ipaddr, netmask, gateway, nameserver;
    gXBee.getAddress(ipaddr, netmask, gateway, nameserver);
    gPc.printf("IP address %d.%d.%d.%d\r\n", ipaddr[0], ipaddr[1], ipaddr[2], ipaddr[3]);
    if (ipaddr == 0) {
        gPc.printf("not configure\r\n");
        error("");
    }

    // CAN
    //gSpi.format( 8, 1 ); //Arduino's SPI Mode = 1
    gSpi.frequency( 8000000 ); //8MHz

    gPc.printf("CAN reset\r\n");
    gCan.reset();
    gPc.printf("CAN baudConfig\r\n");
    gCan.baudConfig(BUS_SPEED);
    //gCan.configRate(CAN_500KBPS_8MHZ);
    gPc.printf("CAN setMask\r\n");
    //gCan.setMask(MASK_SID_CPL_MATCH);//test no mask
    //gCan.setMask(MASK_SID_ALL_HIT);//test no mask

    //enable RX1 and RX0 buffers and rollover
    gCan.bitModify(RXB0CTRL, RXB_RX_MASK | RXB_BUKT_MASK, RXB_RX_STDEXT | RXB_BUKT_MASK );
    gCan.bitModify(RXB1CTRL, RXB_RX_MASK, RXB_RX_STDEXT);

    //gCan.setRegister(MCP_RXM0SIDL, 0x11);
    //gCan.setRegister(MCP_RXM0SIDH, 0x32);

    byte addrHi;
    byte addrLo;

    gPc.printf("readRegister\r\n");
    gCan.readRegister(RXM0SIDH, &addrHi);
    gPc.printf("readRegister\r\n");
    gCan.readRegister(RXM0SIDL, &addrLo);

    gPc.printf("CAN address");
    gPc.printf("%02x",addrHi);
    gPc.printf("%02x",addrLo);
    gPc.printf("\r\n");

    //gCan.setMode(LOOPBACK);
    gCan.setMode(NORMAL);

    //
    gMbedCan0.frequency(MBED_CAN_BUS_SPEED);
    gMbedCan1.frequency(MBED_CAN_BUS_SPEED);

    wait(1.0f);

    gTimer .start();
    
    //freezeTimer.start();
    //freezeChecker.attach(&freezeCheck, 0.3f);
}

//--------------------------------------------------------------
//--------------------------------------------------------------
void beginTransmitQuaternion()
{
    sendCanMessage('b');
    for (int i=0; i<N_MBED_CAN; i++)
        sendMbedCanMessage(i, 'b');
}

//--------------------------------------------------------------
void endTransmitQuaternion()
{
    sendCanMessage('e');
    for (int i=0; i<N_MBED_CAN; i++)
        sendMbedCanMessage(i, 'e');
}

//--------------------------------------------------------------
void sendCanMessage(uint8_t cmd)
{
    uint8_t length = 3;
    uint8_t frame_data[length];

    frame_data[0] = 'm';
    frame_data[1] = 't';
    frame_data[2] = cmd;

    unsigned short frame_id = 0x0040;

    gCan.load_ff_0(length,frame_id,frame_data);
    gCan.send_0();
}

//--------------------------------------------------------------
void sendMbedCanMessage(int id, uint8_t cmd)
{
    CANMessage msg;
    msg.len = 3;
    msg.data[0] = 'm';
    msg.data[1] = 't';
    msg.data[2] = cmd;
    msg.id = 0x0040;

    if (id==0)
        gMbedCan0.write(msg);
    else
        gMbedCan1.write(msg);
}

//--------------------------------------------------------------

//--------------------------------------------------------------
void updateCAN(float time)
{
    const float canBegin = gTimer.read();
    int nCanUpdate = 0;

    beginTransmitQuaternion();

    // loop for update can
    while (gTimer.read()-canBegin < time) {
        //CAN
        byte nodeId = 0x00;
        byte length = 0x00, rx_status = 0x00;
        unsigned short frame_id = 0x0000;
        byte frame_data[8];

        for (int i=0; i<8; i++)
            frame_data[i] = 0x00;

        rx_status = gCan.readStatus();

        if ((rx_status & 0x01) == 0x01) {
            gCan.readDATA_ff_0(&length,frame_data,&frame_id);
            //gPc.printf("0");
        } else if ((rx_status & 0x02) == 0x02) {
            gCan.readDATA_ff_1(&length,frame_data,&frame_id);
            //gPc.printf("1");
        }

        if (((rx_status & 0x01) == 0x01) || ((rx_status & 0x02) == 0x02)) {

            // 11bit address
            nodeId = frame_id >> 3;
            uint8_t slot = frame_id & 0x01;

            //if (gFrame % 120 == 0)
            //    gPc.printf("%d %d, ", frame_id, nodeId);

            gReceived[nodeId][slot] = true;

            // 0to18 Joints and RX buffer*2(quaternion xy or zw) and 8 byte CAN data
            if (nodeId < N_JOINTS && slot < 2 && length == 8) {
                //gPc.printf("%x, %x\n\r", nodeId, slot);

                for (int q=0; q<2; q++) { //quaternion x, y or z, w
                    for (int f=0; f<4; f++) { //float byte 0 - 4
                        //[node id][quaternion x, y or z, w][float bytes 0 - 4]
                        gTmpQuat[nodeId][q+slot*2].b[f] = frame_data[f+q*4];
                    }
                }
            }
        }

        //mbed CAN
        for (int canId=0; canId<N_MBED_CAN; canId++) {

            CAN *can = NULL;
            byte nodeId = 0x00;

            if (canId == 0)
                can = &gMbedCan0;
            else
                can = &gMbedCan1;

            if (can->read(gMbedCanMessage[canId])) {

                CANMessage &msg = gMbedCanMessage[canId];

                nodeId = msg.id >> 3;
                uint8_t slot = msg.id & 0x01;
                gReceived[nodeId][slot] = true;

                // 0to18 Joints and RX buffer*2(quaternion xy or zw) and 8 byte CAN data
                if (nodeId < N_JOINTS && slot < 2 && msg.len == 8) {
                    //gPc.printf("%x, %x\n\r", nodeId, slot);

                    for (int q=0; q<2; q++) { //quaternion x, y or z, w
                        for (int f=0; f<4; f++) { //float byte 0 - 4
                            //[node id][quaternion x, y or z, w][float bytes 0 - 4]
                            gTmpQuat[nodeId][q+slot*2].b[f] = msg.data[f+q*4];
                        }
                    }
                }
            }
        }

        nCanUpdate++; //how many times could we update can?
    }// loop for update can

    //if (gFrame % 120 == 0)
    //    gPc.printf("\n\r");

    if (gFrame % 240 == 0) {
        //gPc.printf("\r\n");
        const float elapsed = gTimer.read()-canBegin;
        gPc.printf("c%f/%d\r\n", elapsed, nCanUpdate);
        for (int i=0; i<N_JOINTS; i++)
            if (gReceived[i][0] || gReceived[i][1])
                gPc.printf("%d:%d%d, ", i, gReceived[i][0], gReceived[i][1]);
        //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);
        gPc.printf("\r\n");
    }

    endTransmitQuaternion();

}
//--------------------------------------------------------------
void updateSerial()
{
    while(gPc.readable()) {
        char c = gPc.getc();
        switch (c) {

        };
    }
}

//--------------------------------------------------------------
void updateXBeeWiFi()
{
    IPv4TransmitRequest data;

    //XBee WiFi Transmit
    //IPv4TransmitRequest data;
    //int len = 2*4*18+2*3; //18*quaternion+3*vector q // short
    //int len = 4*4*18+4*3; //18*quaternion+3*vector // float
    //int len = 8; //18*quaternion+3*vector // float
    //char buf[len];

    // debug
    //for (int i=0; i<len; i++)
    //buf[i] = (char)('0'+i%70);

    for (int i=0; i<N_JOINTS; i++) {
        if (gReceived[i][0] && gReceived[i][1]) {
            for (int j=0; j<4; j++) {
                __gQuat__[i][j] = gTmpQuat[i][j].f;
            }
        }
    }

    uint8_t *buf = (uint8_t *)__gQuat__;

    buf[DATA_LEN-4] = (char)'a';
    buf[DATA_LEN-3] = (char)'b';
    buf[DATA_LEN-2] = (char)'c';
    buf[DATA_LEN-1] = (char)'d';


    IpAddr addr(192, 168, 2, 1);
    data.setAddress(addr);
    data.setDstPort(9750);
    data.setSrcPort(9750);
    data.setProtocol(PROTOCOL_UDP);
    //data.setPayload((uint8_t*)buf);
    //data.setPayloadLength(strlen(buf));
    data.setPayload(buf);
    data.setPayloadLength(DATA_LEN);
    data.setFrameId(gXBee.getNextFrameId());//0
    gXBee.send(data);

    //gPc.printf("len:%d\r\n",strlen(buf));
    const int r = gXBee.getWiResponse(TX_STATUS_RESPONSE, data.getFrameId());

    /// error
    if (r==-1) {
        gPc.printf("Fatal error! XBee WiFi didn't responce!\r\nTrying to reset modem again.");
        if (initXBeeWiFi(20)) {
            gPc.printf("Failure...");
            error("");
        } else {
            gPc.printf("succeed\r\n");
        }
    }
}

//--------------------------------------------------------------
void loop()
{
    const float transBegin = gTimer.read();

    updateSerial();

    updateXBeeWiFi();

    const float elapsed = gTimer.read() - transBegin;
    const float frameTime = 1.0f/60.0f;
    const float t = frameTime-elapsed;

    if (gFrame % 120 == 0)
        gPc.printf("x%f\r\n", elapsed);

    gFrame++;

    for (int i=0; i<N_JOINTS; i++)
        for (int j=0; j<2; j++)
            gReceived[i][j] = false;

    updateCAN(t);
}

//--------------------------------------------------------------
//--------------------------------------------------------------
int initXBeeWiFi(int timeout)
{
    int i, r;

    gPc.printf("Reset XBee Wi-Fi\r\n");
    r = gXBee.reset();
    if (r < 0) {
        gPc.printf("Error reset %d\r\n", r);
        return -1;
    }

    gPc.printf("Get Responce from XBee Wi-Fi\r\n");
    gXBee.getWiResponse(MODEM_STATUS_RESPONSE, 5000);

    gPc.printf("Setup XBee Wi-Fi %d\r\n", r);
    r = gXBee.setup(SECURITY, SSID, PASSPHRASE);
    //r = gXBee.setup(SSID);
    if (r < 0) {
        gPc.printf("Error setup %d\r\n", r);
        return -1;
    }

    for (i = 0; i < timeout; i ++) {
        wait(1.0f);
        r = gXBee.getStatus();
        gPc.printf("Status %02x: ", r);
        switch (r) {
            case JOINED_AP:
                gPc.printf("Successfully joined an access point.\r\n");
                return 0;
            case INITIALIZATION:
                gPc.printf("WiFi initialization in progress.\r\n");
                break;
            case SSID_NOT_FOUND:
                gPc.printf("SSID not found.\r\n");
                return -1;
            case SSID_NOT_CONFIGURED:
                gPc.printf("SSID not configured.\r\n");
                return -1;
            case JOIN_FAILED:
                gPc.printf("SSID join failed.\r\n");
                return -1;
            case WAITING_IPADDRESS:
                gPc.printf("Waiting for IP configuration.\r\n");
                break;
            case WAITING_SOCKETS:
                gPc.printf("Listening sockets are being set up.\r\n");
                break;
            case SCANNING_SSID:
                gPc.printf("Currently scanning for SSID.\r\n");
                break;
            default:
                gPc.printf("\r\n");
                break;
        }
    }
    return -1;
}