This is robot program. it is AX-12, CameraC328 used.

Dependencies:   CameraC328 WIZnetInterface_Ricky mbed-src

Files at this revision

API Documentation at this revision

Comitter:
Ricky_Kwon
Date:
Fri Jul 31 03:34:57 2015 +0000
Commit message:
FREEDMAN

Changed in this revision

CameraC328.lib Show annotated file Show diff for this revision Revisions of this file
WIZnetInterface.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-src.lib Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CameraC328.lib	Fri Jul 31 03:34:57 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/shintamainjp/code/CameraC328/#49cfda6c547f
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WIZnetInterface.lib	Fri Jul 31 03:34:57 2015 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/WIZnet/code/WIZnetInterface_Ricky/#5a1969320666
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jul 31 03:34:57 2015 +0000
@@ -0,0 +1,531 @@
+#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);
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-src.lib	Fri Jul 31 03:34:57 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-src/#523654f2b367