main.cpp

00001 #include "mbed.h"
00002 #include "EthernetInterface.h"
00003 #include "CameraC328.h"
00004 /* AX-12 */
00005 #define AX12_REG_GOAL_POSITION 0x1E
00006 #define AX12_REG_MOVING 0x2E
00007 #define AX_Iniit 512
00008 /* CAMERA */
00009 #define USE_JPEG_HIGH_RESOLUTION    3//1=80x64 <--- not working -_-;;, 2=160x128, 3=320x240, 4=640x480
00010 #define START                       "start"
00011 #define END                         "end"
00012 CameraC328 camera(PA_13, PA_14, CameraC328::Baud115200);
00013 
00014 /* TCP/IP */
00015 static char buf[128];
00016 const char dest_ip[] = "192.168.0.2";
00017 int dest_port = 1212;
00018 uint8_t mac_addr[6] = {0x00, 0x08, 0xdc, 0x12, 0x34, 0x45};
00019 const char ip_addr[] = "192.168.0.123"; 
00020 const char mask_addr[] = "255.255.255.0"; 
00021 const char gateway_addr[] = "192.168.0.1"; 
00022 EthernetInterface eth;
00023 TCPSocketConnection Streaming;
00024 
00025 /* UART */
00026 Serial pc(USBTX, USBRX);
00027 
00028 /* AX-12 */
00029 int HeadUD = 200;
00030 int HeadRL = AX_Iniit;
00031 /* Function*/ 
00032 void jpeg_callback(char *buf, size_t siz);
00033 void sync(void);
00034 void test_jpeg_snapshot_picture(void);
00035 void SetGoal(int ID, int degrees, int flags);
00036 int write(int ID, int start, int bytes, char* data, int flag);
00037 int read(int ID, int start, int bytes, char* data);
00038 int isMoving(int ID);
00039 void Forward();
00040 void Backward();
00041 void Right();
00042 void Left();
00043 void FreedmanInit();
00044 int main() {
00045 
00046     printf("Hello Freedman World!\r\n"); 
00047       
00048     
00049     eth.init(mac_addr, ip_addr, mask_addr, gateway_addr); //Use Static
00050     eth.connect();
00051 
00052     
00053     FreedmanInit();
00054     sync();
00055     while (Streaming.connect(dest_ip, dest_port) < 0) {
00056         printf("Unable to connect to (%s) on port (%d)\r\n", dest_ip, dest_port);
00057         wait(1);
00058     }
00059     while(true)
00060     {
00061         int n = Streaming.receive(buf, sizeof(buf));
00062         buf[n] = '\0';
00063                 
00064         if (!strncmp(buf, "123", 3)){
00065             Forward();
00066         }
00067         else if(!strncmp(buf, "220", 3)){
00068             Backward();
00069         }
00070         else if(!strncmp(buf, "789", 3)){
00071             Right();
00072         }
00073         else if(!strncmp(buf, "456", 3)){
00074             Left();
00075         }
00076         else if(!strncmp(buf, "110", 3)){
00077             //Head up
00078             HeadUD = HeadUD + 20;
00079             if(HeadUD>800){
00080                 HeadUD = 800;
00081             }
00082             SetGoal(4, HeadUD, 1);
00083         }
00084         else if(!strncmp(buf, "550", 3)){
00085             //Head down
00086             HeadUD = HeadUD - 20;
00087             if(HeadUD<200){
00088                 HeadUD = 200;
00089             }
00090             SetGoal(4, HeadUD, 1);
00091         }
00092         else if(!strncmp(buf, "330", 3)){
00093             //Head Right
00094             HeadRL = HeadRL - 20;
00095             if(HeadRL<200){
00096                 HeadRL = 200;
00097             }
00098             SetGoal(16, HeadRL, 1);
00099         }
00100         else if(!strncmp(buf, "440", 3)){
00101             //Head Left
00102             HeadRL = HeadRL + 20;
00103             if(HeadRL>800){
00104                 HeadRL = 800;
00105             }
00106             SetGoal(16, HeadRL, 1);
00107         }
00108         
00109         Streaming.send(START, sizeof(START)-1);
00110         test_jpeg_snapshot_picture();
00111         Streaming.send(END, sizeof(END)-1);
00112     }
00113 }
00114 
00115 void jpeg_callback(char *buf, size_t siz) {
00116     //for (int i = 0; i < (int)siz; i++) {
00117         //fprintf(fp_jpeg, "%c", buf[i]);
00118         Streaming.send(buf, siz);
00119     //}
00120 }
00121 void sync(void) {
00122     CameraC328::ErrorNumber err = CameraC328::NoError;
00123 
00124     err = camera.sync();
00125     if (CameraC328::NoError == err) {
00126         printf("[ OK ] : CameraC328::sync\r\n");
00127     } else {
00128         printf("[FAIL] : CameraC328::sync (Error=%02X)\r\n", (int)err);
00129     }
00130 }
00131 void test_jpeg_snapshot_picture() {
00132     CameraC328::ErrorNumber err = CameraC328::NoError;
00133 
00134 #if (USE_JPEG_HIGH_RESOLUTION==1)
00135     err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution80x64);
00136 #elif (USE_JPEG_HIGH_RESOLUTION==2)
00137     err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution160x128);
00138 #elif (USE_JPEG_HIGH_RESOLUTION==3)
00139     err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution320x240);
00140 #elif (USE_JPEG_HIGH_RESOLUTION==4)
00141     err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution640x480);
00142 #endif
00143     if (CameraC328::NoError == err) {
00144         printf("[ OK ] : CameraC328::init\r\n");
00145     } else {
00146         printf("[FAIL] : CameraC328::init (Error=%02X)\r\n", (int)err);
00147     }
00148 
00149     err = camera.getJpegSnapshotPicture(jpeg_callback);
00150     if (CameraC328::NoError == err) {
00151         printf("[ OK ] : CameraC328::getJpegSnapshotPicture\r\n");
00152     } else {
00153         printf("[FAIL] : CameraC328::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
00154     }
00155 }
00156 void SetGoal(int ID, int degrees, int flags) {
00157 
00158     char reg_flag = 0;
00159     char data[2];
00160 
00161     // set the flag is only the register bit is set in the flag
00162     if (flags == 0x2) {
00163         reg_flag = 1;
00164     }
00165 
00166     // 1023 / 300 * degrees
00167     int goal = degrees;
00168     //short goal = (1023 * degrees) / 300;
00169 
00170     data[0] = goal & 0xff; // bottom 8 bits
00171     data[1] = goal >> 8;   // top 8 bits
00172 
00173     // write the packet, return the error code
00174     write(ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
00175     
00176     if (flags == 1) {
00177     // block until it comes to a halt
00178     
00179         while (isMoving(ID)) {}
00180     }
00181 }
00182 int write(int ID, int start, int bytes, char* data, int flag) {
00183 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
00184 
00185     char TxBuf[16];
00186     char sum = 0;
00187     char Status[6];
00188 
00189 #ifdef AX12_WRITE_DEBUG
00190     pc.printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
00191 #endif
00192 
00193     // Build the TxPacket first in RAM, then we'll send in one go
00194 #ifdef AX12_WRITE_DEBUG
00195     pc.printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
00196 #endif
00197 
00198     TxBuf[0] = 0xff;
00199     TxBuf[1] = 0xff;
00200 
00201     // ID
00202     TxBuf[2] = ID;
00203     sum += TxBuf[2];
00204 
00205 #ifdef AX12_WRITE_DEBUG
00206     pc.printf("  ID : %d\n",TxBuf[2]);
00207 #endif
00208 
00209     // packet Length
00210     TxBuf[3] = 3+bytes;
00211     sum += TxBuf[3];
00212 
00213 #ifdef AX12_WRITE_DEBUG
00214     pc.printf("  Length : %d\n",TxBuf[3]);
00215 #endif
00216 
00217     // Instruction
00218     if (flag == 1) {
00219         TxBuf[4]=0x04;
00220         sum += TxBuf[4];
00221     } else {
00222         TxBuf[4]=0x03;
00223         sum += TxBuf[4];
00224     }
00225 
00226 #ifdef AX12_WRITE_DEBUG
00227     pc.printf("  Instruction : 0x%x\n",TxBuf[4]);
00228 #endif
00229 
00230     // Start Address
00231     TxBuf[5] = start;
00232     sum += TxBuf[5];
00233 
00234 #ifdef AX12_WRITE_DEBUG
00235     pc.printf("  Start : 0x%x\n",TxBuf[5]);
00236 #endif
00237 
00238     // data
00239     for (char i=0; i<bytes ; i++) {
00240         TxBuf[6+i] = data[i];
00241         sum += TxBuf[6+i];
00242 
00243 #ifdef AX12_WRITE_DEBUG
00244         pc.printf("  Data : 0x%x\n",TxBuf[6+i]);
00245 #endif
00246 
00247     }
00248 
00249     // checksum
00250     TxBuf[6+bytes] = 0xFF - sum;
00251 
00252 #ifdef AX12_WRITE_DEBUG
00253     pc.printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
00254 #endif
00255 
00256     // Transmit the packet in one burst with no pausing
00257     for (int i = 0; i < (7 + bytes) ; i++) {
00258         pc.putc(TxBuf[i]);
00259     }
00260         // Wait for the bytes to be transmitted
00261     wait (0.00002);
00262 
00263     // Skip if the read was to the broadcast address
00264     if (ID != 0xFE) {
00265 
00266 
00267 
00268         // response packet is always 6 + bytes
00269         // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
00270         // timeout is a little more than the time to transmit
00271         // the packet back, i.e. (6+bytes)*10 bit periods
00272 
00273         int timeout = 0;
00274         int plen = 0;
00275         while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {
00276 
00277             if (pc.readable()) {
00278                 Status[plen] = pc.getc();
00279                 plen++;
00280                 timeout = 0;
00281             }
00282 
00283             // wait for the bit period
00284             wait (1.0/9600);
00285             timeout++;
00286         }
00287 
00288         if (timeout == ((6+bytes)*10) ) {
00289             return(-1);
00290         }
00291 
00292         // Copy the data from Status into data for return
00293         for (int i=0; i < Status[3]-2 ; i++) {
00294             data[i] = Status[5+i];
00295         }
00296 
00297 #ifdef AX12_READ_DEBUG
00298         printf("\nStatus Packet\n");
00299         printf("  Header : 0x%x\n",Status[0]);
00300         printf("  Header : 0x%x\n",Status[1]);
00301         printf("  ID : 0x%x\n",Status[2]);
00302         printf("  Length : 0x%x\n",Status[3]);
00303         printf("  Error Code : 0x%x\n",Status[4]);
00304 
00305         for (int i=0; i < Status[3]-2 ; i++) {
00306             printf("  Data : 0x%x\n",Status[5+i]);
00307         }
00308 
00309         printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
00310 #endif
00311 
00312     } // if (ID!=0xFE)
00313 
00314     return(Status[4]);
00315 }
00316 int read(int ID, int start, int bytes, char* data) {
00317 
00318     char PacketLength = 0x4;
00319     char TxBuf[16];
00320     char sum = 0;
00321     char Status[16];
00322 
00323     Status[4] = 0xFE; // return code
00324 
00325 #ifdef AX12_READ_DEBUG
00326     printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
00327 #endif
00328 
00329     // Build the TxPacket first in RAM, then we'll send in one go
00330 #ifdef AX12_READ_DEBUG
00331     printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
00332 #endif
00333 
00334     TxBuf[0] = 0xff;
00335     TxBuf[1] = 0xff;
00336 
00337     // ID
00338     TxBuf[2] = ID;
00339     sum += TxBuf[2];
00340 
00341 #ifdef AX12_READ_DEBUG
00342     printf("  ID : %d\n",TxBuf[2]);
00343 #endif
00344 
00345     // Packet Length
00346     TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
00347     sum += TxBuf[3];            // Accululate the packet sum
00348 
00349 #ifdef AX12_READ_DEBUG
00350     printf("  Length : 0x%x\n",TxBuf[3]);
00351 #endif
00352 
00353     // Instruction - Read
00354     TxBuf[4] = 0x2;
00355     sum += TxBuf[4];
00356 
00357 #ifdef AX12_READ_DEBUG
00358     printf("  Instruction : 0x%x\n",TxBuf[4]);
00359 #endif
00360 
00361     // Start Address
00362     TxBuf[5] = start;
00363     sum += TxBuf[5];
00364 
00365 #ifdef AX12_READ_DEBUG
00366     printf("  Start Address : 0x%x\n",TxBuf[5]);
00367 #endif
00368 
00369     // Bytes to read
00370     TxBuf[6] = bytes;
00371     sum += TxBuf[6];
00372 
00373 #ifdef AX12_READ_DEBUG
00374     printf("  No bytes : 0x%x\n",TxBuf[6]);
00375 #endif
00376 
00377     // Checksum
00378     TxBuf[7] = 0xFF - sum;
00379 #ifdef AX12_READ_DEBUG
00380     printf("  Checksum : 0x%x\n",TxBuf[7]);
00381 #endif
00382 
00383     // Transmit the packet in one burst with no pausing
00384     for (int i = 0; i<8 ; i++) {
00385         pc.putc(TxBuf[i]);
00386     }
00387 
00388     // Wait for the bytes to be transmitted
00389     wait (0.00002);
00390 
00391     // Skip if the read was to the broadcast address
00392     if (ID != 0xFE) {
00393 
00394 
00395 
00396         // response packet is always 6 + bytes
00397         // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
00398         // timeout is a little more than the time to transmit
00399         // the packet back, i.e. (6+bytes)*10 bit periods
00400 
00401         int timeout = 0;
00402         int plen = 0;
00403         while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {
00404 
00405             if (pc.readable()) {
00406                 Status[plen] = pc.getc();
00407                 plen++;
00408                 timeout = 0;
00409             }
00410 
00411             // wait for the bit period
00412             wait (1.0/9600);
00413             timeout++;
00414         }
00415 
00416         if (timeout == ((6+bytes)*10) ) {
00417             return(-1);
00418         }
00419 
00420         // Copy the data from Status into data for return
00421         for (int i=0; i < Status[3]-2 ; i++) {
00422             data[i] = Status[5+i];
00423         }
00424 
00425 #ifdef AX12_READ_DEBUG
00426         printf("\nStatus Packet\n");
00427         printf("  Header : 0x%x\n",Status[0]);
00428         printf("  Header : 0x%x\n",Status[1]);
00429         printf("  ID : 0x%x\n",Status[2]);
00430         printf("  Length : 0x%x\n",Status[3]);
00431         printf("  Error Code : 0x%x\n",Status[4]);
00432 
00433         for (int i=0; i < Status[3]-2 ; i++) {
00434             printf("  Data : 0x%x\n",Status[5+i]);
00435         }
00436 
00437         printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
00438 #endif
00439 
00440     } // if (ID!=0xFE)
00441 
00442     return(Status[4]);
00443 }
00444 int isMoving(int ID) {
00445 
00446     char data[1];
00447     read(ID,AX12_REG_MOVING,1,data);
00448     return(data[0]);
00449 }
00450 void Forward()
00451 {
00452     char i, j;
00453     int Forward_LegUR[8]={646, 646, 448, 377, 372, 587, 643, 512};
00454     int Forward_LegUL[8]={650, 650, 437, 375, 367, 590, 650, 512};
00455     int Forward_LegDR[8]={512, 444, 412, 512, 643, 682, 512, 512};
00456     int Forward_LegDL[8]={512, 359, 326, 512, 572, 607, 512, 512};
00457     
00458     for(i=0; i<8; i++){
00459         for(j=0; j<2; j++){
00460             SetGoal(15, Forward_LegUR[i], 1);
00461             SetGoal(5, Forward_LegUL[i], 1);
00462             SetGoal(8, Forward_LegDR[i], 1);
00463             SetGoal(3, Forward_LegDL[i], 1);
00464         }
00465         wait(0.2);
00466     } 
00467 }
00468 void Backward()
00469 {
00470     char i, j;
00471     int Backward_LegUR[8]={641, 641, 425, 378, 385, 589, 643, 512};
00472     int Backward_LegUL[8]={646, 646, 422, 379, 377, 593, 648, 512};
00473     int Backward_LegDR[8]={512, 653, 672, 512, 433, 417, 512, 512};
00474     int Backward_LegDL[8]={512, 582, 605, 512, 357, 344, 512, 512};
00475     
00476     for(i=0; i<8; i++){
00477         for(j=0; j<2; j++){
00478             SetGoal(15, Backward_LegUR[i], 1);
00479             SetGoal(5, Backward_LegUL[i], 1);
00480             SetGoal(8, Backward_LegDR[i], 1);
00481             SetGoal(3, Backward_LegDL[i], 1);
00482         }
00483         wait(0.2);
00484     } 
00485 }
00486 void Right()
00487 {
00488     char i, j;
00489     int Right_LegUR[6]={512, 512, 512, 512, 512, 512};
00490     int Right_LegUL[6]={512, 512, 404, 404, 404, 512};
00491     int Right_LegDR[6]={512, 684, 661, 512, 458, 512};
00492     int Right_LegDL[6]={512, 585, 604, 512, 398, 512};
00493     
00494     for(i=0; i<6; i++){
00495         for(j=0; j<2; j++){
00496             SetGoal(15, Right_LegUR[i], 1);
00497             SetGoal(5, Right_LegUL[i], 1);
00498             SetGoal(8, Right_LegDR[i], 1);
00499             SetGoal(3, Right_LegDL[i], 1);
00500         }
00501         wait(0.2);
00502     } 
00503 }
00504 void Left()
00505 {
00506     char i, j;
00507     int Left_LegUR[6]={512, 512, 620, 620, 620, 512};
00508     int Left_LegUL[6]={512, 512, 512, 512, 512, 512};
00509     int Left_LegDR[6]={512, 441, 419, 512, 660, 512};
00510     int Left_LegDL[6]={512, 332, 341, 512, 575, 512};
00511     
00512     for(i=0; i<6; i++){
00513         for(j=0; j<2; j++){
00514             SetGoal(15, Left_LegUR[i], 1);
00515             SetGoal(5, Left_LegUL[i], 1);
00516             SetGoal(8, Left_LegDR[i], 1);
00517             SetGoal(3, Left_LegDL[i], 1);
00518         }
00519         wait(0.2);
00520     } 
00521 }
00522 
00523 void FreedmanInit()
00524 {
00525     SetGoal(4, HeadUD, 1);
00526     SetGoal(16, AX_Iniit, 1);
00527     SetGoal(15, AX_Iniit, 1);
00528     SetGoal(5, AX_Iniit, 1);
00529     SetGoal(8, AX_Iniit, 1);
00530     SetGoal(3, AX_Iniit, 1);
00531 }