Revision 0:1b4c3bbaf102, committed 2013-03-12

Comitter:

mariob

Date:

Tue Mar 12 07:39:13 2013 +0000

Commit message:

This project aims at hacking a remote controlled car by replacing the tx/rx and navigation systems. This project implements the pc side.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/joystick/joystick.cpp	Tue Mar 12 07:39:13 2013 +0000
@@ -0,0 +1,65 @@
+#include "joystick.hpp"
+
+
+void Joystick::tuneChannel (int nOfSamples, int channel)
+{
+    if (channel<0 || channel>3)
+        return;
+    ranges[channel].maxV = std::numeric_limits<float>::min();
+    ranges[channel].minV = std::numeric_limits<float>::max();
+    for (int i=0; i<nOfSamples; i++) {
+        float val = channels[channel]->read();
+        if (val>ranges[channel].maxV)
+            ranges[channel].maxV = val;
+        else if (val<ranges[channel].minV)
+            ranges[channel].minV = val;
+        wait(0.1);
+    }
+}
+
+void Joystick::tuneCentring (int nOfSamples, int channel)
+{
+    if (channel<0 || channel>3)
+        return;
+    ranges[channel].center = 0.0;
+    for (int i=0; i<nOfSamples; i++) {
+        float val = channels[channel]->read();
+        ranges[channel].center += val;
+        wait(0.1);
+    }
+    ranges[channel].center /= nOfSamples;
+}
+
+
+
+float Joystick::read(int channel)
+{
+    if (channel<0 || channel>3)
+        return std::numeric_limits<float>::min();
+    mutexes[channel].lock();
+    float val = values[channel];
+    mutexes[channel].unlock();
+    return val;
+}
+
+
+void Joystick::update (int channel)
+{
+    mutexes[channel].lock();
+    float val = channels[channel]->read();
+    mutexes[channel].unlock();
+    if (val>ranges[channel].center)
+        values[channel] = -(val-ranges[channel].center)/
+                (ranges[channel].maxV-ranges[channel].center);
+    else
+        values[channel] = (ranges[channel].center-val)/
+                (ranges[channel].center-ranges[channel].minV);
+}
+
+void Joystick::update ()
+{
+    update(JOYSTICK_SX_X);
+    update(JOYSTICK_SX_Y);
+    update(JOYSTICK_DX_X);
+    update(JOYSTICK_DX_Y);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/joystick/joystick.hpp	Tue Mar 12 07:39:13 2013 +0000
@@ -0,0 +1,61 @@
+#ifndef __JOYSTICK_HPP__
+#define __JOYSTICK_HPP__
+
+#include <limits>
+
+#include "mbed.h"
+#include "rtos.h"
+
+#define JOYSTICK_SX_X 1
+#define JOYSTICK_SX_Y 0
+#define JOYSTICK_DX_X 2
+#define JOYSTICK_DX_Y 3
+
+class Joystick
+{
+
+    AnalogIn* channels[4];
+    float values[4];
+    Mutex mutexes[4];
+
+    struct RangeJoystichChannel {
+        float maxV;
+        float minV;
+        float center;
+        RangeJoystichChannel () {
+            maxV = std::numeric_limits<float>::min();
+            minV = std::numeric_limits<float>::max();
+            center = 0.0;
+        }
+    };
+
+    RangeJoystichChannel ranges[4];
+
+    void update(int channel);
+
+public:
+
+    Joystick (PinName pinSX_X, PinName pinSX_Y, PinName pinDX_X, PinName pinDX_Y) {
+        channels[JOYSTICK_SX_X] = new AnalogIn(pinSX_X);
+        channels[JOYSTICK_SX_Y] = new AnalogIn(pinSX_Y);
+        channels[JOYSTICK_DX_X] = new AnalogIn(pinDX_X);
+        channels[JOYSTICK_DX_Y] = new AnalogIn(pinDX_Y);
+    }
+
+    void tuneCentring (int nOfSamples, int channel);
+    void tuneChannel (int nOfSamples, int channel);
+    
+    void update();
+
+    float read(int channel);
+
+    ~Joystick () {
+        delete channels[JOYSTICK_SX_X];
+        delete channels[JOYSTICK_SX_Y];
+        delete channels[JOYSTICK_DX_X];
+        delete channels[JOYSTICK_DX_Y];
+    }
+};
+
+
+#endif //__JOYSTICK_HPP__

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 12 07:39:13 2013 +0000
@@ -0,0 +1,109 @@
+#include "mbed.h"
+#include "rtos.h"
+
+#include "xbee.hpp"
+#include "ssWiSocket.hpp"
+#include "joystick.hpp"
+
+
+#define UPDATING_PERIOD_MS 200
+
+#define PORT_MOTOR 20
+#define PORT_SERVO 30
+#define PORT_SERVO_CAMERA_X 40
+#define PORT_SERVO_CAMERA_Y 50
+
+
+ssWiSocket* socket_motor;
+ssWiSocket* socket_servo;
+ssWiSocket* socket_camera_x;
+ssWiSocket* socket_camera_y;
+
+Joystick joystick(p15, p16, p17, p18);
+
+
+void tuneJoystickChannel (PinName pinLed, int joystickChannel);
+void updateFunction (const void* arg);
+
+
+int main ()
+{
+    //Communication link connection
+    XBeeModule xbee(p9, p10, 102, 14);
+    xbee.setDstAddress(XBeeBroadcastAddress());
+    xbee.init(5, -1);
+
+    //Socket creation
+    socket_motor = ssWiSocket::createSocket(PORT_MOTOR);
+    socket_servo = ssWiSocket::createSocket(PORT_SERVO);
+    socket_camera_x = ssWiSocket::createSocket(PORT_SERVO_CAMERA_X);
+    socket_camera_y = ssWiSocket::createSocket(PORT_SERVO_CAMERA_Y);
+    if (socket_motor==NULL || socket_servo==NULL || 
+                socket_camera_x==NULL || socket_camera_y==NULL)
+        printf("Error: socket error\n\r");
+
+    //Joystick centering
+    joystick.tuneCentring(20, JOYSTICK_SX_X);
+    joystick.tuneCentring(20, JOYSTICK_SX_Y);
+    joystick.tuneCentring(20, JOYSTICK_DX_X);
+    joystick.tuneCentring(20, JOYSTICK_DX_Y);
+
+    //Joystick tuning
+    {
+        //tuneJoystickChannel(LED1, JOYSTICK_SX_X);
+        DigitalOut led1(LED1);
+        led1 = 1;
+        joystick.tuneChannel(100, JOYSTICK_SX_X);
+        led1 = 0;
+    }
+    {
+        //tuneJoystickChannel(LED2, JOYSTICK_SX_Y);
+        DigitalOut led2(LED2);
+        led2 = 1;
+        joystick.tuneChannel(100, JOYSTICK_SX_Y);
+        led2 = 0;
+    }
+    {
+        //tuneJoystickChannel(LED3, JOYSTICK_DX_X);
+        DigitalOut led3(LED3);
+        led3 = 1;
+        joystick.tuneChannel(100, JOYSTICK_DX_X);
+        led3 = 0;
+    }
+    {
+        //tuneJoystickChannel(LED4, JOYSTICK_DX_Y);
+        DigitalOut led4(LED4);
+        led4 = 1;
+        joystick.tuneChannel(100, JOYSTICK_DX_Y);
+        led4 = 0;
+    }
+    //Timer setting
+    RtosTimer timer (updateFunction, osTimerPeriodic, NULL);
+    timer.start(200);
+
+    //Infinite wait
+    Thread::wait(osWaitForever);
+}
+
+
+void updateFunction (const void* arg)
+{
+//    while(1) {
+    joystick.update();
+    socket_motor->write(-joystick.read(JOYSTICK_SX_Y)*1000.0);
+    socket_servo->write(joystick.read(JOYSTICK_SX_X)*1000.0);
+    socket_camera_x->write(joystick.read(JOYSTICK_DX_X)*1000.0);
+    socket_camera_y->write(joystick.read(JOYSTICK_DX_Y)*1000.0);
+//    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));
+//        Thread::wait(UPDATING_PERIOD_MS);
+//    }
+}
+
+
+void tuneJoystickChannel (PinName pinLed, int joystickChannel)
+{
+    DigitalOut led(pinLed);
+    led = 1;
+    joystick.tuneChannel(100, joystickChannel);
+    led = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue Mar 12 07:39:13 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#53e6cccd8782

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Mar 12 07:39:13 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/cd19af002ccc
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ssWi.lib	Tue Mar 12 07:39:13 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mariob/code/ssWi/#b50c3b3b241c