From: konolige <kko...@us...> - 2007-02-11 19:31:22
|
Update of /cvsroot/playerstage/code/player/server/drivers/mixed/erratic In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7364/server/drivers/mixed/erratic Modified Files: Tag: release-2-0-patches erratic.cc erratic.h Log Message: Index: erratic.cc =================================================================== RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.cc,v retrieving revision 1.1.2.15 retrieving revision 1.1.2.16 diff -C2 -d -r1.1.2.15 -r1.1.2.16 *** erratic.cc 5 Feb 2007 05:08:02 -0000 1.1.2.15 --- erratic.cc 12 Feb 2007 03:31:21 -0000 1.1.2.16 *************** *** 224,227 **** --- 224,228 ---- memset(&this->sonar_id, 0, sizeof(player_devaddr_t)); memset(&this->ptz_id, 0, sizeof(player_devaddr_t)); + memset(&this->ptz2_id, 0, sizeof(player_devaddr_t)); memset(&this->last_position_cmd, 0, sizeof(player_position2d_cmd_vel_t)); *************** *** 232,235 **** --- 233,237 ---- this->sonar_subscriptions = 0; this->ptz_subscriptions = 0; + this->ptz2_subscriptions = 0; // intialise members *************** *** 285,288 **** --- 287,298 ---- } + // Do we create the second ptz interface? + if(cf->ReadDeviceAddr(&(this->ptz2_id), section, "provides", PLAYER_PTZ_CODE, -1, NULL) == 0) { + if(this->AddInterface(this->ptz2_id) != 0) { + this->SetError(-1); + return; + } + } + // build the table of robot parameters. initialize_robot_params(); *************** *** 773,776 **** --- 783,789 ---- if(Device::MatchDeviceAddress(id, this->ptz_id)) this->ptz_subscriptions++; + + if(Device::MatchDeviceAddress(id, this->ptz2_id)) + this->ptz2_subscriptions++; } *************** *** 801,804 **** --- 814,821 ---- if(Device::MatchDeviceAddress(id, this->ptz_id)) this->ptz_subscriptions--; + + if(Device::MatchDeviceAddress(id, this->ptz2_id)) + this->ptz2_subscriptions--; + } *************** *** 1614,1618 **** // Process PTZ command // Zoom is not used ! // Pan is on servo 0, tilt on servo 1 // Commands are in degrees, positive and negative // Conversions are handled through the Erratic parameter structure, --- 1631,1639 ---- // Process PTZ command // Zoom is not used ! // For PTZ #0: ! // Pan is on servo 1, tilt on servo 2 ! // For PTZ #1: ! // Pan is on servo 0 ! // // Commands are in degrees, positive and negative // Conversions are handled through the Erratic parameter structure, *************** *** 1629,1635 **** void ! Erratic::HandlePtzCommand(player_ptz_cmd_t cmd) { ! int pan, tilt; unsigned char payload[6]; ErraticPacket *packet; --- 1650,1656 ---- void ! Erratic::HandlePtzCommand(player_ptz_cmd_t cmd, player_devaddr_t id) { ! int pan, tilt, servo; unsigned char payload[6]; ErraticPacket *packet; *************** *** 1644,1657 **** // printf("Send command to servo %d: %d / %d\n", 0, (int)(RTOD(cmd.pan)), pan); ! ! packet = new ErraticPacket(); ! payload[0] = (command_e)servo_pos; ! payload[1] = (argtype_e)argstr; ! payload[2] = 3; // 3 bytes in string ! payload[3] = 2; // servo #2 ! payload[4] = pan&0xff; ! payload[5] = (pan&0xff00)>>8; ! packet->Build(payload, 6); ! this->Send(packet); // send tilt command --- 1665,1680 ---- // printf("Send command to servo %d: %d / %d\n", 0, (int)(RTOD(cmd.pan)), pan); ! if (Device::MatchDeviceAddress(id,this->ptz_id)) // 1st pan/tilt unit ! { ! packet = new ErraticPacket(); ! payload[0] = (command_e)servo_pos; ! payload[1] = (argtype_e)argstr; ! payload[2] = 3; // 3 bytes in string ! payload[3] = 2; // servo #2 ! payload[4] = pan&0xff; ! payload[5] = (pan&0xff00)>>8; ! packet->Build(payload, 6); ! this->Send(packet); ! } // send tilt command *************** *** 1664,1673 **** // printf("Send command to servo %d: %d / %d\n", 1, (int)(RTOD(cmd.tilt)), tilt); packet = new ErraticPacket(); payload[0] = (command_e)servo_pos; payload[1] = (argtype_e)argstr; ! payload[2] = 3; // 3 bytes in string ! payload[3] = 1; // servo #1 payload[4] = tilt&0xff; payload[5] = (tilt&0xff00)>>8; --- 1687,1700 ---- // printf("Send command to servo %d: %d / %d\n", 1, (int)(RTOD(cmd.tilt)), tilt); + if (Device::MatchDeviceAddress(id,this->ptz_id)) + servo = 1; + else if (Device::MatchDeviceAddress(id,this->ptz2_id)) + servo = 0; packet = new ErraticPacket(); payload[0] = (command_e)servo_pos; payload[1] = (argtype_e)argstr; ! payload[2] = 3; // 3 bytes in string ! payload[3] = servo; // servo number payload[4] = tilt&0xff; payload[5] = (tilt&0xff00)>>8; *************** *** 1694,1698 **** { player_ptz_cmd_t ptz_cmd = *(player_ptz_cmd_t*)data; ! this->HandlePtzCommand(ptz_cmd); } else --- 1721,1730 ---- { player_ptz_cmd_t ptz_cmd = *(player_ptz_cmd_t*)data; ! this->HandlePtzCommand(ptz_cmd, this->ptz_id); ! } ! else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_PTZ_CMD_STATE, this->ptz2_id)) ! { ! player_ptz_cmd_t ptz_cmd = *(player_ptz_cmd_t*)data; ! this->HandlePtzCommand(ptz_cmd, this->ptz2_id); } else Index: erratic.h =================================================================== RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.h,v retrieving revision 1.1.2.9 retrieving revision 1.1.2.10 diff -C2 -d -r1.1.2.9 -r1.1.2.10 *** erratic.h 4 Feb 2007 05:54:31 -0000 1.1.2.9 --- erratic.h 12 Feb 2007 03:31:21 -0000 1.1.2.10 *************** *** 142,146 **** player_devaddr_t ir_id; player_devaddr_t sonar_id; ! player_devaddr_t ptz_id; int position_subscriptions; --- 142,146 ---- player_devaddr_t ir_id; player_devaddr_t sonar_id; ! player_devaddr_t ptz_id, ptz2_id; int position_subscriptions; *************** *** 148,151 **** --- 148,152 ---- int sonar_subscriptions; int ptz_subscriptions; + int ptz2_subscriptions; //ErraticMotorPacket* sippacket; *************** *** 166,170 **** void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd); void HandleCarCommand(player_position2d_cmd_car_t position_cmd); ! void HandlePtzCommand(player_ptz_cmd_t ptz_cmd); void PublishAllData(); --- 167,171 ---- void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd); void HandleCarCommand(player_position2d_cmd_car_t position_cmd); ! void HandlePtzCommand(player_ptz_cmd_t ptz_cmd, player_devaddr_t id); void PublishAllData(); |