source: pacpussensors/trunk/Wifibot/ps3remote.cpp@ 24

Last change on this file since 24 was 24, checked in by phudelai, 11 years ago

maj
File size: 5.2 KB
RevLine 
[21]1/*********************************************************************
2// created: 2012/03/01 - 14:06
3// filename: PS3remote.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
14#include "ps3remote.h"
15
16using namespace pacpus;
17
18
19PS3remote::PS3remote()
20{
21 quit = 0;
22 b = NULL;
[22]23
[21]24 shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
[22]25
26 char buf[4] = {1, 1, 1, 1};
27
28 timer_ = new QTimer();
29 connect(timer_, SIGNAL(timeout()), this, SLOT(wifibotRemanentSending()));
[21]30}
31
32
33PS3remote::PS3remote(PacpusSocket * socket)
34{
35 quit = 0;
36 b = NULL;
[22]37
[21]38 shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
[22]39
[21]40 socket_ = socket;
[22]41
42 char buf[4] = {1, 1, 1, 1};
43
44 timer_ = new QTimer();
45 connect(timer_, SIGNAL(timeout()), this, SLOT(wifibotRemanentSending()));
[21]46}
47
48
49PS3remote::~PS3remote()
50{
51}
52
53
[22]54void PS3remote::setCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF)
[21]55{
56 float right, left;
57 static char lastLeft = 1;
58 static char lastRight = 1;
[22]59
[21]60 buf[1] = leftF;
61 buf[3] = rightF;
62
63 left = ((float)Ls / 127) * 100;
64 right = ((float)Rs / 127) * 100;
[22]65
[21]66 if(left > 100.0)
67 left = 100.0;
[22]68
[21]69 if(right > 100.0)
70 right = 100.0;
[22]71
[21]72 buf[0] = (char)left + 1;
73 buf[2] = (char)right + 1;
[22]74
[21]75 if(buf[0] != lastLeft || buf[2] != lastRight) {
76 fprintf(stderr, "left = %c%d right = %c%d\n", buf[1] == 1 ? '+' : '-', buf[0], buf[3] == 1 ? '+' : '-', buf[2]);
[22]77 sendCmd();
78 }
[21]79
80 lastLeft = buf[0];
81 lastRight = buf[2];
82}
83
84
[22]85void PS3remote::sendCmd()
[21]86{
[22]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()
96{
[21]97 uint16_t leftPWM = b->buttons.stick.leftStick_y;
98 uint16_t rightPWM = b->buttons.stick.rightStick_y;
99 int leftF = 1, rightF = 1;
100
[22]101 if(leftPWM < 135 && leftPWM > 120) {
[21]102 leftPWM = 0;
[22]103 } else {
[21]104 if(leftPWM > 128) {
105 leftPWM -= 128;
106 leftF = -1;
[22]107 } else {
[21]108 leftPWM = 128 - leftPWM;
[22]109 }
[21]110 }
[22]111
112 if(rightPWM < 135 && rightPWM > 120) {
[21]113 rightPWM = 0;
[22]114 } else {
[21]115 if(rightPWM > 128) {
116 rightF = -1;
117 rightPWM -= 128;
[22]118 } else {
[21]119 rightPWM = 128 - rightPWM;
[22]120 }
[21]121 }
[22]122
[21]123 if(leftPWM > 127)
124 leftPWM = 126;
[22]125
[21]126 if(rightPWM > 127)
127 rightPWM = 126;
[22]128
129 setCmd(leftPWM, rightPWM, leftF, rightF);
[21]130}
131
132
[22]133void PS3remote::modePS3()
[21]134{
[22]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()
171{
[21]172 uint16_t speed = b->buttons.stick.leftStick_y;
173 float x = (float)b->buttons.stick.rightStick_x;
174 int leftF = 1, rightF = 1;
[22]175
[21]176 if(speed < 140 && speed > 115) {
177 if(x > 140) {
178 x = x - 128;
179 rightF = -(leftF = 1);
[22]180 } else if (x < 115) {
[21]181 x = 128 - x;
182 leftF = -(rightF = 1);
[22]183 } else {
[21]184 x = 0;
[22]185 }
186
187 setCmd(x, x, leftF, rightF);
188 } else {
[21]189 if(speed > 128) {
190 rightF = leftF = -1;
191 speed -= 128;
[22]192 } else {
[21]193 speed = 128 - speed;
[22]194 }
195
[21]196 float right = 1.0 - (x > 140 ? 0.0 : ((128.0 - x) / 128.0));
197 float left = 1.0 - (x < 115 ? 0.0 : ((x - 128.0) / 128.0));
[22]198
199 setCmd(right * speed, left * speed, leftF, rightF);
[21]200 }
201}
202
203
204void PS3remote::checkMode()
205{
206 static int lastState = 0;
[22]207 static int mode = 0;
[21]208
209 if(b->buttons.digital.select) {
210 if(!lastState) {
[22]211 //mode = mode == 1 ? 2 : 1;
[21]212 // TODO later
213 //blueshock_setLeds(0, mode);
[22]214 mode = (mode + 1) % 3;
215 lastState = 1;
[21]216 }
[22]217 } else {
218 lastState = 0;
[21]219 }
220
[22]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 }
[21]235}
236
237
[22]238void PS3remote::wifibotRemanentSending()
239{
240 sendCmd();
241 fprintf(stderr, "Remanence\n");
242 timer_->start(500);
243}
244
[24]245
[21]246void PS3remote::run()
247{
248 while(!quit) {
[22]249
[21]250 shmem->wait();
251 b = (struct dualshockButtons_s *)shmem->read();
252
253 checkMode();
254
255 if(!b->available)
[22]256 fprintf(stderr, "Waiting for remote...\n");
[21]257 }
258}
259
260
261void PS3remote::stop()
262{
[22]263 timer_->stop();
[21]264 quit = 1;
265}
Note: See TracBrowser for help on using the repository browser.