File content as of revision 0:7c0f5d94db37:

#include "mbed.h"
#include "EthernetInterface.h"
#include "CameraC328.h"
/* AX-12 */
#define AX12_REG_GOAL_POSITION 0x1E
#define AX12_REG_MOVING 0x2E
#define AX_Iniit 512
/* CAMERA */
#define USE_JPEG_HIGH_RESOLUTION    3//1=80x64 <--- not working -_-;;, 2=160x128, 3=320x240, 4=640x480
#define START                       "start"
#define END                         "end"
CameraC328 camera(PA_13, PA_14, CameraC328::Baud115200);

/* TCP/IP */
static char buf[128];
const char dest_ip[] = "192.168.0.2";
int dest_port = 1212;
uint8_t mac_addr[6] = {0x00, 0x08, 0xdc, 0x12, 0x34, 0x45};
const char ip_addr[] = "192.168.0.123"; 
const char mask_addr[] = "255.255.255.0"; 
const char gateway_addr[] = "192.168.0.1"; 
EthernetInterface eth;
TCPSocketConnection Streaming;

/* UART */
Serial pc(USBTX, USBRX);

/* AX-12 */
int HeadUD = 200;
int HeadRL = AX_Iniit;
/* Function*/ 
void jpeg_callback(char *buf, size_t siz);
void sync(void);
void test_jpeg_snapshot_picture(void);
void SetGoal(int ID, int degrees, int flags);
int write(int ID, int start, int bytes, char* data, int flag);
int read(int ID, int start, int bytes, char* data);
int isMoving(int ID);
void Forward();
void Backward();
void Right();
void Left();
void FreedmanInit();
int main() {

    printf("Hello Freedman World!\r\n"); 
      
    
    eth.init(mac_addr, ip_addr, mask_addr, gateway_addr); //Use Static
    eth.connect();

    
    FreedmanInit();
    sync();
    while (Streaming.connect(dest_ip, dest_port) < 0) {
        printf("Unable to connect to (%s) on port (%d)\r\n", dest_ip, dest_port);
        wait(1);
    }
    while(true)
    {
        int n = Streaming.receive(buf, sizeof(buf));
        buf[n] = '\0';
                
        if (!strncmp(buf, "123", 3)){
            Forward();
        }
        else if(!strncmp(buf, "220", 3)){
            Backward();
        }
        else if(!strncmp(buf, "789", 3)){
            Right();
        }
        else if(!strncmp(buf, "456", 3)){
            Left();
        }
        else if(!strncmp(buf, "110", 3)){
            //Head up
            HeadUD = HeadUD + 20;
            if(HeadUD>800){
                HeadUD = 800;
            }
            SetGoal(4, HeadUD, 1);
        }
        else if(!strncmp(buf, "550", 3)){
            //Head down
            HeadUD = HeadUD - 20;
            if(HeadUD<200){
                HeadUD = 200;
            }
            SetGoal(4, HeadUD, 1);
        }
        else if(!strncmp(buf, "330", 3)){
            //Head Right
            HeadRL = HeadRL - 20;
            if(HeadRL<200){
                HeadRL = 200;
            }
            SetGoal(16, HeadRL, 1);
        }
        else if(!strncmp(buf, "440", 3)){
            //Head Left
            HeadRL = HeadRL + 20;
            if(HeadRL>800){
                HeadRL = 800;
            }
            SetGoal(16, HeadRL, 1);
        }
        
        Streaming.send(START, sizeof(START)-1);
        test_jpeg_snapshot_picture();
        Streaming.send(END, sizeof(END)-1);
    }
}

void jpeg_callback(char *buf, size_t siz) {
    //for (int i = 0; i < (int)siz; i++) {
        //fprintf(fp_jpeg, "%c", buf[i]);
        Streaming.send(buf, siz);
    //}
}
void sync(void) {
    CameraC328::ErrorNumber err = CameraC328::NoError;

    err = camera.sync();
    if (CameraC328::NoError == err) {
        printf("[ OK ] : CameraC328::sync\r\n");
    } else {
        printf("[FAIL] : CameraC328::sync (Error=%02X)\r\n", (int)err);
    }
}
void test_jpeg_snapshot_picture() {
    CameraC328::ErrorNumber err = CameraC328::NoError;

#if (USE_JPEG_HIGH_RESOLUTION==1)
    err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution80x64);
#elif (USE_JPEG_HIGH_RESOLUTION==2)
    err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution160x128);
#elif (USE_JPEG_HIGH_RESOLUTION==3)
    err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution320x240);
#elif (USE_JPEG_HIGH_RESOLUTION==4)
    err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution640x480);
#endif
    if (CameraC328::NoError == err) {
        printf("[ OK ] : CameraC328::init\r\n");
    } else {
        printf("[FAIL] : CameraC328::init (Error=%02X)\r\n", (int)err);
    }

    err = camera.getJpegSnapshotPicture(jpeg_callback);
    if (CameraC328::NoError == err) {
        printf("[ OK ] : CameraC328::getJpegSnapshotPicture\r\n");
    } else {
        printf("[FAIL] : CameraC328::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
    }
}
void SetGoal(int ID, int degrees, int flags) {

    char reg_flag = 0;
    char data[2];

    // set the flag is only the register bit is set in the flag
    if (flags == 0x2) {
        reg_flag = 1;
    }

    // 1023 / 300 * degrees
    int goal = degrees;
    //short goal = (1023 * degrees) / 300;

    data[0] = goal & 0xff; // bottom 8 bits
    data[1] = goal >> 8;   // top 8 bits

    // write the packet, return the error code
    write(ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
    
    if (flags == 1) {
    // block until it comes to a halt
    
        while (isMoving(ID)) {}
    }
}
int write(int ID, int start, int bytes, char* data, int flag) {
// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum

    char TxBuf[16];
    char sum = 0;
    char Status[6];

#ifdef AX12_WRITE_DEBUG
    pc.printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
#endif

    // Build the TxPacket first in RAM, then we'll send in one go
#ifdef AX12_WRITE_DEBUG
    pc.printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
#endif

    TxBuf[0] = 0xff;
    TxBuf[1] = 0xff;

    // ID
    TxBuf[2] = ID;
    sum += TxBuf[2];

#ifdef AX12_WRITE_DEBUG
    pc.printf("  ID : %d\n",TxBuf[2]);
#endif

    // packet Length
    TxBuf[3] = 3+bytes;
    sum += TxBuf[3];

#ifdef AX12_WRITE_DEBUG
    pc.printf("  Length : %d\n",TxBuf[3]);
#endif

    // Instruction
    if (flag == 1) {
        TxBuf[4]=0x04;
        sum += TxBuf[4];
    } else {
        TxBuf[4]=0x03;
        sum += TxBuf[4];
    }

#ifdef AX12_WRITE_DEBUG
    pc.printf("  Instruction : 0x%x\n",TxBuf[4]);
#endif

    // Start Address
    TxBuf[5] = start;
    sum += TxBuf[5];

#ifdef AX12_WRITE_DEBUG
    pc.printf("  Start : 0x%x\n",TxBuf[5]);
#endif

    // data
    for (char i=0; i<bytes ; i++) {
        TxBuf[6+i] = data[i];
        sum += TxBuf[6+i];

#ifdef AX12_WRITE_DEBUG
        pc.printf("  Data : 0x%x\n",TxBuf[6+i]);
#endif

    }

    // checksum
    TxBuf[6+bytes] = 0xFF - sum;

#ifdef AX12_WRITE_DEBUG
    pc.printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
#endif

    // Transmit the packet in one burst with no pausing
    for (int i = 0; i < (7 + bytes) ; i++) {
        pc.putc(TxBuf[i]);
    }
        // Wait for the bytes to be transmitted
    wait (0.00002);

    // Skip if the read was to the broadcast address
    if (ID != 0xFE) {



        // response packet is always 6 + bytes
        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
        // timeout is a little more than the time to transmit
        // the packet back, i.e. (6+bytes)*10 bit periods

        int timeout = 0;
        int plen = 0;
        while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {

            if (pc.readable()) {
                Status[plen] = pc.getc();
                plen++;
                timeout = 0;
            }

            // wait for the bit period
            wait (1.0/9600);
            timeout++;
        }

        if (timeout == ((6+bytes)*10) ) {
            return(-1);
        }

        // Copy the data from Status into data for return
        for (int i=0; i < Status[3]-2 ; i++) {
            data[i] = Status[5+i];
        }

#ifdef AX12_READ_DEBUG
        printf("\nStatus Packet\n");
        printf("  Header : 0x%x\n",Status[0]);
        printf("  Header : 0x%x\n",Status[1]);
        printf("  ID : 0x%x\n",Status[2]);
        printf("  Length : 0x%x\n",Status[3]);
        printf("  Error Code : 0x%x\n",Status[4]);

        for (int i=0; i < Status[3]-2 ; i++) {
            printf("  Data : 0x%x\n",Status[5+i]);
        }

        printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
#endif

    } // if (ID!=0xFE)

    return(Status[4]);
}
int read(int ID, int start, int bytes, char* data) {

    char PacketLength = 0x4;
    char TxBuf[16];
    char sum = 0;
    char Status[16];

    Status[4] = 0xFE; // return code

#ifdef AX12_READ_DEBUG
    printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
#endif

    // Build the TxPacket first in RAM, then we'll send in one go
#ifdef AX12_READ_DEBUG
    printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
#endif

    TxBuf[0] = 0xff;
    TxBuf[1] = 0xff;

    // ID
    TxBuf[2] = ID;
    sum += TxBuf[2];

#ifdef AX12_READ_DEBUG
    printf("  ID : %d\n",TxBuf[2]);
#endif

    // Packet Length
    TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
    sum += TxBuf[3];            // Accululate the packet sum

#ifdef AX12_READ_DEBUG
    printf("  Length : 0x%x\n",TxBuf[3]);
#endif

    // Instruction - Read
    TxBuf[4] = 0x2;
    sum += TxBuf[4];

#ifdef AX12_READ_DEBUG
    printf("  Instruction : 0x%x\n",TxBuf[4]);
#endif

    // Start Address
    TxBuf[5] = start;
    sum += TxBuf[5];

#ifdef AX12_READ_DEBUG
    printf("  Start Address : 0x%x\n",TxBuf[5]);
#endif

    // Bytes to read
    TxBuf[6] = bytes;
    sum += TxBuf[6];

#ifdef AX12_READ_DEBUG
    printf("  No bytes : 0x%x\n",TxBuf[6]);
#endif

    // Checksum
    TxBuf[7] = 0xFF - sum;
#ifdef AX12_READ_DEBUG
    printf("  Checksum : 0x%x\n",TxBuf[7]);
#endif

    // Transmit the packet in one burst with no pausing
    for (int i = 0; i<8 ; i++) {
        pc.putc(TxBuf[i]);
    }

    // Wait for the bytes to be transmitted
    wait (0.00002);

    // Skip if the read was to the broadcast address
    if (ID != 0xFE) {



        // response packet is always 6 + bytes
        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
        // timeout is a little more than the time to transmit
        // the packet back, i.e. (6+bytes)*10 bit periods

        int timeout = 0;
        int plen = 0;
        while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {

            if (pc.readable()) {
                Status[plen] = pc.getc();
                plen++;
                timeout = 0;
            }

            // wait for the bit period
            wait (1.0/9600);
            timeout++;
        }

        if (timeout == ((6+bytes)*10) ) {
            return(-1);
        }

        // Copy the data from Status into data for return
        for (int i=0; i < Status[3]-2 ; i++) {
            data[i] = Status[5+i];
        }

#ifdef AX12_READ_DEBUG
        printf("\nStatus Packet\n");
        printf("  Header : 0x%x\n",Status[0]);
        printf("  Header : 0x%x\n",Status[1]);
        printf("  ID : 0x%x\n",Status[2]);
        printf("  Length : 0x%x\n",Status[3]);
        printf("  Error Code : 0x%x\n",Status[4]);

        for (int i=0; i < Status[3]-2 ; i++) {
            printf("  Data : 0x%x\n",Status[5+i]);
        }

        printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
#endif

    } // if (ID!=0xFE)

    return(Status[4]);
}
int isMoving(int ID) {

    char data[1];
    read(ID,AX12_REG_MOVING,1,data);
    return(data[0]);
}
void Forward()
{
    char i, j;
    int Forward_LegUR[8]={646, 646, 448, 377, 372, 587, 643, 512};
    int Forward_LegUL[8]={650, 650, 437, 375, 367, 590, 650, 512};
    int Forward_LegDR[8]={512, 444, 412, 512, 643, 682, 512, 512};
    int Forward_LegDL[8]={512, 359, 326, 512, 572, 607, 512, 512};
    
    for(i=0; i<8; i++){
        for(j=0; j<2; j++){
            SetGoal(15, Forward_LegUR[i], 1);
            SetGoal(5, Forward_LegUL[i], 1);
            SetGoal(8, Forward_LegDR[i], 1);
            SetGoal(3, Forward_LegDL[i], 1);
        }
        wait(0.2);
    } 
}
void Backward()
{
    char i, j;
    int Backward_LegUR[8]={641, 641, 425, 378, 385, 589, 643, 512};
    int Backward_LegUL[8]={646, 646, 422, 379, 377, 593, 648, 512};
    int Backward_LegDR[8]={512, 653, 672, 512, 433, 417, 512, 512};
    int Backward_LegDL[8]={512, 582, 605, 512, 357, 344, 512, 512};
    
    for(i=0; i<8; i++){
        for(j=0; j<2; j++){
            SetGoal(15, Backward_LegUR[i], 1);
            SetGoal(5, Backward_LegUL[i], 1);
            SetGoal(8, Backward_LegDR[i], 1);
            SetGoal(3, Backward_LegDL[i], 1);
        }
        wait(0.2);
    } 
}
void Right()
{
    char i, j;
    int Right_LegUR[6]={512, 512, 512, 512, 512, 512};
    int Right_LegUL[6]={512, 512, 404, 404, 404, 512};
    int Right_LegDR[6]={512, 684, 661, 512, 458, 512};
    int Right_LegDL[6]={512, 585, 604, 512, 398, 512};
    
    for(i=0; i<6; i++){
        for(j=0; j<2; j++){
            SetGoal(15, Right_LegUR[i], 1);
            SetGoal(5, Right_LegUL[i], 1);
            SetGoal(8, Right_LegDR[i], 1);
            SetGoal(3, Right_LegDL[i], 1);
        }
        wait(0.2);
    } 
}
void Left()
{
    char i, j;
    int Left_LegUR[6]={512, 512, 620, 620, 620, 512};
    int Left_LegUL[6]={512, 512, 512, 512, 512, 512};
    int Left_LegDR[6]={512, 441, 419, 512, 660, 512};
    int Left_LegDL[6]={512, 332, 341, 512, 575, 512};
    
    for(i=0; i<6; i++){
        for(j=0; j<2; j++){
            SetGoal(15, Left_LegUR[i], 1);
            SetGoal(5, Left_LegUL[i], 1);
            SetGoal(8, Left_LegDR[i], 1);
            SetGoal(3, Left_LegDL[i], 1);
        }
        wait(0.2);
    } 
}

void FreedmanInit()
{
    SetGoal(4, HeadUD, 1);
    SetGoal(16, AX_Iniit, 1);
    SetGoal(15, AX_Iniit, 1);
    SetGoal(5, AX_Iniit, 1);
    SetGoal(8, AX_Iniit, 1);
    SetGoal(3, AX_Iniit, 1);
}