main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 
00004 #include "xbee.hpp"
00005 #include "ssWiSocket.hpp"
00006 #include "joystick.hpp"
00007 
00008 
00009 #define UPDATING_PERIOD_MS 200
00010 
00011 #define PORT_MOTOR 20
00012 #define PORT_SERVO 30
00013 #define PORT_SERVO_CAMERA_X 40
00014 #define PORT_SERVO_CAMERA_Y 50
00015 
00016 
00017 ssWiSocket* socket_motor;
00018 ssWiSocket* socket_servo;
00019 ssWiSocket* socket_camera_x;
00020 ssWiSocket* socket_camera_y;
00021 
00022 Joystick joystick(p15, p16, p17, p18);
00023 
00024 
00025 void tuneJoystickChannel (PinName pinLed, int joystickChannel);
00026 void updateFunction (const void* arg);
00027 
00028 
00029 int main ()
00030 {
00031     //Communication link connection
00032     XBeeModule xbee(p9, p10, 102, 14);
00033     xbee.setDstAddress(XBeeBroadcastAddress());
00034     xbee.init(5, -1);
00035 
00036     //Socket creation
00037     socket_motor = ssWiSocket::createSocket(PORT_MOTOR);
00038     socket_servo = ssWiSocket::createSocket(PORT_SERVO);
00039     socket_camera_x = ssWiSocket::createSocket(PORT_SERVO_CAMERA_X);
00040     socket_camera_y = ssWiSocket::createSocket(PORT_SERVO_CAMERA_Y);
00041     if (socket_motor==NULL || socket_servo==NULL || 
00042                 socket_camera_x==NULL || socket_camera_y==NULL)
00043         printf("Error: socket error\n\r");
00044 
00045     //Joystick centering
00046     joystick.tuneCentring(20, JOYSTICK_SX_X);
00047     joystick.tuneCentring(20, JOYSTICK_SX_Y);
00048     joystick.tuneCentring(20, JOYSTICK_DX_X);
00049     joystick.tuneCentring(20, JOYSTICK_DX_Y);
00050 
00051     //Joystick tuning
00052     {
00053         //tuneJoystickChannel(LED1, JOYSTICK_SX_X);
00054         DigitalOut led1(LED1);
00055         led1 = 1;
00056         joystick.tuneChannel(100, JOYSTICK_SX_X);
00057         led1 = 0;
00058     }
00059     {
00060         //tuneJoystickChannel(LED2, JOYSTICK_SX_Y);
00061         DigitalOut led2(LED2);
00062         led2 = 1;
00063         joystick.tuneChannel(100, JOYSTICK_SX_Y);
00064         led2 = 0;
00065     }
00066     {
00067         //tuneJoystickChannel(LED3, JOYSTICK_DX_X);
00068         DigitalOut led3(LED3);
00069         led3 = 1;
00070         joystick.tuneChannel(100, JOYSTICK_DX_X);
00071         led3 = 0;
00072     }
00073     {
00074         //tuneJoystickChannel(LED4, JOYSTICK_DX_Y);
00075         DigitalOut led4(LED4);
00076         led4 = 1;
00077         joystick.tuneChannel(100, JOYSTICK_DX_Y);
00078         led4 = 0;
00079     }
00080     //Timer setting
00081     RtosTimer timer (updateFunction, osTimerPeriodic, NULL);
00082     timer.start(200);
00083 
00084     //Infinite wait
00085     Thread::wait(osWaitForever);
00086 }
00087 
00088 
00089 void updateFunction (const void* arg)
00090 {
00091 //    while(1) {
00092     joystick.update();
00093     socket_motor->write(-joystick.read(JOYSTICK_SX_Y)*1000.0);
00094     socket_servo->write(joystick.read(JOYSTICK_SX_X)*1000.0);
00095     socket_camera_x->write(joystick.read(JOYSTICK_DX_X)*1000.0);
00096     socket_camera_y->write(joystick.read(JOYSTICK_DX_Y)*1000.0);
00097 //    printf("%f %f %f %f\n\r", joystick.read(JOYSTICK_SX_X), joystick.read(JOYSTICK_SX_Y), joystick.read(JOYSTICK_DX_X), joystick.read(JOYSTICK_DX_Y));
00098 //        Thread::wait(UPDATING_PERIOD_MS);
00099 //    }
00100 }
00101 
00102 
00103 void tuneJoystickChannel (PinName pinLed, int joystickChannel)
00104 {
00105     DigitalOut led(pinLed);
00106     led = 1;
00107     joystick.tuneChannel(100, joystickChannel);
00108     led = 0;
00109 }