Changeset 22 in pacpussensors for trunk


Ignore:
Timestamp:
Aug 2, 2013, 4:19:44 PM (11 years ago)
Author:
phudelai
Message:

New control mode added + Security with the controller

Location:
trunk
Files:
9 edited

Legend:

Unmodified
Added
Removed

  • trunk/Dualshock/controller.cpp

    r21 r22  
     1/*********************************************************************
     2//  created:    2012/03/01 - 14:06
     3//  filename:   controller.cpp
     4//
     5//  author:     Pierre Hudelaine
     6//              Copyright Heudiasyc UMR UTC/CNRS 7253
     7//
     8//  version:    $Id: $
     9//
     10//  purpose:    Read the dualshock 3 button using bluetooth
     11//
     12*********************************************************************/
     13
    114#include "controller.h"
    215
    … …  
    4558    struct dualshockButtons_s d;
    4659    d.available = controller.paired;
     60    d.timeout = controller.timeout;
    4761    d.buttons = controller.buttons;
    4862    d.time = road_time();
    … …  
    7387        //std::cerr << "    elem\n";
    7488        //std::cerr << "Select\n";
    75         struct timeval tv = {2, 0}; //timeout = 2s
    76         if (select(fdmax + 1, &fds, NULL, NULL, &tv) < 0)
     89        struct timeval tv = {0, 500 * 1000}; //timeout = 0.5s
     90        int ret = 0;
     91        ret = select(fdmax + 1, &fds, NULL, NULL, &tv);
     92        if (ret < 0)
    7793            throw "Select";
     94       
     95        controller.timeout = (ret == 0 && controller.paired);
    7896
    7997        if (FD_ISSET(csk, &fds))
    … …  
    87105                handle_report(report, nr);
    88106        }
     107        update_shared_memory();
    89108    }
    90109    quit = 2;
    … …  
    129148    controller.addr = baddr;
    130149    controller.paired = 1;
     150    controller.timeout = 0;
    131151    controller.csk = cs;
    132152    controller.isk = is;
    … …  
    149169        HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUTPUT, 0x01,
    150170        '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00',
    151         '\00', static_cast<char>(ledmask[index % 10] << 1),
     171        '\00', static_cast<char>(index << 1),
    152172        LED_PERMANENT,
    153173        LED_PERMANENT,
    … …  
    217237    controller.buttons.axis.z                = buf[47];
    218238    controller.buttons.axis.gZ               = buf[47];
    219 
    220     update_shared_memory();
    221239}
    222240

  • trunk/Dualshock/controller.h

    r21 r22  
     1/*********************************************************************
     2//  created:    2012/03/01 - 14:06
     3//  filename:   controller.cpp
     4//
     5//  author:     Pierre Hudelaine
     6//              Copyright Heudiasyc UMR UTC/CNRS 7253
     7//
     8//  version:    $Id: $
     9//
     10//  purpose:    Read the dualshock 3 button using bluetooth
     11//
     12*********************************************************************/
     13
    114#ifndef CONTROLLER_H
    215# define CONTROLLER_H
    … …  
    8699            int isk;
    87100            int paired;
     101            int timeout;
    88102            unsigned index;
    89103    };
    … …  
    93107        struct buttons_s  buttons;
    94108        int               available;
     109        int               timeout;
    95110    };
    96111

  • trunk/Dualshock/dualshock.cpp

    r21 r22  
     1/*********************************************************************
     2//  created:    2012/03/01 - 14:06
     3//  filename:   dualshock.cpp
     4//
     5//  author:     Pierre Hudelaine
     6//              Copyright Heudiasyc UMR UTC/CNRS 7253
     7//
     8//  version:    $Id: $
     9//
     10//  purpose:    Read the dualshock 3 button using bluetooth
     11//
     12*********************************************************************/
     13
    114#include "dualshock.h"
    215

  • trunk/Dualshock/dualshock.h

    r21 r22  
     1/*********************************************************************
     2//  created:    2012/03/01 - 14:06
     3//  filename:   dualshock.cpp
     4//
     5//  author:     Pierre Hudelaine
     6//              Copyright Heudiasyc UMR UTC/CNRS 7253
     7//
     8//  version:    $Id: $
     9//
     10//  purpose:    Read the dualshock 3 button using bluetooth
     11//
     12*********************************************************************/
     13
    114#ifndef DUALSHOCK_H
    215# define DUALSHOCK_H

  • trunk/PacpusSocket/CMakeLists.txt

    r21 r22  
    5151set(
    5252    FILES_TO_MOC
    53     PacpusSocket.hpp
     53    PacpusSocket.h
    5454    ${PLUGIN_H}
    5555)

  • trunk/Wifibot/ps3remote.cpp

    r21 r22  
    2121    quit = 0;
    2222    b = NULL;
     23   
    2324    shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
     25   
     26    char buf[4] = {1, 1, 1, 1};
     27   
     28    timer_ = new QTimer();
     29    connect(timer_, SIGNAL(timeout()), this, SLOT(wifibotRemanentSending()));
    2430}
    2531
    … …  
    2935    quit = 0;
    3036    b = NULL;
     37   
    3138    shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
     39   
    3240    socket_ = socket;
     41   
     42    char buf[4] = {1, 1, 1, 1};
     43   
     44    timer_ = new QTimer();
     45    connect(timer_, SIGNAL(timeout()), this, SLOT(wifibotRemanentSending()));
    3346}
    3447
    … …  
    3649PS3remote::~PS3remote()
    3750{
    38 
    39 }
    40 
    41 
    42 void PS3remote::sendCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF)
     51}
     52
     53
     54void PS3remote::setCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF)
    4355{
    4456    float right, left;
    4557    static char lastLeft = 1;
    4658    static char lastRight = 1;
    47     char buf[5] = {0};
     59   
    4860    buf[1] = leftF;
    4961    buf[3] = rightF;
    … …  
    5163    left = ((float)Ls / 127) * 100;
    5264    right = ((float)Rs / 127) * 100;
     65   
    5366    if(left > 100.0)
    5467        left = 100.0;
     68   
    5569    if(right > 100.0)
    5670        right = 100.0;
     71   
    5772    buf[0] = (char)left + 1;
    5873    buf[2] = (char)right + 1;
     74   
    5975    if(buf[0] != lastLeft || buf[2] != lastRight) {
    6076        fprintf(stderr, "left = %c%d    right = %c%d\n", buf[1] == 1 ? '+' : '-', buf[0], buf[3] == 1 ? '+' : '-', buf[2]);
    61         socket_->sendDatagrams(QByteArray(buf));
    62     }
    63    
     77        sendCmd();
     78    }   
    6479
    6580    lastLeft = buf[0];
    … …  
    6883
    6984
    70 void PS3remote::mode2()
     85void PS3remote::sendCmd()
     86{
     87    if (b->timeout || !b->available)
     88      memset(buf, 1, sizeof(buf)); // Mise a 1 du buffer pour arreter le wifibot
     89   
     90    socket_->sendDatagrams(QByteArray(buf, 4));
     91    timer_->start(500);
     92}
     93
     94
     95void PS3remote::modeChar()
    7196{
    7297    uint16_t leftPWM = b->buttons.stick.leftStick_y;
    … …  
    7499    int leftF = 1, rightF = 1;
    75100
    76     if(leftPWM < 135 && leftPWM > 120)
     101    if(leftPWM < 135 && leftPWM > 120) {
    77102        leftPWM = 0;
    78     else {
     103    } else {
    79104        if(leftPWM > 128) {
    80105            leftPWM -= 128;
    81106            leftF = -1;
    82         }
    83         else
     107        } else {
    84108            leftPWM = 128 - leftPWM;
    85     }
    86     if(rightPWM < 135 && rightPWM > 120)
     109        }
     110    }
     111   
     112    if(rightPWM < 135 && rightPWM > 120) {
    87113        rightPWM = 0;
    88     else {
     114    } else {
    89115        if(rightPWM > 128) {
    90116            rightF = -1;
    91117            rightPWM -= 128;
    92         }
    93         else
     118        } else {
    94119            rightPWM = 128 - rightPWM;
    95     }
     120        }
     121    }
     122   
    96123    if(leftPWM > 127)
    97124        leftPWM = 126;
     125   
    98126    if(rightPWM > 127)
    99127        rightPWM = 126;
    100     sendCmd(leftPWM, rightPWM, leftF, rightF);
    101 }
    102 
    103 
    104 void PS3remote::mode1()
     128   
     129    setCmd(leftPWM, rightPWM, leftF, rightF);
     130}
     131
     132
     133void PS3remote::modePS3()
     134{
     135    int16_t speed = b->buttons.analog.r2 - b->buttons.analog.l2;
     136    float x = (float)b->buttons.stick.leftStick_x;
     137    int leftF = 1, rightF = 1;
     138       
     139   if(speed < 10 && speed > -10) {
     140        if(x > 140) {
     141            x = x - 128;
     142            rightF = -(leftF = 1);
     143        } else if (x < 115) {
     144            x = 128 - x;
     145            leftF = -(rightF = 1);
     146        } else {
     147            x = 0;
     148        }
     149       
     150        setCmd(x, x, leftF, rightF);   
     151    } else {
     152     
     153        if(speed > 0) {
     154            rightF = leftF = 1;
     155        } else {
     156            rightF = leftF = -1;
     157        }
     158       
     159        float right = 1.0 - (x > 140 ? 0.0 : ((128.0 - x) / 128.0));
     160        float left = 1.0 - (x < 115 ? 0.0 : ((x - 128.0) / 128.0));
     161
     162        speed /= 2.5;
     163        speed = abs(speed);
     164       
     165        setCmd(right * speed, left * speed, leftF, rightF);
     166    }   
     167}
     168
     169
     170void PS3remote::modeJoystick()
    105171{
    106172    uint16_t speed = b->buttons.stick.leftStick_y;
    107173    float x = (float)b->buttons.stick.rightStick_x;
    108174    int leftF = 1, rightF = 1;
     175   
    109176    if(speed < 140 && speed > 115) {
    110177        if(x > 140) {
    111178            x = x - 128;
    112179            rightF = -(leftF = 1);
    113         }
    114         else if (x < 115) {
     180        } else if (x < 115) {
    115181            x = 128 - x;
    116182            leftF = -(rightF = 1);
    117         }
    118         else
     183        } else {
    119184            x = 0;
    120         sendCmd(x, x, leftF, rightF);
    121     }
    122     else {
     185        }
     186       
     187        setCmd(x, x, leftF, rightF);
     188    } else {
    123189        if(speed > 128) {
    124190            rightF = leftF = -1;
    125191            speed -= 128;
    126         }
    127         else
     192        } else {
    128193            speed = 128 - speed;
     194        }
     195       
    129196        float right = 1.0 - (x > 140 ? 0.0 : ((128.0 - x) / 128.0));
    130197        float left = 1.0 - (x < 115 ? 0.0 : ((x - 128.0) / 128.0));
    131         sendCmd(right * speed, left * speed, leftF, rightF);
     198       
     199        setCmd(right * speed, left * speed, leftF, rightF);
    132200    }
    133201}
    … …  
    137205{
    138206    static int lastState = 0;
    139     static int mode = 1;
     207    static int mode = 0;
    140208
    141209    if(b->buttons.digital.select) {
    142210        if(!lastState) {
    143             mode = mode == 1 ? 2 : 1;
     211            //mode = mode == 1 ? 2 : 1;
    144212            // TODO later
    145213            //blueshock_setLeds(0, mode);
    146             lastState = 1;
     214            mode = (mode + 1) % 3;
     215            lastState = 1;
    147216        }
    148     }
    149     else
     217    } else {
    150218        lastState = 0;
    151    
    152     if (mode == 1)
    153         mode1();
    154     else
    155         mode2();
    156 }
    157 
     219    }
     220   
     221    switch (mode){
     222      case 0:
     223        modePS3();
     224        break;
     225     
     226      case 1:
     227        modeJoystick();
     228        break;
     229       
     230      default:
     231      case 2:
     232        modeChar();
     233        break;
     234    }
     235}
     236
     237
     238void PS3remote::wifibotRemanentSending()
     239{
     240    sendCmd();
     241    fprintf(stderr, "Remanence\n");
     242    timer_->start(500);
     243}
    158244
    159245void PS3remote::run()
    160246{
    161247    while(!quit) {
     248     
    162249        shmem->wait();
    163250        b = (struct dualshockButtons_s *)shmem->read();
    … …  
    166253       
    167254        if(!b->available)
    168             std::cerr << "Waiting for remote...\n";
     255            fprintf(stderr, "Waiting for remote...\n");
    169256    }
    170257}
    … …  
    173260void PS3remote::stop()
    174261{
     262    timer_->stop();
    175263    quit = 1;
    176264}

  • trunk/Wifibot/ps3remote.h

    r21 r22  
    88//  version:    $Id: $
    99//
    10 //  purpose:    Read the dualshock 3 button using bluetooth
     10//  purpose:    Read the dualshock 3 buttons using bluetooth
    1111//
    1212*********************************************************************/
    … …  
    1717# include <QApplication>
    1818# include <QThread>
     19# include <QTimer>
    1920
    2021# include "../Dualshock/controller.h"
    21 # include "../../../pacpusbase/PacpusSocket/PacpusSocket.hpp"
     22# include "../../../pacpusbase/PacpusSocket/PacpusSocket.h"
    2223
    2324namespace pacpus {
    … …  
    3233            void run();
    3334            void stop();
    34             void sendCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF);
    35             void mode2();
    36             void mode1();
    37             void checkMode();
     35            void setCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF);
     36            void sendCmd();
     37            void modePS3();
     38            void modeChar();
     39            void modeJoystick();
     40            void checkMode();       
     41           
    3842        private:
    3943            ShMem *shmem;
    … …  
    4246            int quit;
    4347            PacpusSocket * socket_;
     48            QTimer * timer_;
     49            char buf[4];
     50
     51        public Q_SLOTS:
     52            void wifibotRemanentSending();
    4453    };
    4554

  • trunk/Wifibot/wifibot.cpp

    r21 r22  
    88//  version:    $Id: $
    99//
    10 //  purpose:   
     10//  purpose:    Control the wifibot with the dualshock 3
    1111//
    1212*********************************************************************/
    … …  
    6767{
    6868    dualshock = new PS3remote(pacpusSocket_);
    69 
    7069    dualshock->start();
    7170}

  • trunk/Wifibot/wifibot.h

    r21 r22  
    88//  version:    $Id: $
    99//
    10 //  purpose:   
     10//  purpose:    Control the wifibot with the dualshock 3
    1111//
    1212*********************************************************************/
Note: See TracChangeset for help on using the changeset viewer.