segway_RMP400.cpp 10.3 KB
Newer Older
Guillem Alenyà's avatar
Guillem Alenyà committed
1
2
#include "segway_RMP400.h"
#include "segway_RMP400_exception.h"
3
#include "segway_rmp200_exceptions.h"
Guillem Alenyà's avatar
Guillem Alenyà committed
4
5
6
#include <cmath>

CSegwayRMP400::CSegwayRMP400() :
7
8
9
  forward_displacement(0.0),
  yaw_displacement(0.0),
  yaw_rate(0.0)
Guillem Alenyà's avatar
Guillem Alenyà committed
10
{
11
  this->connected=false;
12
  this->segways.resize(NUM_SEGWAY_200);
Guillem Alenyà's avatar
Guillem Alenyà committed
13
14
}

15
16
17
18
CSegwayRMP400::CSegwayRMP400(const std::string &front, const std::string &rear) :
  forward_displacement(0.0),
  yaw_displacement(0.0),
  yaw_rate(0.0)
Guillem Alenyà's avatar
Guillem Alenyà committed
19
{
20
  this->connected=false;
21
  this->segways.resize(NUM_SEGWAY_200);
22
  this->connect(front,rear);
Guillem Alenyà's avatar
Guillem Alenyà committed
23
24
}

25
void *CSegwayRMP400::monitor_thread(void *param)
Guillem Alenyà's avatar
Guillem Alenyà committed
26
{
27
  CSegwayRMP400 *segway=(CSegwayRMP400 *)param;
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
  std::vector<bool> status_received(false,NUM_SEGWAY_200);
  double fwd_displ_front,fwd_displ_rear; 
  std::list<std::string> events;
  int event_id=0;
  bool end=false;

  events.push_back(segway->finish_thread_event_id);
  events.push_back(segway->segways[front_segway].get_new_status_event());
  events.push_back(segway->segways[rear_segway].get_new_status_event());
  while(!end)
  {
    event_id=segway->event_server->wait_first(events);
    segway->status_access.enter();
    try{
      if(event_id==0)
        end=true;
      else if(event_id==1)// front status
      {
        status_received[front_segway]=true;
        segway->status.rmp200[front_segway]=segway->segways[front_segway].get_status();
      }
      else if(event_id==2)// rear status
      {
        status_received[rear_segway]=true;
        segway->status.rmp200[rear_segway]=segway->segways[rear_segway].get_status();
      }
      if(status_received[front_segway] && status_received[rear_segway])
      {
        /* compute the forward displacement  */
        fwd_displ_front=(segway->segways[front_segway].get_right_wheel_displacement()+segway->segways[front_segway].get_left_wheel_displacement())/2;
        fwd_displ_rear=(segway->segways[rear_segway].get_right_wheel_displacement()+segway->segways[rear_segway].get_left_wheel_displacement())/2;
        segway->forward_displacement=(fwd_displ_front+fwd_displ_rear)/2;
        /* compute yaw displacement*/
        segway->yaw_displacement=(segway->segways[front_segway].get_yaw_displacement()+segway->segways[rear_segway].get_yaw_displacement())/2;
        /* compute yaw rate */
        segway->yaw_rate=(segway->segways[front_segway].get_yaw_rate()+segway->segways[rear_segway].get_yaw_rate())/2;
        /* activate event */
        if(!segway->event_server->event_is_set(segway->new_status_event_id))
          segway->event_server->set_event(segway->new_status_event_id);
        status_received[front_segway]=false;
        status_received[rear_segway]=false;
      }
    }catch(CException &e){
      std::cout << e.what() << std::endl;
    }
    segway->status_access.exit();
  }
Guillem Alenyà's avatar
Guillem Alenyà committed
75

76
  pthread_exit(NULL);
Guillem Alenyà's avatar
Guillem Alenyà committed
77
78
}

79
void CSegwayRMP400::init_events_threads(void)
80
{
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
  std::string id="segway_"+this->front_serial+"_"+this->rear_serial;

  /* initialize threads */
  this->thread_server=CThreadServer::instance();
  this->monitor_thread_id=id+"_monitor_thread";
  this->thread_server->create_thread(this->monitor_thread_id);
  this->thread_server->attach_thread(this->monitor_thread_id,this->monitor_thread,this);
  /* initialize events */
  this->event_server=CEventServer::instance();
  this->finish_thread_event_id=id+"_finish_thread_event";
  this->event_server->create_event(this->finish_thread_event_id);
  this->front_cable_disconnect_event_id=id+"_front_disconnect_event";
  this->event_server->create_event(this->front_cable_disconnect_event_id);
  this->rear_cable_disconnect_event_id=id+"_rear_disconnect_event";
  this->event_server->create_event(this->rear_cable_disconnect_event_id);
  this->front_power_off_event_id=id+"_front_power_off_event";
  this->event_server->create_event(this->front_power_off_event_id);
  this->rear_power_off_event_id=id+"_rear_power_off_event";
  this->event_server->create_event(this->rear_power_off_event_id);
  this->front_no_heartbeat_event_id=id+"_front_no_heartbeat_event";
  this->event_server->create_event(this->front_no_heartbeat_event_id);
  this->rear_no_heartbeat_event_id=id+"_rear_no_heartbeat_event";
  this->event_server->create_event(this->rear_no_heartbeat_event_id);
  this->new_status_event_id=id+"_new_status_event";
  this->event_server->create_event(this->new_status_event_id);
  // start the internal thread
  this->thread_server->start_thread(this->monitor_thread_id);
108
109
}

110
void CSegwayRMP400::connect(const std::string &front, const std::string &rear)
Guillem Alenyà's avatar
Guillem Alenyà committed
111
{
112
  if(!this->connected)
Guillem Alenyà's avatar
Guillem Alenyà committed
113
  {
114
115
    if(front==rear)
      throw CSegwayRMP400Exception(_HERE_,"Rear and front serial number can't be the same",front);
Guillem Alenyà's avatar
Guillem Alenyà committed
116
117
    else
    {
118
119
120
121
122
123
124
125
126
127
128
      try{
        this->segways[front_segway].connect(front);
        this->front_serial=front;
        this->segways[rear_segway].connect(rear);
        this->rear_serial=rear;
        this->init_events_threads();
        this->connected=true;
      }catch(CException &e){
        this->close();
        throw e;
      }
Guillem Alenyà's avatar
Guillem Alenyà committed
129
    }
130
131
132
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is already connected",front+"_"+rear);
Guillem Alenyà's avatar
Guillem Alenyà committed
133
134
}

135
void CSegwayRMP400::close()
Guillem Alenyà's avatar
Guillem Alenyà committed
136
{
137
  // signal the thread to end
138
  if(this->monitor_thread_id.size()>0)
139
  {
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
    if(this->thread_server->get_thread_state(this->monitor_thread_id)==starting ||
       this->thread_server->get_thread_state(this->monitor_thread_id)==active)
    {
      this->event_server->set_event(this->finish_thread_event_id);
      this->thread_server->end_thread(this->monitor_thread_id);
      this->thread_server->detach_thread(this->monitor_thread_id);
      this->thread_server->delete_thread(this->monitor_thread_id);
      this->monitor_thread_id.clear();
      this->event_server->delete_event(this->finish_thread_event_id);
      this->finish_thread_event_id.clear();
      this->event_server->delete_event(this->front_cable_disconnect_event_id);
      this->front_cable_disconnect_event_id.clear();
      this->event_server->delete_event(this->rear_cable_disconnect_event_id);
      this->rear_cable_disconnect_event_id.clear();
      this->event_server->delete_event(this->front_power_off_event_id);
      this->front_power_off_event_id.clear();
      this->event_server->delete_event(this->rear_power_off_event_id);
      this->rear_power_off_event_id.clear();
      this->event_server->delete_event(this->front_no_heartbeat_event_id);
      this->front_no_heartbeat_event_id.clear();
      this->event_server->delete_event(this->rear_no_heartbeat_event_id);
      this->rear_no_heartbeat_event_id.clear();
      this->event_server->delete_event(this->new_status_event_id);
      this->new_status_event_id.clear();
    }
165
166
167
168
169
170
171
172
173
174
175
176
  }
  if(this->front_serial.size()>0) 
  {
    this->segways[front_segway].close();
    this->front_serial.clear();
  }
  if(this->rear_serial.size()>0)
  {
    this->segways[rear_segway].close();
    this->rear_serial.clear();
  }
  this->connected=false;
Guillem Alenyà's avatar
Guillem Alenyà committed
177
178
}

179
std::ostream & operator<< (std::ostream& data, CSegwayRMP400 & segway)
Guillem Alenyà's avatar
Guillem Alenyà committed
180
{
181
182
183
184
  data << "Front segway status data:" << std::endl;
  data << segway.segways[front_segway] << std::endl;
  data << "Rear segway status data:" << std::endl;
  data << segway.segways[rear_segway] << std::endl;
Guillem Alenyà's avatar
Guillem Alenyà committed
185

186
  return data;
187
188
}

189
TSegwayRMP400Status CSegwayRMP400::get_status()
190
{
191
192
193
194
195
196
197
198
199
200
201
202
  TSegwayRMP400Status status_tmp;

  if(this->connected)
  {
    this->status_access.enter();
    status_tmp=this->status;
    this->status_access.exit();

    return status_tmp;
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
Guillem Alenyà's avatar
Guillem Alenyà committed
203
204
}

205
double CSegwayRMP400::get_forward_displacement()
Guillem Alenyà's avatar
Guillem Alenyà committed
206
{
207
  double fwd_disp;
Guillem Alenyà's avatar
Guillem Alenyà committed
208

209
210
  if(this->connected)
  {
211
212
213
    this->status_access.enter();
    fwd_disp=this->forward_displacement;
    this->status_access.exit();
Guillem Alenyà's avatar
Guillem Alenyà committed
214

215
    return fwd_disp;
216
217
218
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
Guillem Alenyà's avatar
Guillem Alenyà committed
219
220
}

221
double CSegwayRMP400::get_yaw_displacement()
Guillem Alenyà's avatar
Guillem Alenyà committed
222
{
223
224
  double yaw_disp;

225
226
  if(this->connected)
  {
227
228
229
    this->status_access.enter();
    yaw_disp=this->yaw_displacement;
    this->status_access.exit();
Guillem Alenyà's avatar
Guillem Alenyà committed
230

231
    return yaw_disp;
232
233
234
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
Guillem Alenyà's avatar
Guillem Alenyà committed
235
236
}

237
double CSegwayRMP400::get_yaw_rate()
Guillem Alenyà's avatar
Guillem Alenyà committed
238
{
239
240
  double yaw_rate_tmp;

241
242
  if(this->connected)
  {
243
244
245
    this->status_access.enter();
    yaw_rate_tmp=this->yaw_rate;
    this->status_access.exit();
José Luis Rivero Partida's avatar
José Luis Rivero Partida committed
246

247
    return yaw_rate_tmp;
248
249
250
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
Guillem Alenyà's avatar
Guillem Alenyà committed
251
252
}

253
void CSegwayRMP400::reset_integrators()
Guillem Alenyà's avatar
Guillem Alenyà committed
254
{
255
256
257
  double fd=0.0,yd=0.0;
  unsigned int i=0;
  bool reset=false;
Guillem Alenyà's avatar
Guillem Alenyà committed
258

259
260
  if(this->connected)
  {
Guillem Alenyà's avatar
Guillem Alenyà committed
261
    do {
262
263
264
265
266
267
      for(i=0;i<this->segways.size();i++)
      {
        this->segways[i].reset_right_wheel_integrator();
        this->segways[i].reset_left_wheel_integrator();
        this->segways[i].reset_yaw_integrator();
        this->segways[i].reset_forward_integrator();
Guillem Alenyà's avatar
Guillem Alenyà committed
268
269
270
      }
      // Need to leave the segwayrmp 200 driver to read need values
      sleep(1);
271
272
273
274
275
276
277
278
279
280
      fd=this->get_forward_displacement();
      yd=this->get_yaw_displacement();
      if(std::fabs(fd)<0.1 && std::fabs(yd)<0.1)
        reset=true;
      else
        reset=false;
    }while(!reset);
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
Guillem Alenyà's avatar
Guillem Alenyà committed
281
282
}

283
std::string CSegwayRMP400::get_front_cable_disconnected_event_id(void)
Guillem Alenyà's avatar
Guillem Alenyà committed
284
{
285
286
  return this->front_cable_disconnect_event_id;
}
Guillem Alenyà's avatar
Guillem Alenyà committed
287

288
289
290
291
std::string CSegwayRMP400::get_rear_cable_disconnected_event_id(void)
{
  return this->rear_cable_disconnect_event_id;
}
Guillem Alenyà's avatar
Guillem Alenyà committed
292

293
294
295
296
std::string CSegwayRMP400::get_front_power_off_event_id(void)
{
  return this->front_power_off_event_id;
}
Guillem Alenyà's avatar
Guillem Alenyà committed
297

298
299
300
std::string CSegwayRMP400::get_rear_power_off_event_id(void)
{
  return this->rear_power_off_event_id;
Guillem Alenyà's avatar
Guillem Alenyà committed
301
302
}

303
std::string CSegwayRMP400::get_front_no_heartbeat_event_id(void)
Guillem Alenyà's avatar
Guillem Alenyà committed
304
{
305
306
  return this->front_no_heartbeat_event_id;
}
Guillem Alenyà's avatar
Guillem Alenyà committed
307

308
309
310
std::string CSegwayRMP400::get_rear_no_heartbeat_event_id(void)
{
  return this->rear_no_heartbeat_event_id;
Guillem Alenyà's avatar
Guillem Alenyà committed
311
312
}

313
std::string CSegwayRMP400::get_new_status_event_id(void)
Guillem Alenyà's avatar
Guillem Alenyà committed
314
{
315
316
  return this->new_status_event_id;
}
Guillem Alenyà's avatar
Guillem Alenyà committed
317

318
319
320
321
322
323
324
325
326
327
void CSegwayRMP400::move(double vT, double vR)
{
  if(this->connected)
  {
    this->segways[front_segway].move(vT,vR);
    this->segways[rear_segway].move(vT,vR);
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}
Guillem Alenyà's avatar
Guillem Alenyà committed
328

329
330
331
332
333
334
335
336
337
void CSegwayRMP400::stop()
{
  if(this->connected)
  {
    this->segways[front_segway].stop();
    this->segways[rear_segway].stop();
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
Guillem Alenyà's avatar
Guillem Alenyà committed
338
339
340
341
}

CSegwayRMP400::~CSegwayRMP400()
{
342
  this->close();
Guillem Alenyà's avatar
Guillem Alenyà committed
343
}