main.cpp

00001 #include "mbed.h"
00002 #include "EthernetInterface.h"
00003 #include "C12832_lcd.h"
00004 #include "SensorDataParser.h"
00005 #include <math.h>
00006 
00007 static EthernetInterface eth;
00008 static C12832_LCD lcd;
00009 static Serial pc(USBTX, USBRX);
00010 
00011 DigitalOut led_left(LED1);
00012 DigitalOut led_up(LED2);
00013 DigitalOut led_down(LED3);
00014 DigitalOut led_right(LED4);
00015 
00016 // Configuration
00017 #define SERVER_PORT     5555
00018 #define MAX_BUF_SIZE    512
00019 #define SENSE_DELTA     0.1
00020 
00021 static void ethernet_init() {
00022     eth.init();
00023     if(eth.connect(30000) == 0)
00024         pc.printf("Connect OK\n\r");
00025 
00026     lcd.locate(0,0);
00027     lcd.printf("IP:%s", eth.getIPAddress());
00028 
00029     pc.printf("IP Address: %s\n\r", eth.getIPAddress());
00030 }
00031 
00032 static void main_loop() {
00033     UDPSocket server;
00034     Endpoint client;
00035     char buffer[MAX_BUF_SIZE];
00036     SENSOR_DATA pd;
00037     
00038     server.bind(SERVER_PORT);
00039     while (true) {
00040         int n = server.receiveFrom(client, buffer, sizeof(buffer) - 1);
00041         if (n == sizeof(buffer) - 1)
00042             continue;
00043         buffer[n] = 0;
00044 
00045         if (parse_sensor_packet(buffer, &pd) == 0) continue;
00046         
00047         if(fabs(pd.ax) > SENSE_DELTA) {
00048             //printf("ax: %f\r\n", pd.ax);
00049         }
00050         if(fabs(pd.ay) > SENSE_DELTA) {
00051             //printf("ay: %f\r\n", pd.ay);
00052         }
00053 
00054         if (pd.ax > 0) {
00055             led_left = 0;
00056             led_right = fabs(pd.ax) * 2;
00057         } else {
00058             led_left = fabs(pd.ax) * 2;
00059             led_right = 0;
00060         }
00061 
00062         if (pd.ay > 0) {
00063             led_up = 0;
00064             led_down = fabs(pd.ay) * 2;
00065         } else {
00066             led_up = fabs(pd.ay) * 2;
00067             led_down = 0;
00068         }
00069     }
00070 }
00071 
00072 int main() {
00073     lcd.cls(); 
00074     ethernet_init();
00075     main_loop();
00076 }