Ricky Kwon / Freedman

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

Dependencies:   CameraC328 WIZnetInterface_Ricky mbed-src

main.cpp@0:7c0f5d94db37, 2015-07-31 (annotated)

Committer:
Ricky_Kwon
Date:
Fri Jul 31 03:34:57 2015 +0000
Revision:
0:7c0f5d94db37
FREEDMAN

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ricky_Kwon 0:7c0f5d94db37 1 #include "mbed.h"
Ricky_Kwon 0:7c0f5d94db37 2 #include "EthernetInterface.h"
Ricky_Kwon 0:7c0f5d94db37 3 #include "CameraC328.h"
Ricky_Kwon 0:7c0f5d94db37 4 /* AX-12 */
Ricky_Kwon 0:7c0f5d94db37 5 #define AX12_REG_GOAL_POSITION 0x1E
Ricky_Kwon 0:7c0f5d94db37 6 #define AX12_REG_MOVING 0x2E
Ricky_Kwon 0:7c0f5d94db37 7 #define AX_Iniit 512
Ricky_Kwon 0:7c0f5d94db37 8 /* CAMERA */
Ricky_Kwon 0:7c0f5d94db37 9 #define USE_JPEG_HIGH_RESOLUTION 3//1=80x64 <--- not working -_-;;, 2=160x128, 3=320x240, 4=640x480
Ricky_Kwon 0:7c0f5d94db37 10 #define START "start"
Ricky_Kwon 0:7c0f5d94db37 11 #define END "end"
Ricky_Kwon 0:7c0f5d94db37 12 CameraC328 camera(PA_13, PA_14, CameraC328::Baud115200);
Ricky_Kwon 0:7c0f5d94db37 13
Ricky_Kwon 0:7c0f5d94db37 14 /* TCP/IP */
Ricky_Kwon 0:7c0f5d94db37 15 static char buf[128];
Ricky_Kwon 0:7c0f5d94db37 16 const char dest_ip[] = "192.168.0.2";
Ricky_Kwon 0:7c0f5d94db37 17 int dest_port = 1212;
Ricky_Kwon 0:7c0f5d94db37 18 uint8_t mac_addr[6] = {0x00, 0x08, 0xdc, 0x12, 0x34, 0x45};
Ricky_Kwon 0:7c0f5d94db37 19 const char ip_addr[] = "192.168.0.123";
Ricky_Kwon 0:7c0f5d94db37 20 const char mask_addr[] = "255.255.255.0";
Ricky_Kwon 0:7c0f5d94db37 21 const char gateway_addr[] = "192.168.0.1";
Ricky_Kwon 0:7c0f5d94db37 22 EthernetInterface eth;
Ricky_Kwon 0:7c0f5d94db37 23 TCPSocketConnection Streaming;
Ricky_Kwon 0:7c0f5d94db37 24
Ricky_Kwon 0:7c0f5d94db37 25 /* UART */
Ricky_Kwon 0:7c0f5d94db37 26 Serial pc(USBTX, USBRX);
Ricky_Kwon 0:7c0f5d94db37 27
Ricky_Kwon 0:7c0f5d94db37 28 /* AX-12 */
Ricky_Kwon 0:7c0f5d94db37 29 int HeadUD = 200;
Ricky_Kwon 0:7c0f5d94db37 30 int HeadRL = AX_Iniit;
Ricky_Kwon 0:7c0f5d94db37 31 /* Function*/
Ricky_Kwon 0:7c0f5d94db37 32 void jpeg_callback(char *buf, size_t siz);
Ricky_Kwon 0:7c0f5d94db37 33 void sync(void);
Ricky_Kwon 0:7c0f5d94db37 34 void test_jpeg_snapshot_picture(void);
Ricky_Kwon 0:7c0f5d94db37 35 void SetGoal(int ID, int degrees, int flags);
Ricky_Kwon 0:7c0f5d94db37 36 int write(int ID, int start, int bytes, char* data, int flag);
Ricky_Kwon 0:7c0f5d94db37 37 int read(int ID, int start, int bytes, char* data);
Ricky_Kwon 0:7c0f5d94db37 38 int isMoving(int ID);
Ricky_Kwon 0:7c0f5d94db37 39 void Forward();
Ricky_Kwon 0:7c0f5d94db37 40 void Backward();
Ricky_Kwon 0:7c0f5d94db37 41 void Right();
Ricky_Kwon 0:7c0f5d94db37 42 void Left();
Ricky_Kwon 0:7c0f5d94db37 43 void FreedmanInit();
Ricky_Kwon 0:7c0f5d94db37 44 int main() {
Ricky_Kwon 0:7c0f5d94db37 45
Ricky_Kwon 0:7c0f5d94db37 46 printf("Hello Freedman World!\r\n");
Ricky_Kwon 0:7c0f5d94db37 47
Ricky_Kwon 0:7c0f5d94db37 48
Ricky_Kwon 0:7c0f5d94db37 49 eth.init(mac_addr, ip_addr, mask_addr, gateway_addr); //Use Static
Ricky_Kwon 0:7c0f5d94db37 50 eth.connect();
Ricky_Kwon 0:7c0f5d94db37 51
Ricky_Kwon 0:7c0f5d94db37 52
Ricky_Kwon 0:7c0f5d94db37 53 FreedmanInit();
Ricky_Kwon 0:7c0f5d94db37 54 sync();
Ricky_Kwon 0:7c0f5d94db37 55 while (Streaming.connect(dest_ip, dest_port) < 0) {
Ricky_Kwon 0:7c0f5d94db37 56 printf("Unable to connect to (%s) on port (%d)\r\n", dest_ip, dest_port);
Ricky_Kwon 0:7c0f5d94db37 57 wait(1);
Ricky_Kwon 0:7c0f5d94db37 58 }
Ricky_Kwon 0:7c0f5d94db37 59 while(true)
Ricky_Kwon 0:7c0f5d94db37 60 {
Ricky_Kwon 0:7c0f5d94db37 61 int n = Streaming.receive(buf, sizeof(buf));
Ricky_Kwon 0:7c0f5d94db37 62 buf[n] = '\0';
Ricky_Kwon 0:7c0f5d94db37 63
Ricky_Kwon 0:7c0f5d94db37 64 if (!strncmp(buf, "123", 3)){
Ricky_Kwon 0:7c0f5d94db37 65 Forward();
Ricky_Kwon 0:7c0f5d94db37 66 }
Ricky_Kwon 0:7c0f5d94db37 67 else if(!strncmp(buf, "220", 3)){
Ricky_Kwon 0:7c0f5d94db37 68 Backward();
Ricky_Kwon 0:7c0f5d94db37 69 }
Ricky_Kwon 0:7c0f5d94db37 70 else if(!strncmp(buf, "789", 3)){
Ricky_Kwon 0:7c0f5d94db37 71 Right();
Ricky_Kwon 0:7c0f5d94db37 72 }
Ricky_Kwon 0:7c0f5d94db37 73 else if(!strncmp(buf, "456", 3)){
Ricky_Kwon 0:7c0f5d94db37 74 Left();
Ricky_Kwon 0:7c0f5d94db37 75 }
Ricky_Kwon 0:7c0f5d94db37 76 else if(!strncmp(buf, "110", 3)){
Ricky_Kwon 0:7c0f5d94db37 77 //Head up
Ricky_Kwon 0:7c0f5d94db37 78 HeadUD = HeadUD + 20;
Ricky_Kwon 0:7c0f5d94db37 79 if(HeadUD>800){
Ricky_Kwon 0:7c0f5d94db37 80 HeadUD = 800;
Ricky_Kwon 0:7c0f5d94db37 81 }
Ricky_Kwon 0:7c0f5d94db37 82 SetGoal(4, HeadUD, 1);
Ricky_Kwon 0:7c0f5d94db37 83 }
Ricky_Kwon 0:7c0f5d94db37 84 else if(!strncmp(buf, "550", 3)){
Ricky_Kwon 0:7c0f5d94db37 85 //Head down
Ricky_Kwon 0:7c0f5d94db37 86 HeadUD = HeadUD - 20;
Ricky_Kwon 0:7c0f5d94db37 87 if(HeadUD<200){
Ricky_Kwon 0:7c0f5d94db37 88 HeadUD = 200;
Ricky_Kwon 0:7c0f5d94db37 89 }
Ricky_Kwon 0:7c0f5d94db37 90 SetGoal(4, HeadUD, 1);
Ricky_Kwon 0:7c0f5d94db37 91 }
Ricky_Kwon 0:7c0f5d94db37 92 else if(!strncmp(buf, "330", 3)){
Ricky_Kwon 0:7c0f5d94db37 93 //Head Right
Ricky_Kwon 0:7c0f5d94db37 94 HeadRL = HeadRL - 20;
Ricky_Kwon 0:7c0f5d94db37 95 if(HeadRL<200){
Ricky_Kwon 0:7c0f5d94db37 96 HeadRL = 200;
Ricky_Kwon 0:7c0f5d94db37 97 }
Ricky_Kwon 0:7c0f5d94db37 98 SetGoal(16, HeadRL, 1);
Ricky_Kwon 0:7c0f5d94db37 99 }
Ricky_Kwon 0:7c0f5d94db37 100 else if(!strncmp(buf, "440", 3)){
Ricky_Kwon 0:7c0f5d94db37 101 //Head Left
Ricky_Kwon 0:7c0f5d94db37 102 HeadRL = HeadRL + 20;
Ricky_Kwon 0:7c0f5d94db37 103 if(HeadRL>800){
Ricky_Kwon 0:7c0f5d94db37 104 HeadRL = 800;
Ricky_Kwon 0:7c0f5d94db37 105 }
Ricky_Kwon 0:7c0f5d94db37 106 SetGoal(16, HeadRL, 1);
Ricky_Kwon 0:7c0f5d94db37 107 }
Ricky_Kwon 0:7c0f5d94db37 108
Ricky_Kwon 0:7c0f5d94db37 109 Streaming.send(START, sizeof(START)-1);
Ricky_Kwon 0:7c0f5d94db37 110 test_jpeg_snapshot_picture();
Ricky_Kwon 0:7c0f5d94db37 111 Streaming.send(END, sizeof(END)-1);
Ricky_Kwon 0:7c0f5d94db37 112 }
Ricky_Kwon 0:7c0f5d94db37 113 }
Ricky_Kwon 0:7c0f5d94db37 114
Ricky_Kwon 0:7c0f5d94db37 115 void jpeg_callback(char *buf, size_t siz) {
Ricky_Kwon 0:7c0f5d94db37 116 //for (int i = 0; i < (int)siz; i++) {
Ricky_Kwon 0:7c0f5d94db37 117 //fprintf(fp_jpeg, "%c", buf[i]);
Ricky_Kwon 0:7c0f5d94db37 118 Streaming.send(buf, siz);
Ricky_Kwon 0:7c0f5d94db37 119 //}
Ricky_Kwon 0:7c0f5d94db37 120 }
Ricky_Kwon 0:7c0f5d94db37 121 void sync(void) {
Ricky_Kwon 0:7c0f5d94db37 122 CameraC328::ErrorNumber err = CameraC328::NoError;
Ricky_Kwon 0:7c0f5d94db37 123
Ricky_Kwon 0:7c0f5d94db37 124 err = camera.sync();
Ricky_Kwon 0:7c0f5d94db37 125 if (CameraC328::NoError == err) {
Ricky_Kwon 0:7c0f5d94db37 126 printf("[ OK ] : CameraC328::sync\r\n");
Ricky_Kwon 0:7c0f5d94db37 127 } else {
Ricky_Kwon 0:7c0f5d94db37 128 printf("[FAIL] : CameraC328::sync (Error=%02X)\r\n", (int)err);
Ricky_Kwon 0:7c0f5d94db37 129 }
Ricky_Kwon 0:7c0f5d94db37 130 }
Ricky_Kwon 0:7c0f5d94db37 131 void test_jpeg_snapshot_picture() {
Ricky_Kwon 0:7c0f5d94db37 132 CameraC328::ErrorNumber err = CameraC328::NoError;
Ricky_Kwon 0:7c0f5d94db37 133
Ricky_Kwon 0:7c0f5d94db37 134 #if (USE_JPEG_HIGH_RESOLUTION==1)
Ricky_Kwon 0:7c0f5d94db37 135 err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution80x64);
Ricky_Kwon 0:7c0f5d94db37 136 #elif (USE_JPEG_HIGH_RESOLUTION==2)
Ricky_Kwon 0:7c0f5d94db37 137 err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution160x128);
Ricky_Kwon 0:7c0f5d94db37 138 #elif (USE_JPEG_HIGH_RESOLUTION==3)
Ricky_Kwon 0:7c0f5d94db37 139 err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution320x240);
Ricky_Kwon 0:7c0f5d94db37 140 #elif (USE_JPEG_HIGH_RESOLUTION==4)
Ricky_Kwon 0:7c0f5d94db37 141 err = camera.init(CameraC328::Jpeg, CameraC328::RawResolution80x60, CameraC328::JpegResolution640x480);
Ricky_Kwon 0:7c0f5d94db37 142 #endif
Ricky_Kwon 0:7c0f5d94db37 143 if (CameraC328::NoError == err) {
Ricky_Kwon 0:7c0f5d94db37 144 printf("[ OK ] : CameraC328::init\r\n");
Ricky_Kwon 0:7c0f5d94db37 145 } else {
Ricky_Kwon 0:7c0f5d94db37 146 printf("[FAIL] : CameraC328::init (Error=%02X)\r\n", (int)err);
Ricky_Kwon 0:7c0f5d94db37 147 }
Ricky_Kwon 0:7c0f5d94db37 148
Ricky_Kwon 0:7c0f5d94db37 149 err = camera.getJpegSnapshotPicture(jpeg_callback);
Ricky_Kwon 0:7c0f5d94db37 150 if (CameraC328::NoError == err) {
Ricky_Kwon 0:7c0f5d94db37 151 printf("[ OK ] : CameraC328::getJpegSnapshotPicture\r\n");
Ricky_Kwon 0:7c0f5d94db37 152 } else {
Ricky_Kwon 0:7c0f5d94db37 153 printf("[FAIL] : CameraC328::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
Ricky_Kwon 0:7c0f5d94db37 154 }
Ricky_Kwon 0:7c0f5d94db37 155 }
Ricky_Kwon 0:7c0f5d94db37 156 void SetGoal(int ID, int degrees, int flags) {
Ricky_Kwon 0:7c0f5d94db37 157
Ricky_Kwon 0:7c0f5d94db37 158 char reg_flag = 0;
Ricky_Kwon 0:7c0f5d94db37 159 char data[2];
Ricky_Kwon 0:7c0f5d94db37 160
Ricky_Kwon 0:7c0f5d94db37 161 // set the flag is only the register bit is set in the flag
Ricky_Kwon 0:7c0f5d94db37 162 if (flags == 0x2) {
Ricky_Kwon 0:7c0f5d94db37 163 reg_flag = 1;
Ricky_Kwon 0:7c0f5d94db37 164 }
Ricky_Kwon 0:7c0f5d94db37 165
Ricky_Kwon 0:7c0f5d94db37 166 // 1023 / 300 * degrees
Ricky_Kwon 0:7c0f5d94db37 167 int goal = degrees;
Ricky_Kwon 0:7c0f5d94db37 168 //short goal = (1023 * degrees) / 300;
Ricky_Kwon 0:7c0f5d94db37 169
Ricky_Kwon 0:7c0f5d94db37 170 data[0] = goal & 0xff; // bottom 8 bits
Ricky_Kwon 0:7c0f5d94db37 171 data[1] = goal >> 8; // top 8 bits
Ricky_Kwon 0:7c0f5d94db37 172
Ricky_Kwon 0:7c0f5d94db37 173 // write the packet, return the error code
Ricky_Kwon 0:7c0f5d94db37 174 write(ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
Ricky_Kwon 0:7c0f5d94db37 175
Ricky_Kwon 0:7c0f5d94db37 176 if (flags == 1) {
Ricky_Kwon 0:7c0f5d94db37 177 // block until it comes to a halt
Ricky_Kwon 0:7c0f5d94db37 178
Ricky_Kwon 0:7c0f5d94db37 179 while (isMoving(ID)) {}
Ricky_Kwon 0:7c0f5d94db37 180 }
Ricky_Kwon 0:7c0f5d94db37 181 }
Ricky_Kwon 0:7c0f5d94db37 182 int write(int ID, int start, int bytes, char* data, int flag) {
Ricky_Kwon 0:7c0f5d94db37 183 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
Ricky_Kwon 0:7c0f5d94db37 184
Ricky_Kwon 0:7c0f5d94db37 185 char TxBuf[16];
Ricky_Kwon 0:7c0f5d94db37 186 char sum = 0;
Ricky_Kwon 0:7c0f5d94db37 187 char Status[6];
Ricky_Kwon 0:7c0f5d94db37 188
Ricky_Kwon 0:7c0f5d94db37 189 #ifdef AX12_WRITE_DEBUG
Ricky_Kwon 0:7c0f5d94db37 190 pc.printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
Ricky_Kwon 0:7c0f5d94db37 191 #endif
Ricky_Kwon 0:7c0f5d94db37 192
Ricky_Kwon 0:7c0f5d94db37 193 // Build the TxPacket first in RAM, then we'll send in one go
Ricky_Kwon 0:7c0f5d94db37 194 #ifdef AX12_WRITE_DEBUG
Ricky_Kwon 0:7c0f5d94db37 195 pc.printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
Ricky_Kwon 0:7c0f5d94db37 196 #endif
Ricky_Kwon 0:7c0f5d94db37 197
Ricky_Kwon 0:7c0f5d94db37 198 TxBuf[0] = 0xff;
Ricky_Kwon 0:7c0f5d94db37 199 TxBuf[1] = 0xff;
Ricky_Kwon 0:7c0f5d94db37 200
Ricky_Kwon 0:7c0f5d94db37 201 // ID
Ricky_Kwon 0:7c0f5d94db37 202 TxBuf[2] = ID;
Ricky_Kwon 0:7c0f5d94db37 203 sum += TxBuf[2];
Ricky_Kwon 0:7c0f5d94db37 204
Ricky_Kwon 0:7c0f5d94db37 205 #ifdef AX12_WRITE_DEBUG
Ricky_Kwon 0:7c0f5d94db37 206 pc.printf(" ID : %d\n",TxBuf[2]);
Ricky_Kwon 0:7c0f5d94db37 207 #endif
Ricky_Kwon 0:7c0f5d94db37 208
Ricky_Kwon 0:7c0f5d94db37 209 // packet Length
Ricky_Kwon 0:7c0f5d94db37 210 TxBuf[3] = 3+bytes;
Ricky_Kwon 0:7c0f5d94db37 211 sum += TxBuf[3];
Ricky_Kwon 0:7c0f5d94db37 212
Ricky_Kwon 0:7c0f5d94db37 213 #ifdef AX12_WRITE_DEBUG
Ricky_Kwon 0:7c0f5d94db37 214 pc.printf(" Length : %d\n",TxBuf[3]);
Ricky_Kwon 0:7c0f5d94db37 215 #endif
Ricky_Kwon 0:7c0f5d94db37 216
Ricky_Kwon 0:7c0f5d94db37 217 // Instruction
Ricky_Kwon 0:7c0f5d94db37 218 if (flag == 1) {
Ricky_Kwon 0:7c0f5d94db37 219 TxBuf[4]=0x04;
Ricky_Kwon 0:7c0f5d94db37 220 sum += TxBuf[4];
Ricky_Kwon 0:7c0f5d94db37 221 } else {
Ricky_Kwon 0:7c0f5d94db37 222 TxBuf[4]=0x03;
Ricky_Kwon 0:7c0f5d94db37 223 sum += TxBuf[4];
Ricky_Kwon 0:7c0f5d94db37 224 }
Ricky_Kwon 0:7c0f5d94db37 225
Ricky_Kwon 0:7c0f5d94db37 226 #ifdef AX12_WRITE_DEBUG
Ricky_Kwon 0:7c0f5d94db37 227 pc.printf(" Instruction : 0x%x\n",TxBuf[4]);
Ricky_Kwon 0:7c0f5d94db37 228 #endif
Ricky_Kwon 0:7c0f5d94db37 229
Ricky_Kwon 0:7c0f5d94db37 230 // Start Address
Ricky_Kwon 0:7c0f5d94db37 231 TxBuf[5] = start;
Ricky_Kwon 0:7c0f5d94db37 232 sum += TxBuf[5];
Ricky_Kwon 0:7c0f5d94db37 233
Ricky_Kwon 0:7c0f5d94db37 234 #ifdef AX12_WRITE_DEBUG
Ricky_Kwon 0:7c0f5d94db37 235 pc.printf(" Start : 0x%x\n",TxBuf[5]);
Ricky_Kwon 0:7c0f5d94db37 236 #endif
Ricky_Kwon 0:7c0f5d94db37 237
Ricky_Kwon 0:7c0f5d94db37 238 // data
Ricky_Kwon 0:7c0f5d94db37 239 for (char i=0; i<bytes ; i++) {
Ricky_Kwon 0:7c0f5d94db37 240 TxBuf[6+i] = data[i];
Ricky_Kwon 0:7c0f5d94db37 241 sum += TxBuf[6+i];
Ricky_Kwon 0:7c0f5d94db37 242
Ricky_Kwon 0:7c0f5d94db37 243 #ifdef AX12_WRITE_DEBUG
Ricky_Kwon 0:7c0f5d94db37 244 pc.printf(" Data : 0x%x\n",TxBuf[6+i]);
Ricky_Kwon 0:7c0f5d94db37 245 #endif
Ricky_Kwon 0:7c0f5d94db37 246
Ricky_Kwon 0:7c0f5d94db37 247 }
Ricky_Kwon 0:7c0f5d94db37 248
Ricky_Kwon 0:7c0f5d94db37 249 // checksum
Ricky_Kwon 0:7c0f5d94db37 250 TxBuf[6+bytes] = 0xFF - sum;
Ricky_Kwon 0:7c0f5d94db37 251
Ricky_Kwon 0:7c0f5d94db37 252 #ifdef AX12_WRITE_DEBUG
Ricky_Kwon 0:7c0f5d94db37 253 pc.printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
Ricky_Kwon 0:7c0f5d94db37 254 #endif
Ricky_Kwon 0:7c0f5d94db37 255
Ricky_Kwon 0:7c0f5d94db37 256 // Transmit the packet in one burst with no pausing
Ricky_Kwon 0:7c0f5d94db37 257 for (int i = 0; i < (7 + bytes) ; i++) {
Ricky_Kwon 0:7c0f5d94db37 258 pc.putc(TxBuf[i]);
Ricky_Kwon 0:7c0f5d94db37 259 }
Ricky_Kwon 0:7c0f5d94db37 260 // Wait for the bytes to be transmitted
Ricky_Kwon 0:7c0f5d94db37 261 wait (0.00002);
Ricky_Kwon 0:7c0f5d94db37 262
Ricky_Kwon 0:7c0f5d94db37 263 // Skip if the read was to the broadcast address
Ricky_Kwon 0:7c0f5d94db37 264 if (ID != 0xFE) {
Ricky_Kwon 0:7c0f5d94db37 265
Ricky_Kwon 0:7c0f5d94db37 266
Ricky_Kwon 0:7c0f5d94db37 267
Ricky_Kwon 0:7c0f5d94db37 268 // response packet is always 6 + bytes
Ricky_Kwon 0:7c0f5d94db37 269 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
Ricky_Kwon 0:7c0f5d94db37 270 // timeout is a little more than the time to transmit
Ricky_Kwon 0:7c0f5d94db37 271 // the packet back, i.e. (6+bytes)*10 bit periods
Ricky_Kwon 0:7c0f5d94db37 272
Ricky_Kwon 0:7c0f5d94db37 273 int timeout = 0;
Ricky_Kwon 0:7c0f5d94db37 274 int plen = 0;
Ricky_Kwon 0:7c0f5d94db37 275 while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {
Ricky_Kwon 0:7c0f5d94db37 276
Ricky_Kwon 0:7c0f5d94db37 277 if (pc.readable()) {
Ricky_Kwon 0:7c0f5d94db37 278 Status[plen] = pc.getc();
Ricky_Kwon 0:7c0f5d94db37 279 plen++;
Ricky_Kwon 0:7c0f5d94db37 280 timeout = 0;
Ricky_Kwon 0:7c0f5d94db37 281 }
Ricky_Kwon 0:7c0f5d94db37 282
Ricky_Kwon 0:7c0f5d94db37 283 // wait for the bit period
Ricky_Kwon 0:7c0f5d94db37 284 wait (1.0/9600);
Ricky_Kwon 0:7c0f5d94db37 285 timeout++;
Ricky_Kwon 0:7c0f5d94db37 286 }
Ricky_Kwon 0:7c0f5d94db37 287
Ricky_Kwon 0:7c0f5d94db37 288 if (timeout == ((6+bytes)*10) ) {
Ricky_Kwon 0:7c0f5d94db37 289 return(-1);
Ricky_Kwon 0:7c0f5d94db37 290 }
Ricky_Kwon 0:7c0f5d94db37 291
Ricky_Kwon 0:7c0f5d94db37 292 // Copy the data from Status into data for return
Ricky_Kwon 0:7c0f5d94db37 293 for (int i=0; i < Status[3]-2 ; i++) {
Ricky_Kwon 0:7c0f5d94db37 294 data[i] = Status[5+i];
Ricky_Kwon 0:7c0f5d94db37 295 }
Ricky_Kwon 0:7c0f5d94db37 296
Ricky_Kwon 0:7c0f5d94db37 297 #ifdef AX12_READ_DEBUG
Ricky_Kwon 0:7c0f5d94db37 298 printf("\nStatus Packet\n");
Ricky_Kwon 0:7c0f5d94db37 299 printf(" Header : 0x%x\n",Status[0]);
Ricky_Kwon 0:7c0f5d94db37 300 printf(" Header : 0x%x\n",Status[1]);
Ricky_Kwon 0:7c0f5d94db37 301 printf(" ID : 0x%x\n",Status[2]);
Ricky_Kwon 0:7c0f5d94db37 302 printf(" Length : 0x%x\n",Status[3]);
Ricky_Kwon 0:7c0f5d94db37 303 printf(" Error Code : 0x%x\n",Status[4]);
Ricky_Kwon 0:7c0f5d94db37 304
Ricky_Kwon 0:7c0f5d94db37 305 for (int i=0; i < Status[3]-2 ; i++) {
Ricky_Kwon 0:7c0f5d94db37 306 printf(" Data : 0x%x\n",Status[5+i]);
Ricky_Kwon 0:7c0f5d94db37 307 }
Ricky_Kwon 0:7c0f5d94db37 308
Ricky_Kwon 0:7c0f5d94db37 309 printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
Ricky_Kwon 0:7c0f5d94db37 310 #endif
Ricky_Kwon 0:7c0f5d94db37 311
Ricky_Kwon 0:7c0f5d94db37 312 } // if (ID!=0xFE)
Ricky_Kwon 0:7c0f5d94db37 313
Ricky_Kwon 0:7c0f5d94db37 314 return(Status[4]);
Ricky_Kwon 0:7c0f5d94db37 315 }
Ricky_Kwon 0:7c0f5d94db37 316 int read(int ID, int start, int bytes, char* data) {
Ricky_Kwon 0:7c0f5d94db37 317
Ricky_Kwon 0:7c0f5d94db37 318 char PacketLength = 0x4;
Ricky_Kwon 0:7c0f5d94db37 319 char TxBuf[16];
Ricky_Kwon 0:7c0f5d94db37 320 char sum = 0;
Ricky_Kwon 0:7c0f5d94db37 321 char Status[16];
Ricky_Kwon 0:7c0f5d94db37 322
Ricky_Kwon 0:7c0f5d94db37 323 Status[4] = 0xFE; // return code
Ricky_Kwon 0:7c0f5d94db37 324
Ricky_Kwon 0:7c0f5d94db37 325 #ifdef AX12_READ_DEBUG
Ricky_Kwon 0:7c0f5d94db37 326 printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
Ricky_Kwon 0:7c0f5d94db37 327 #endif
Ricky_Kwon 0:7c0f5d94db37 328
Ricky_Kwon 0:7c0f5d94db37 329 // Build the TxPacket first in RAM, then we'll send in one go
Ricky_Kwon 0:7c0f5d94db37 330 #ifdef AX12_READ_DEBUG
Ricky_Kwon 0:7c0f5d94db37 331 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
Ricky_Kwon 0:7c0f5d94db37 332 #endif
Ricky_Kwon 0:7c0f5d94db37 333
Ricky_Kwon 0:7c0f5d94db37 334 TxBuf[0] = 0xff;
Ricky_Kwon 0:7c0f5d94db37 335 TxBuf[1] = 0xff;
Ricky_Kwon 0:7c0f5d94db37 336
Ricky_Kwon 0:7c0f5d94db37 337 // ID
Ricky_Kwon 0:7c0f5d94db37 338 TxBuf[2] = ID;
Ricky_Kwon 0:7c0f5d94db37 339 sum += TxBuf[2];
Ricky_Kwon 0:7c0f5d94db37 340
Ricky_Kwon 0:7c0f5d94db37 341 #ifdef AX12_READ_DEBUG
Ricky_Kwon 0:7c0f5d94db37 342 printf(" ID : %d\n",TxBuf[2]);
Ricky_Kwon 0:7c0f5d94db37 343 #endif
Ricky_Kwon 0:7c0f5d94db37 344
Ricky_Kwon 0:7c0f5d94db37 345 // Packet Length
Ricky_Kwon 0:7c0f5d94db37 346 TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
Ricky_Kwon 0:7c0f5d94db37 347 sum += TxBuf[3]; // Accululate the packet sum
Ricky_Kwon 0:7c0f5d94db37 348
Ricky_Kwon 0:7c0f5d94db37 349 #ifdef AX12_READ_DEBUG
Ricky_Kwon 0:7c0f5d94db37 350 printf(" Length : 0x%x\n",TxBuf[3]);
Ricky_Kwon 0:7c0f5d94db37 351 #endif
Ricky_Kwon 0:7c0f5d94db37 352
Ricky_Kwon 0:7c0f5d94db37 353 // Instruction - Read
Ricky_Kwon 0:7c0f5d94db37 354 TxBuf[4] = 0x2;
Ricky_Kwon 0:7c0f5d94db37 355 sum += TxBuf[4];
Ricky_Kwon 0:7c0f5d94db37 356
Ricky_Kwon 0:7c0f5d94db37 357 #ifdef AX12_READ_DEBUG
Ricky_Kwon 0:7c0f5d94db37 358 printf(" Instruction : 0x%x\n",TxBuf[4]);
Ricky_Kwon 0:7c0f5d94db37 359 #endif
Ricky_Kwon 0:7c0f5d94db37 360
Ricky_Kwon 0:7c0f5d94db37 361 // Start Address
Ricky_Kwon 0:7c0f5d94db37 362 TxBuf[5] = start;
Ricky_Kwon 0:7c0f5d94db37 363 sum += TxBuf[5];
Ricky_Kwon 0:7c0f5d94db37 364
Ricky_Kwon 0:7c0f5d94db37 365 #ifdef AX12_READ_DEBUG
Ricky_Kwon 0:7c0f5d94db37 366 printf(" Start Address : 0x%x\n",TxBuf[5]);
Ricky_Kwon 0:7c0f5d94db37 367 #endif
Ricky_Kwon 0:7c0f5d94db37 368
Ricky_Kwon 0:7c0f5d94db37 369 // Bytes to read
Ricky_Kwon 0:7c0f5d94db37 370 TxBuf[6] = bytes;
Ricky_Kwon 0:7c0f5d94db37 371 sum += TxBuf[6];
Ricky_Kwon 0:7c0f5d94db37 372
Ricky_Kwon 0:7c0f5d94db37 373 #ifdef AX12_READ_DEBUG
Ricky_Kwon 0:7c0f5d94db37 374 printf(" No bytes : 0x%x\n",TxBuf[6]);
Ricky_Kwon 0:7c0f5d94db37 375 #endif
Ricky_Kwon 0:7c0f5d94db37 376
Ricky_Kwon 0:7c0f5d94db37 377 // Checksum
Ricky_Kwon 0:7c0f5d94db37 378 TxBuf[7] = 0xFF - sum;
Ricky_Kwon 0:7c0f5d94db37 379 #ifdef AX12_READ_DEBUG
Ricky_Kwon 0:7c0f5d94db37 380 printf(" Checksum : 0x%x\n",TxBuf[7]);
Ricky_Kwon 0:7c0f5d94db37 381 #endif
Ricky_Kwon 0:7c0f5d94db37 382
Ricky_Kwon 0:7c0f5d94db37 383 // Transmit the packet in one burst with no pausing
Ricky_Kwon 0:7c0f5d94db37 384 for (int i = 0; i<8 ; i++) {
Ricky_Kwon 0:7c0f5d94db37 385 pc.putc(TxBuf[i]);
Ricky_Kwon 0:7c0f5d94db37 386 }
Ricky_Kwon 0:7c0f5d94db37 387
Ricky_Kwon 0:7c0f5d94db37 388 // Wait for the bytes to be transmitted
Ricky_Kwon 0:7c0f5d94db37 389 wait (0.00002);
Ricky_Kwon 0:7c0f5d94db37 390
Ricky_Kwon 0:7c0f5d94db37 391 // Skip if the read was to the broadcast address
Ricky_Kwon 0:7c0f5d94db37 392 if (ID != 0xFE) {
Ricky_Kwon 0:7c0f5d94db37 393
Ricky_Kwon 0:7c0f5d94db37 394
Ricky_Kwon 0:7c0f5d94db37 395
Ricky_Kwon 0:7c0f5d94db37 396 // response packet is always 6 + bytes
Ricky_Kwon 0:7c0f5d94db37 397 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
Ricky_Kwon 0:7c0f5d94db37 398 // timeout is a little more than the time to transmit
Ricky_Kwon 0:7c0f5d94db37 399 // the packet back, i.e. (6+bytes)*10 bit periods
Ricky_Kwon 0:7c0f5d94db37 400
Ricky_Kwon 0:7c0f5d94db37 401 int timeout = 0;
Ricky_Kwon 0:7c0f5d94db37 402 int plen = 0;
Ricky_Kwon 0:7c0f5d94db37 403 while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {
Ricky_Kwon 0:7c0f5d94db37 404
Ricky_Kwon 0:7c0f5d94db37 405 if (pc.readable()) {
Ricky_Kwon 0:7c0f5d94db37 406 Status[plen] = pc.getc();
Ricky_Kwon 0:7c0f5d94db37 407 plen++;
Ricky_Kwon 0:7c0f5d94db37 408 timeout = 0;
Ricky_Kwon 0:7c0f5d94db37 409 }
Ricky_Kwon 0:7c0f5d94db37 410
Ricky_Kwon 0:7c0f5d94db37 411 // wait for the bit period
Ricky_Kwon 0:7c0f5d94db37 412 wait (1.0/9600);
Ricky_Kwon 0:7c0f5d94db37 413 timeout++;
Ricky_Kwon 0:7c0f5d94db37 414 }
Ricky_Kwon 0:7c0f5d94db37 415
Ricky_Kwon 0:7c0f5d94db37 416 if (timeout == ((6+bytes)*10) ) {
Ricky_Kwon 0:7c0f5d94db37 417 return(-1);
Ricky_Kwon 0:7c0f5d94db37 418 }
Ricky_Kwon 0:7c0f5d94db37 419
Ricky_Kwon 0:7c0f5d94db37 420 // Copy the data from Status into data for return
Ricky_Kwon 0:7c0f5d94db37 421 for (int i=0; i < Status[3]-2 ; i++) {
Ricky_Kwon 0:7c0f5d94db37 422 data[i] = Status[5+i];
Ricky_Kwon 0:7c0f5d94db37 423 }
Ricky_Kwon 0:7c0f5d94db37 424
Ricky_Kwon 0:7c0f5d94db37 425 #ifdef AX12_READ_DEBUG
Ricky_Kwon 0:7c0f5d94db37 426 printf("\nStatus Packet\n");
Ricky_Kwon 0:7c0f5d94db37 427 printf(" Header : 0x%x\n",Status[0]);
Ricky_Kwon 0:7c0f5d94db37 428 printf(" Header : 0x%x\n",Status[1]);
Ricky_Kwon 0:7c0f5d94db37 429 printf(" ID : 0x%x\n",Status[2]);
Ricky_Kwon 0:7c0f5d94db37 430 printf(" Length : 0x%x\n",Status[3]);
Ricky_Kwon 0:7c0f5d94db37 431 printf(" Error Code : 0x%x\n",Status[4]);
Ricky_Kwon 0:7c0f5d94db37 432
Ricky_Kwon 0:7c0f5d94db37 433 for (int i=0; i < Status[3]-2 ; i++) {
Ricky_Kwon 0:7c0f5d94db37 434 printf(" Data : 0x%x\n",Status[5+i]);
Ricky_Kwon 0:7c0f5d94db37 435 }
Ricky_Kwon 0:7c0f5d94db37 436
Ricky_Kwon 0:7c0f5d94db37 437 printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
Ricky_Kwon 0:7c0f5d94db37 438 #endif
Ricky_Kwon 0:7c0f5d94db37 439
Ricky_Kwon 0:7c0f5d94db37 440 } // if (ID!=0xFE)
Ricky_Kwon 0:7c0f5d94db37 441
Ricky_Kwon 0:7c0f5d94db37 442 return(Status[4]);
Ricky_Kwon 0:7c0f5d94db37 443 }
Ricky_Kwon 0:7c0f5d94db37 444 int isMoving(int ID) {
Ricky_Kwon 0:7c0f5d94db37 445
Ricky_Kwon 0:7c0f5d94db37 446 char data[1];
Ricky_Kwon 0:7c0f5d94db37 447 read(ID,AX12_REG_MOVING,1,data);
Ricky_Kwon 0:7c0f5d94db37 448 return(data[0]);
Ricky_Kwon 0:7c0f5d94db37 449 }
Ricky_Kwon 0:7c0f5d94db37 450 void Forward()
Ricky_Kwon 0:7c0f5d94db37 451 {
Ricky_Kwon 0:7c0f5d94db37 452 char i, j;
Ricky_Kwon 0:7c0f5d94db37 453 int Forward_LegUR[8]={646, 646, 448, 377, 372, 587, 643, 512};
Ricky_Kwon 0:7c0f5d94db37 454 int Forward_LegUL[8]={650, 650, 437, 375, 367, 590, 650, 512};
Ricky_Kwon 0:7c0f5d94db37 455 int Forward_LegDR[8]={512, 444, 412, 512, 643, 682, 512, 512};
Ricky_Kwon 0:7c0f5d94db37 456 int Forward_LegDL[8]={512, 359, 326, 512, 572, 607, 512, 512};
Ricky_Kwon 0:7c0f5d94db37 457
Ricky_Kwon 0:7c0f5d94db37 458 for(i=0; i<8; i++){
Ricky_Kwon 0:7c0f5d94db37 459 for(j=0; j<2; j++){
Ricky_Kwon 0:7c0f5d94db37 460 SetGoal(15, Forward_LegUR[i], 1);
Ricky_Kwon 0:7c0f5d94db37 461 SetGoal(5, Forward_LegUL[i], 1);
Ricky_Kwon 0:7c0f5d94db37 462 SetGoal(8, Forward_LegDR[i], 1);
Ricky_Kwon 0:7c0f5d94db37 463 SetGoal(3, Forward_LegDL[i], 1);
Ricky_Kwon 0:7c0f5d94db37 464 }
Ricky_Kwon 0:7c0f5d94db37 465 wait(0.2);
Ricky_Kwon 0:7c0f5d94db37 466 }
Ricky_Kwon 0:7c0f5d94db37 467 }
Ricky_Kwon 0:7c0f5d94db37 468 void Backward()
Ricky_Kwon 0:7c0f5d94db37 469 {
Ricky_Kwon 0:7c0f5d94db37 470 char i, j;
Ricky_Kwon 0:7c0f5d94db37 471 int Backward_LegUR[8]={641, 641, 425, 378, 385, 589, 643, 512};
Ricky_Kwon 0:7c0f5d94db37 472 int Backward_LegUL[8]={646, 646, 422, 379, 377, 593, 648, 512};
Ricky_Kwon 0:7c0f5d94db37 473 int Backward_LegDR[8]={512, 653, 672, 512, 433, 417, 512, 512};
Ricky_Kwon 0:7c0f5d94db37 474 int Backward_LegDL[8]={512, 582, 605, 512, 357, 344, 512, 512};
Ricky_Kwon 0:7c0f5d94db37 475
Ricky_Kwon 0:7c0f5d94db37 476 for(i=0; i<8; i++){
Ricky_Kwon 0:7c0f5d94db37 477 for(j=0; j<2; j++){
Ricky_Kwon 0:7c0f5d94db37 478 SetGoal(15, Backward_LegUR[i], 1);
Ricky_Kwon 0:7c0f5d94db37 479 SetGoal(5, Backward_LegUL[i], 1);
Ricky_Kwon 0:7c0f5d94db37 480 SetGoal(8, Backward_LegDR[i], 1);
Ricky_Kwon 0:7c0f5d94db37 481 SetGoal(3, Backward_LegDL[i], 1);
Ricky_Kwon 0:7c0f5d94db37 482 }
Ricky_Kwon 0:7c0f5d94db37 483 wait(0.2);
Ricky_Kwon 0:7c0f5d94db37 484 }
Ricky_Kwon 0:7c0f5d94db37 485 }
Ricky_Kwon 0:7c0f5d94db37 486 void Right()
Ricky_Kwon 0:7c0f5d94db37 487 {
Ricky_Kwon 0:7c0f5d94db37 488 char i, j;
Ricky_Kwon 0:7c0f5d94db37 489 int Right_LegUR[6]={512, 512, 512, 512, 512, 512};
Ricky_Kwon 0:7c0f5d94db37 490 int Right_LegUL[6]={512, 512, 404, 404, 404, 512};
Ricky_Kwon 0:7c0f5d94db37 491 int Right_LegDR[6]={512, 684, 661, 512, 458, 512};
Ricky_Kwon 0:7c0f5d94db37 492 int Right_LegDL[6]={512, 585, 604, 512, 398, 512};
Ricky_Kwon 0:7c0f5d94db37 493
Ricky_Kwon 0:7c0f5d94db37 494 for(i=0; i<6; i++){
Ricky_Kwon 0:7c0f5d94db37 495 for(j=0; j<2; j++){
Ricky_Kwon 0:7c0f5d94db37 496 SetGoal(15, Right_LegUR[i], 1);
Ricky_Kwon 0:7c0f5d94db37 497 SetGoal(5, Right_LegUL[i], 1);
Ricky_Kwon 0:7c0f5d94db37 498 SetGoal(8, Right_LegDR[i], 1);
Ricky_Kwon 0:7c0f5d94db37 499 SetGoal(3, Right_LegDL[i], 1);
Ricky_Kwon 0:7c0f5d94db37 500 }
Ricky_Kwon 0:7c0f5d94db37 501 wait(0.2);
Ricky_Kwon 0:7c0f5d94db37 502 }
Ricky_Kwon 0:7c0f5d94db37 503 }
Ricky_Kwon 0:7c0f5d94db37 504 void Left()
Ricky_Kwon 0:7c0f5d94db37 505 {
Ricky_Kwon 0:7c0f5d94db37 506 char i, j;
Ricky_Kwon 0:7c0f5d94db37 507 int Left_LegUR[6]={512, 512, 620, 620, 620, 512};
Ricky_Kwon 0:7c0f5d94db37 508 int Left_LegUL[6]={512, 512, 512, 512, 512, 512};
Ricky_Kwon 0:7c0f5d94db37 509 int Left_LegDR[6]={512, 441, 419, 512, 660, 512};
Ricky_Kwon 0:7c0f5d94db37 510 int Left_LegDL[6]={512, 332, 341, 512, 575, 512};
Ricky_Kwon 0:7c0f5d94db37 511
Ricky_Kwon 0:7c0f5d94db37 512 for(i=0; i<6; i++){
Ricky_Kwon 0:7c0f5d94db37 513 for(j=0; j<2; j++){
Ricky_Kwon 0:7c0f5d94db37 514 SetGoal(15, Left_LegUR[i], 1);
Ricky_Kwon 0:7c0f5d94db37 515 SetGoal(5, Left_LegUL[i], 1);
Ricky_Kwon 0:7c0f5d94db37 516 SetGoal(8, Left_LegDR[i], 1);
Ricky_Kwon 0:7c0f5d94db37 517 SetGoal(3, Left_LegDL[i], 1);
Ricky_Kwon 0:7c0f5d94db37 518 }
Ricky_Kwon 0:7c0f5d94db37 519 wait(0.2);
Ricky_Kwon 0:7c0f5d94db37 520 }
Ricky_Kwon 0:7c0f5d94db37 521 }
Ricky_Kwon 0:7c0f5d94db37 522
Ricky_Kwon 0:7c0f5d94db37 523 void FreedmanInit()
Ricky_Kwon 0:7c0f5d94db37 524 {
Ricky_Kwon 0:7c0f5d94db37 525 SetGoal(4, HeadUD, 1);
Ricky_Kwon 0:7c0f5d94db37 526 SetGoal(16, AX_Iniit, 1);
Ricky_Kwon 0:7c0f5d94db37 527 SetGoal(15, AX_Iniit, 1);
Ricky_Kwon 0:7c0f5d94db37 528 SetGoal(5, AX_Iniit, 1);
Ricky_Kwon 0:7c0f5d94db37 529 SetGoal(8, AX_Iniit, 1);
Ricky_Kwon 0:7c0f5d94db37 530 SetGoal(3, AX_Iniit, 1);
Ricky_Kwon 0:7c0f5d94db37 531 }