Update of /cvsroot/playerstage/code/player/server/drivers/mixed/erratic
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv5954/server/drivers/mixed/erratic
Modified Files:
Tag: release-2-0-patches
erratic.cc erratic.h robot_params.cc robot_params.h
Log Message:
Index: robot_params.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/robot_params.cc,v
retrieving revision 1.1.2.4
retrieving revision 1.1.2.5
diff -C2 -d -r1.1.2.4 -r1.1.2.5
*** robot_params.cc 19 Dec 2006 18:55:56 -0000 1.1.2.4
--- robot_params.cc 26 Dec 2006 19:36:19 -0000 1.1.2.5
***************
*** 81,88 ****
{ 0.190, 0.65, DTOR(-53) },
{ 0.170, 0.45, DTOR(-24) },
- { 0.170, -0.45, DTOR( 24) },
- { 0.190, -0.65, DTOR( 53) },
- { 0.200, -0.85, DTOR( 0) },
{ 0.180,-0.110, DTOR(-90) },
},
2,
--- 81,88 ----
{ 0.190, 0.65, DTOR(-53) },
{ 0.170, 0.45, DTOR(-24) },
{ 0.180,-0.110, DTOR(-90) },
+ { 0.200, -0.85, DTOR( 0) },
+ { 0.190, -0.65, DTOR( 53) },
+ { 0.170, -0.45, DTOR( 24) },
},
2,
Index: robot_params.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/robot_params.h,v
retrieving revision 1.1.2.4
retrieving revision 1.1.2.5
diff -C2 -d -r1.1.2.4 -r1.1.2.5
*** robot_params.h 19 Dec 2006 18:55:56 -0000 1.1.2.4
--- robot_params.h 26 Dec 2006 19:36:19 -0000 1.1.2.5
***************
*** 35,46 ****
typedef struct
{
- double x;
- double y;
- double th;
- } sonar_pose_t;
-
-
- typedef struct
- {
double AngleConvFactor; //
char* Class;
--- 35,38 ----
***************
*** 90,94 ****
double VelConvFactor; //
int NumSonars;
! sonar_pose_t sonar_pose[32];
int NumIR;
player_pose_t IRPose[8];
--- 82,86 ----
double VelConvFactor; //
int NumSonars;
! player_pose_t sonar_pose[32];
int NumIR;
player_pose_t IRPose[8];
Index: erratic.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.cc,v
retrieving revision 1.1.2.8
retrieving revision 1.1.2.9
diff -C2 -d -r1.1.2.8 -r1.1.2.9
*** erratic.cc 25 Dec 2006 01:14:11 -0000 1.1.2.8
--- erratic.cc 26 Dec 2006 19:36:19 -0000 1.1.2.9
***************
*** 217,220 ****
--- 217,221 ----
this->position_subscriptions = 0;
this->aio_ir_subscriptions = 0;
+ this->sonar_subscriptions = 0;
// intialise members
***************
*** 254,257 ****
--- 255,265 ----
}
+ // Do we create a sonar interface?
+ if(cf->ReadDeviceAddr(&(this->sonar_id), section, "provides",
PLAYER_SONAR_CODE, -1, NULL) == 0) {
+ if(this->AddInterface(this->sonar_id) != 0) {
+ this->SetError(-1);
+ return;
+ }
+ }
// build the table of robot parameters.
initialize_robot_params();
***************
*** 734,737 ****
--- 742,748 ----
if(Device::MatchDeviceAddress(id, this->ir_id))
this->aio_ir_subscriptions++;
+
+ if(Device::MatchDeviceAddress(id, this->sonar_id))
+ this->sonar_subscriptions++;
}
***************
*** 756,759 ****
--- 767,773 ----
if(Device::MatchDeviceAddress(id, this->ir_id))
this->aio_ir_subscriptions--;
+
+ if(Device::MatchDeviceAddress(id, this->sonar_id))
+ this->sonar_subscriptions--;
}
***************
*** 767,771 ****
// Listens to the robot
! void Erratic::ReceiveThread() {
for (;;) {
pthread_testcancel();
--- 781,786 ----
// Listens to the robot
! void Erratic::ReceiveThread()
! {
for (;;) {
pthread_testcancel();
***************
*** 792,795 ****
--- 807,811 ----
//Lock();
+ int count;
switch(packet.packet[3])
{
***************
*** 844,847 ****
--- 860,881 ----
break;
+ case (reply_e)sonar:
+ // Sonar packet
+ count = RobotParams[this->param_idx]->NumSonars;
+ erratic_data.sonar.ranges_count = count;
+ for (int i = 0; i < count; i++)
+ {
+ if (i >=
PLAYER_SONAR_MAX_SAMPLES)
+ continue;
+ int ch = packet.packet[5+i*3];
// channel number
+ if (ch >= count)
+ continue;
// bad channel number
+ erratic_data.sonar.ranges[ch] =
.001 * (double)(packet.packet[6+i*3]
+
+ 256*packet.packet[7+i*3]);
+ }
+
+ PublishSonar();
+ break;
+
case (reply_e)debug:
if (debug_mode) {
***************
*** 972,975 ****
--- 1006,1024 ----
}
+ // Enable or disable sonar reporting
+ void Erratic::ToggleSonar(unsigned char val)
+ {
+ unsigned char command[4];
+ ErraticPacket *packet = new ErraticPacket();
+
+ command[0] = (command_e)set_sonar;
+ command[1] = (argtype_e)argint;
+ command[2] = val ? 1 : 0;
+ command[3] = 0;
+ packet->Build(command, 4);
+
+ Send(packet);
+ }
+
// This describes the IR hardware
float Erratic::IRRangeFromVoltage(float voltage)
***************
*** 995,998 ****
--- 1044,1048 ----
int last_position_subscrcount=0;
int last_aio_ir_subscriptions=0;
+ int last_sonar_subscriptions=0;
for(;;)
***************
*** 1031,1034 ****
--- 1081,1093 ----
+ // We'll ask the robot to enable sonar packets if we just got
our
+ // first subscriber
+ if(!last_sonar_subscriptions && this->sonar_subscriptions)
+ this->ToggleSonar(1);
+ else if(last_sonar_subscriptions &&
!(this->sonar_subscriptions))
+ this->ToggleSonar(0);
+ last_sonar_subscriptions = this->sonar_subscriptions;
+
+
this->Unlock();
***************
*** 1086,1089 ****
--- 1145,1156 ----
NULL);
}
+ void Erratic::PublishSonar() {
+ this->Publish(this->sonar_id, NULL,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_SONAR_DATA_RANGES,
+ (void*)&(this->erratic_data.sonar),
+ sizeof(player_sonar_data_t),
+ NULL);
+ }
void Erratic::PublishAllData() {
this->PublishPosition2D();
***************
*** 1091,1094 ****
--- 1158,1162 ----
this->PublishAIn();
this->PublishIR();
+ this->PublishSonar();
}
***************
*** 1230,1233 ****
--- 1298,1316 ----
return(0);
}
+ // Request for getting sonar locations
+ else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
+ PLAYER_SONAR_DATA_GEOM,
+ this->sonar_id)) {
+ player_sonar_geom_t pose;
+ pose.poses_count = RobotParams[param_idx]->NumSonars;
+ for (uint16_t i = 0; i < pose.poses_count ;i++)
+ pose.poses[i] = RobotParams[param_idx]->sonar_pose[i];
+
+ this->Publish(this->sonar_id, resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_SONAR_DATA_GEOM,
+ (void*)&pose, sizeof(pose), NULL);
+ return(0);
+ }
else
{
Index: erratic.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.h,v
retrieving revision 1.1.2.7
retrieving revision 1.1.2.8
diff -C2 -d -r1.1.2.7 -r1.1.2.8
*** erratic.h 19 Dec 2006 19:19:55 -0000 1.1.2.7
--- erratic.h 26 Dec 2006 19:36:19 -0000 1.1.2.8
***************
*** 76,79 ****
--- 76,80 ----
rot_vel = 21, // deg/s
set_max_rot_acc = 23,
+ set_sonar = 28,
stop = 29,
wheel_vel = 32, // mm/s
***************
*** 106,110 ****
motor = 0x80,
encoder = 0x90,
! ain = 0x9a
} reply_e;
--- 107,112 ----
motor = 0x80,
encoder = 0x90,
! ain = 0x9a,
! sonar = 0x9b
} reply_e;
***************
*** 118,121 ****
--- 120,124 ----
player_aio_data_t aio;
player_ir_data ir;
+ player_sonar_data sonar;
} __attribute__ ((packed)) player_erratic_data_t;
***************
*** 137,143 ****
--- 140,148 ----
player_devaddr_t aio_id;
player_devaddr_t ir_id;
+ player_devaddr_t sonar_id;
int position_subscriptions;
int aio_ir_subscriptions;
+ int sonar_subscriptions;
//ErraticMotorPacket* sippacket;
***************
*** 152,155 ****
--- 157,161 ----
void ToggleAIn(unsigned char val);
+ void ToggleSonar(unsigned char val);
int HandleConfig(MessageQueue* resp_queue, player_msghdr * hdr, void* data);
***************
*** 163,166 ****
--- 169,173 ----
void PublishAIn();
void PublishIR();
+ void PublishSonar();
float IRRangeFromVoltage(float voltage);
-------------------------------------------------------------------------
Take Surveys. Earn Cash. Influence the Future of IT
Join SourceForge.net's Techsay panel and you'll get the chance to share your
opinions on IT & business topics through brief surveys - and earn cash
http://www.techsay.com/default.php?page=join.php&p=sourceforge&CID=DEVDEV
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
