Ricky Kwon
This is robot program. it is AX-12, CameraC328 used.
Dependencies: CameraC328 WIZnetInterface_Ricky mbed-src
Diff: main.cpp
- Revision:
- 0:7c0f5d94db37
--- /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