Changeset 22 in pacpussensors
- Timestamp:
- Aug 2, 2013, 4:19:44 PM (11 years ago)
- 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 1 14 #include "controller.h" 2 15 … … 45 58 struct dualshockButtons_s d; 46 59 d.available = controller.paired; 60 d.timeout = controller.timeout; 47 61 d.buttons = controller.buttons; 48 62 d.time = road_time(); … … 73 87 //std::cerr << " elem\n"; 74 88 //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) 77 93 throw "Select"; 94 95 controller.timeout = (ret == 0 && controller.paired); 78 96 79 97 if (FD_ISSET(csk, &fds)) … … 87 105 handle_report(report, nr); 88 106 } 107 update_shared_memory(); 89 108 } 90 109 quit = 2; … … 129 148 controller.addr = baddr; 130 149 controller.paired = 1; 150 controller.timeout = 0; 131 151 controller.csk = cs; 132 152 controller.isk = is; … … 149 169 HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUTPUT, 0x01, 150 170 '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00', 151 '\00', static_cast<char>( ledmask[index % 10]<< 1),171 '\00', static_cast<char>(index << 1), 152 172 LED_PERMANENT, 153 173 LED_PERMANENT, … … 217 237 controller.buttons.axis.z = buf[47]; 218 238 controller.buttons.axis.gZ = buf[47]; 219 220 update_shared_memory();221 239 } 222 240 -
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 1 14 #ifndef CONTROLLER_H 2 15 # define CONTROLLER_H … … 86 99 int isk; 87 100 int paired; 101 int timeout; 88 102 unsigned index; 89 103 }; … … 93 107 struct buttons_s buttons; 94 108 int available; 109 int timeout; 95 110 }; 96 111 -
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 1 14 #include "dualshock.h" 2 15 -
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 1 14 #ifndef DUALSHOCK_H 2 15 # define DUALSHOCK_H -
trunk/PacpusSocket/CMakeLists.txt
r21 r22 51 51 set( 52 52 FILES_TO_MOC 53 PacpusSocket.h pp53 PacpusSocket.h 54 54 ${PLUGIN_H} 55 55 ) -
trunk/Wifibot/ps3remote.cpp
r21 r22 21 21 quit = 0; 22 22 b = NULL; 23 23 24 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())); 24 30 } 25 31 … … 29 35 quit = 0; 30 36 b = NULL; 37 31 38 shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s)); 39 32 40 socket_ = socket; 41 42 char buf[4] = {1, 1, 1, 1}; 43 44 timer_ = new QTimer(); 45 connect(timer_, SIGNAL(timeout()), this, SLOT(wifibotRemanentSending())); 33 46 } 34 47 … … 36 49 PS3remote::~PS3remote() 37 50 { 38 39 } 40 41 42 void PS3remote::sendCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF) 51 } 52 53 54 void PS3remote::setCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF) 43 55 { 44 56 float right, left; 45 57 static char lastLeft = 1; 46 58 static char lastRight = 1; 47 char buf[5] = {0};59 48 60 buf[1] = leftF; 49 61 buf[3] = rightF; … … 51 63 left = ((float)Ls / 127) * 100; 52 64 right = ((float)Rs / 127) * 100; 65 53 66 if(left > 100.0) 54 67 left = 100.0; 68 55 69 if(right > 100.0) 56 70 right = 100.0; 71 57 72 buf[0] = (char)left + 1; 58 73 buf[2] = (char)right + 1; 74 59 75 if(buf[0] != lastLeft || buf[2] != lastRight) { 60 76 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 } 64 79 65 80 lastLeft = buf[0]; … … 68 83 69 84 70 void PS3remote::mode2() 85 void 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 95 void PS3remote::modeChar() 71 96 { 72 97 uint16_t leftPWM = b->buttons.stick.leftStick_y; … … 74 99 int leftF = 1, rightF = 1; 75 100 76 if(leftPWM < 135 && leftPWM > 120) 101 if(leftPWM < 135 && leftPWM > 120) { 77 102 leftPWM = 0; 78 else {103 } else { 79 104 if(leftPWM > 128) { 80 105 leftPWM -= 128; 81 106 leftF = -1; 82 } 83 else 107 } else { 84 108 leftPWM = 128 - leftPWM; 85 } 86 if(rightPWM < 135 && rightPWM > 120) 109 } 110 } 111 112 if(rightPWM < 135 && rightPWM > 120) { 87 113 rightPWM = 0; 88 else {114 } else { 89 115 if(rightPWM > 128) { 90 116 rightF = -1; 91 117 rightPWM -= 128; 92 } 93 else 118 } else { 94 119 rightPWM = 128 - rightPWM; 95 } 120 } 121 } 122 96 123 if(leftPWM > 127) 97 124 leftPWM = 126; 125 98 126 if(rightPWM > 127) 99 127 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 133 void 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 170 void PS3remote::modeJoystick() 105 171 { 106 172 uint16_t speed = b->buttons.stick.leftStick_y; 107 173 float x = (float)b->buttons.stick.rightStick_x; 108 174 int leftF = 1, rightF = 1; 175 109 176 if(speed < 140 && speed > 115) { 110 177 if(x > 140) { 111 178 x = x - 128; 112 179 rightF = -(leftF = 1); 113 } 114 else if (x < 115) { 180 } else if (x < 115) { 115 181 x = 128 - x; 116 182 leftF = -(rightF = 1); 117 } 118 else 183 } else { 119 184 x = 0; 120 sendCmd(x, x, leftF, rightF); 121 } 122 else { 185 } 186 187 setCmd(x, x, leftF, rightF); 188 } else { 123 189 if(speed > 128) { 124 190 rightF = leftF = -1; 125 191 speed -= 128; 126 } 127 else 192 } else { 128 193 speed = 128 - speed; 194 } 195 129 196 float right = 1.0 - (x > 140 ? 0.0 : ((128.0 - x) / 128.0)); 130 197 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); 132 200 } 133 201 } … … 137 205 { 138 206 static int lastState = 0; 139 static int mode = 1;207 static int mode = 0; 140 208 141 209 if(b->buttons.digital.select) { 142 210 if(!lastState) { 143 mode = mode == 1 ? 2 : 1;211 //mode = mode == 1 ? 2 : 1; 144 212 // TODO later 145 213 //blueshock_setLeds(0, mode); 146 lastState = 1; 214 mode = (mode + 1) % 3; 215 lastState = 1; 147 216 } 148 } 149 else 217 } else { 150 218 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 238 void PS3remote::wifibotRemanentSending() 239 { 240 sendCmd(); 241 fprintf(stderr, "Remanence\n"); 242 timer_->start(500); 243 } 158 244 159 245 void PS3remote::run() 160 246 { 161 247 while(!quit) { 248 162 249 shmem->wait(); 163 250 b = (struct dualshockButtons_s *)shmem->read(); … … 166 253 167 254 if(!b->available) 168 std::cerr << "Waiting for remote...\n";255 fprintf(stderr, "Waiting for remote...\n"); 169 256 } 170 257 } … … 173 260 void PS3remote::stop() 174 261 { 262 timer_->stop(); 175 263 quit = 1; 176 264 } -
trunk/Wifibot/ps3remote.h
r21 r22 8 8 // version: $Id: $ 9 9 // 10 // purpose: Read the dualshock 3 button using bluetooth10 // purpose: Read the dualshock 3 buttons using bluetooth 11 11 // 12 12 *********************************************************************/ … … 17 17 # include <QApplication> 18 18 # include <QThread> 19 # include <QTimer> 19 20 20 21 # include "../Dualshock/controller.h" 21 # include "../../../pacpusbase/PacpusSocket/PacpusSocket.h pp"22 # include "../../../pacpusbase/PacpusSocket/PacpusSocket.h" 22 23 23 24 namespace pacpus { … … 32 33 void run(); 33 34 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 38 42 private: 39 43 ShMem *shmem; … … 42 46 int quit; 43 47 PacpusSocket * socket_; 48 QTimer * timer_; 49 char buf[4]; 50 51 public Q_SLOTS: 52 void wifibotRemanentSending(); 44 53 }; 45 54 -
trunk/Wifibot/wifibot.cpp
r21 r22 8 8 // version: $Id: $ 9 9 // 10 // purpose: 10 // purpose: Control the wifibot with the dualshock 3 11 11 // 12 12 *********************************************************************/ … … 67 67 { 68 68 dualshock = new PS3remote(pacpusSocket_); 69 70 69 dualshock->start(); 71 70 } -
trunk/Wifibot/wifibot.h
r21 r22 8 8 // version: $Id: $ 9 9 // 10 // purpose: 10 // purpose: Control the wifibot with the dualshock 3 11 11 // 12 12 *********************************************************************/
Note:
See TracChangeset
for help on using the changeset viewer.