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

Last change on this file since 123 was 99, checked in by nguyenhu, 9 years ago

compilation under linux with 0.2.X framework

File size: 5.2 KB
Line 
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;
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()));
30}
31
32
33PS3remote::PS3remote(PacpusUDPSocket * socket)
34{
35 quit = 0;
36 b = NULL;
37
38 shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
39
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()));
46}
47
48
49PS3remote::~PS3remote()
50{
51}
52
53
54void PS3remote::setCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF)
55{
56 float right, left;
57 static char lastLeft = 1;
58 static char lastRight = 1;
59
60 buf[1] = leftF;
61 buf[3] = rightF;
62
63 left = ((float)Ls / 127) * 100;
64 right = ((float)Rs / 127) * 100;
65
66 if(left > 100.0)
67 left = 100.0;
68
69 if(right > 100.0)
70 right = 100.0;
71
72 buf[0] = (char)left + 1;
73 buf[2] = (char)right + 1;
74
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]);
77 sendCmd();
78 }
79
80 lastLeft = buf[0];
81 lastRight = buf[2];
82}
83
84
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()
96{
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
101 if(leftPWM < 135 && leftPWM > 120) {
102 leftPWM = 0;
103 } else {
104 if(leftPWM > 128) {
105 leftPWM -= 128;
106 leftF = -1;
107 } else {
108 leftPWM = 128 - leftPWM;
109 }
110 }
111
112 if(rightPWM < 135 && rightPWM > 120) {
113 rightPWM = 0;
114 } else {
115 if(rightPWM > 128) {
116 rightF = -1;
117 rightPWM -= 128;
118 } else {
119 rightPWM = 128 - rightPWM;
120 }
121 }
122
123 if(leftPWM > 127)
124 leftPWM = 126;
125
126 if(rightPWM > 127)
127 rightPWM = 126;
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()
171{
172 uint16_t speed = b->buttons.stick.leftStick_y;
173 float x = (float)b->buttons.stick.rightStick_x;
174 int leftF = 1, rightF = 1;
175
176 if(speed < 140 && speed > 115) {
177 if(x > 140) {
178 x = x - 128;
179 rightF = -(leftF = 1);
180 } else if (x < 115) {
181 x = 128 - x;
182 leftF = -(rightF = 1);
183 } else {
184 x = 0;
185 }
186
187 setCmd(x, x, leftF, rightF);
188 } else {
189 if(speed > 128) {
190 rightF = leftF = -1;
191 speed -= 128;
192 } else {
193 speed = 128 - speed;
194 }
195
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));
198
199 setCmd(right * speed, left * speed, leftF, rightF);
200 }
201}
202
203
204void PS3remote::checkMode()
205{
206 static int lastState = 0;
207 static int mode = 0;
208
209 if(b->buttons.digital.select) {
210 if(!lastState) {
211 //mode = mode == 1 ? 2 : 1;
212 // TODO later
213 //blueshock_setLeds(0, mode);
214 mode = (mode + 1) % 3;
215 lastState = 1;
216 }
217 } else {
218 lastState = 0;
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}
244
245
246void PS3remote::run()
247{
248 while(!quit) {
249
250 shmem->wait();
251 b = (struct dualshockButtons_s *)shmem->read();
252
253 checkMode();
254
255 if(!b->available)
256 fprintf(stderr, "Waiting for remote...\n");
257 }
258}
259
260
261void PS3remote::stop()
262{
263 timer_->stop();
264 quit = 1;
265}
Note: See TracBrowser for help on using the repository browser.