Lawrence Lee
FAN ARM UDP Server controlled with WiFi
Dependencies: WizFi250Interface mbed
Fork of
WizFi250_AP_HelloWorld
by 
WIZnet
Diff: main.cpp
- Revision:
- 1:8d6d624124fd
- Parent:
- 0:87d959d6a3f3
--- a/main.cpp Tue Jul 14 06:55:18 2015 +0000
+++ b/main.cpp Wed Sep 23 00:50:09 2015 +0000
@@ -21,6 +21,12 @@
#include "mbed.h"
#include "WizFi250Interface.h"
+/* AX-12 */
+#define AX12_REG_GOAL_POSITION 0x1E
+#define AX12_REG_MOVING 0x2E
+#define AX_Init 330
+
+#define SERVER_PORT 5000
#define SECURE WizFi250::SEC_WPA2_MIXED
#define SSID "WizFi250_AP_Test"
@@ -31,12 +37,23 @@
Serial pc(USBTX, USBRX);
#endif
+PwmOut Fan(D3);
-/**
- * \brief Hello World
- * \param none
- * \return int
- */
+DigitalOut red(LED1);
+DigitalOut green(LED2);
+
+
+void UDPServer();
+
+/* AX-12 */
+int HeadUD = 200;
+int HeadRL = AX_Init;
+
+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);
+
int main()
{
pc.baud(115200);
@@ -47,5 +64,401 @@
if ( wizfi250.connect(SECURE, SSID, PASS, WizFi250::WM_AP)) return -1;
printf("IP Address is %s\r\n", wizfi250.getIPAddress());
+ wait(1.0);
+ UDPServer();
+
wizfi250.disconnect();
-}
\ No newline at end of file
+}
+
+
+void UDPServer(){
+ UDPSocket server;
+
+ printf("Socket opened\r\n");
+
+ server.set_blocking(false);
+ server.bind(SERVER_PORT);
+
+ printf("port open\r\n");
+
+ Endpoint client;
+
+ printf("endpoint created");
+
+ char buffer[10];
+
+ int move1 = AX_Init;
+ int move2 = AX_Init;
+
+ while(true)
+ {
+ int n = server.receiveFrom(client, buffer, sizeof(buffer));
+
+ if(n > 0)
+ {
+ //printf("Received packet from: %s\n", client.get_address());
+
+ buffer[n] = '\0';
+
+ if(strstr(buffer, "down"))
+ {
+ printf("down\r\n");
+ if(move1 >= 800){
+ move1 = move1;
+ SetGoal(8, move1, 1);
+ }else{
+ move1 += 20;
+ SetGoal(8, move1, 1);
+ }
+ }
+ else if(strstr(buffer, "up"))
+ {
+ printf("up\r\n");
+ if(move1 == 200){
+ move1 = move1;
+ SetGoal(8, move1, 1);
+ }else{
+ move1 -= 20;
+ SetGoal(8, move1, 1);
+ }
+ }
+ else if(strstr(buffer, "left"))
+ {
+ printf("left\r\n");
+ if(move2 >= 800){
+ move2 = move2;
+ SetGoal(16, move2, 1);
+ }else{
+ move2 += 20;
+ SetGoal(16, move2, 1);
+ }
+ }
+ else if(strstr(buffer, "right"))
+ {
+ printf("right\r\n");
+ if(move2 == 200){
+ move2 = move2;
+ SetGoal(16, move2, 1);
+ }else{
+ move2 -= 20;
+ SetGoal(16, move2, 1);
+ }
+ }
+ else if(strstr(buffer, "init"))
+ {
+ printf("init\r\n");
+ move1 = move1;
+ move2 = move2;
+ SetGoal(8, move1, 1);
+ SetGoal(16, move2, 1);
+ }
+ else if(strstr(buffer, "fanon"))
+ {
+ printf("fan on\r\n");
+ Fan.write(0.8);
+ }
+ else if(strstr(buffer, "fanoff"))
+ {
+ printf("fan off\r\n");
+ Fan.write(0);
+ }
+ }
+ }
+}
+
+
+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]);
+}