segway_RMP400.cpp 8.27 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;
Guillem Alenyà's avatar
Guillem Alenyà committed
12
13
}

14
15
16
17
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
18
{
19
20
  this->connected=false;
  this->connect(front,rear);
Guillem Alenyà's avatar
Guillem Alenyà committed
21
22
}

23
void *CSegwayRMP400::monitor_thread(void *param)
Guillem Alenyà's avatar
Guillem Alenyà committed
24
{
25
  CSegwayRMP400 *segway=(CSegwayRMP400 *)param;
Guillem Alenyà's avatar
Guillem Alenyà committed
26

27
  pthread_exit(NULL);
Guillem Alenyà's avatar
Guillem Alenyà committed
28
29
}

30
void CSegwayRMP400::init_events_threads(void)
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
  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);
59
60
}

61
void CSegwayRMP400::connect(const std::string &front, const std::string &rear)
Guillem Alenyà's avatar
Guillem Alenyà committed
62
{
63
  if(!this->connected)
Guillem Alenyà's avatar
Guillem Alenyà committed
64
  {
65
66
    if(front==rear)
      throw CSegwayRMP400Exception(_HERE_,"Rear and front serial number can't be the same",front);
Guillem Alenyà's avatar
Guillem Alenyà committed
67
68
    else
    {
69
70
71
72
73
74
75
76
77
78
79
      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
80
    }
81
82
83
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is already connected",front+"_"+rear);
Guillem Alenyà's avatar
Guillem Alenyà committed
84
85
}

86
void CSegwayRMP400::close()
Guillem Alenyà's avatar
Guillem Alenyà committed
87
{
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
  // signal the thread to end
  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();
  }
  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
125
126
}

127
std::ostream & operator<< (std::ostream& data, CSegwayRMP400 & segway)
Guillem Alenyà's avatar
Guillem Alenyà committed
128
{
129
130
131
132
  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
133

134
  return data;
135
136
}

137
TSegwayRMP400Status CSegwayRMP400::get_status()
138
{
139
  return this->status;
Guillem Alenyà's avatar
Guillem Alenyà committed
140
141
}

142
double CSegwayRMP400::get_forward_displacement()
Guillem Alenyà's avatar
Guillem Alenyà committed
143
{
144
  double fwd_displ_front,fwd_displ_rear; 
Guillem Alenyà's avatar
Guillem Alenyà committed
145

146
147
148
  // The get_forward_displacement function in segway RMP 200 doesn't
  // return the accumulate value of displacement, wheel_displacement
  // function do. Use them instead.
Guillem Alenyà's avatar
Guillem Alenyà committed
149

150
151
152
153
  if(this->connected)
  {
    fwd_displ_front=(this->segways[front_segway].get_right_wheel_displacement()+this->segways[front_segway].get_left_wheel_displacement())/2;
    fwd_displ_rear=(this->segways[rear_segway].get_right_wheel_displacement()+this->segways[rear_segway].get_left_wheel_displacement())/2;
Guillem Alenyà's avatar
Guillem Alenyà committed
154

155
    this->forward_displacement=(fwd_displ_front+fwd_displ_rear)/2;
Guillem Alenyà's avatar
Guillem Alenyà committed
156

157
158
159
160
    return this->forward_displacement;
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
Guillem Alenyà's avatar
Guillem Alenyà committed
161
162
}

163
double CSegwayRMP400::get_yaw_displacement()
Guillem Alenyà's avatar
Guillem Alenyà committed
164
{
165
166
167
  if(this->connected)
  {
    this->yaw_displacement=(this->segways[front_segway].get_yaw_displacement()+this->segways[rear_segway].get_yaw_displacement())/2;
Guillem Alenyà's avatar
Guillem Alenyà committed
168

169
170
171
172
    return this->yaw_displacement;
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
Guillem Alenyà's avatar
Guillem Alenyà committed
173
174
}

175
double CSegwayRMP400::get_yaw_rate()
Guillem Alenyà's avatar
Guillem Alenyà committed
176
{
177
178
179
  if(this->connected)
  {
    this->yaw_rate=(this->segways[front_segway].get_yaw_rate()+this->segways[rear_segway].get_yaw_rate())/2;
José Luis Rivero Partida's avatar
José Luis Rivero Partida committed
180

181
182
183
184
    return this->yaw_rate;
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
Guillem Alenyà's avatar
Guillem Alenyà committed
185
186
}

187
void CSegwayRMP400::reset_integrators()
Guillem Alenyà's avatar
Guillem Alenyà committed
188
{
189
190
191
  double fd=0.0,yd=0.0;
  unsigned int i=0;
  bool reset=false;
Guillem Alenyà's avatar
Guillem Alenyà committed
192

193
194
  if(this->connected)
  {
Guillem Alenyà's avatar
Guillem Alenyà committed
195
    do {
196
197
198
199
200
201
      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
202
203
204
      }
      // Need to leave the segwayrmp 200 driver to read need values
      sleep(1);
205
206
207
208
209
210
211
212
213
214
      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
215
216
}

217
std::string CSegwayRMP400::get_front_cable_disconnected_event_id(void)
Guillem Alenyà's avatar
Guillem Alenyà committed
218
{
219
220
  return this->front_cable_disconnect_event_id;
}
Guillem Alenyà's avatar
Guillem Alenyà committed
221

222
223
224
225
std::string CSegwayRMP400::get_rear_cable_disconnected_event_id(void)
{
  return this->rear_cable_disconnect_event_id;
}
Guillem Alenyà's avatar
Guillem Alenyà committed
226

227
228
229
230
std::string CSegwayRMP400::get_front_power_off_event_id(void)
{
  return this->front_power_off_event_id;
}
Guillem Alenyà's avatar
Guillem Alenyà committed
231

232
233
234
std::string CSegwayRMP400::get_rear_power_off_event_id(void)
{
  return this->rear_power_off_event_id;
Guillem Alenyà's avatar
Guillem Alenyà committed
235
236
}

237
std::string CSegwayRMP400::get_front_no_heartbeat_event_id(void)
Guillem Alenyà's avatar
Guillem Alenyà committed
238
{
239
240
  return this->front_no_heartbeat_event_id;
}
Guillem Alenyà's avatar
Guillem Alenyà committed
241

242
243
244
std::string CSegwayRMP400::get_rear_no_heartbeat_event_id(void)
{
  return this->rear_no_heartbeat_event_id;
Guillem Alenyà's avatar
Guillem Alenyà committed
245
246
}

247
std::string CSegwayRMP400::get_new_status_event_id(void)
Guillem Alenyà's avatar
Guillem Alenyà committed
248
{
249
250
  return this->new_status_event_id;
}
Guillem Alenyà's avatar
Guillem Alenyà committed
251

252
253
254
255
256
257
258
259
260
261
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
262

263
264
265
266
267
268
269
270
271
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
272
273
274
275
}

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