main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 
00004 #include "xbee.hpp"
00005 #include "ssWiSocket.hpp"
00006 #include "car.hpp"
00007 
00008 
00009 #define PORT_MOTOR 20
00010 #define PORT_SERVO 30
00011 #define PORT_SERVO_CAMERA_X 40
00012 #define PORT_SERVO_CAMERA_Y 50
00013 
00014 #define UPDATING_FUNCTION_MS 200
00015 
00016 
00017 ssWiSocket* socket_motor;
00018 ssWiSocket* socket_servo;
00019 ssWiSocket* socket_camera_x;
00020 ssWiSocket* socket_camera_y;
00021 
00022 Car car(p23, p24, p25, p26);
00023 
00024 
00025 void udateFunction (const void* arg);
00026 
00027 
00028 int main()
00029 {
00030     //Communication link connection
00031     XBeeModule xbee(p9, p10, 102, 14);
00032     xbee.setDstAddress(XBeeBroadcastAddress());
00033     xbee.init(-1, 5);
00034 
00035     //Socket creation
00036     socket_motor = ssWiSocket::createSocket(PORT_MOTOR);
00037     socket_servo = ssWiSocket::createSocket(PORT_SERVO);
00038     socket_camera_x = ssWiSocket::createSocket(PORT_SERVO_CAMERA_X);
00039     socket_camera_y = ssWiSocket::createSocket(PORT_SERVO_CAMERA_Y);
00040     if (socket_motor==NULL || socket_servo==NULL ||
00041                     socket_camera_x==NULL || socket_camera_y==NULL)
00042         printf("Error: sockets\n\r");
00043 
00044     //Timer setting
00045     RtosTimer timer(udateFunction, osTimerPeriodic, NULL);
00046     timer.start(200);
00047 
00048     //Infinite wait
00049     Thread::wait(osWaitForever);
00050 }
00051 
00052 
00053 void udateFunction (const void* arg)
00054 {
00055     static DigitalOut led(LED1);
00056     static int var = 0;
00057 //    while(1) {
00058     car.setSpeed((float)(socket_motor->read())/1000.0);
00059     car.setSteering((float)(socket_servo->read())/1000.0);
00060     float x = (float)socket_camera_x->read()/1000.0;
00061     float y = (float)socket_camera_y->read()/1000.0;
00062     car.setCamera(x, y);
00063     led = var>=10?1:0;
00064     var = var>=20?0:var+1;
00065 //        Thread::wait(UPDATING_FUNCTION_MS);
00066 //    }
00067 }