Update of /cvsroot/playerstage/code/player/server/drivers/actarray
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv10946/server/drivers/actarray
Modified Files:
amtecM5.cc
Log Message:
applied Toby's patch to replace fixed-size arrays
Index: amtecM5.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/actarray/amtecM5.cc,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** amtecM5.cc 23 Aug 2007 19:58:42 -0000 1.4
--- amtecM5.cc 1 Nov 2007 22:16:17 -0000 1.5
***************
*** 23,27 ****
* A driver for the Powercube Modules from Amtec Robotics, Berlin.
* This driver uses the official C++ driver from Amtec (Called Powercube
M5api).
! * You can download it from here after obtaining a password from the company:
* http://www.powercube.de/files/PCube_M5api_Linux/M5DLL_SUSE_10.0.tar
*
--- 23,27 ----
* A driver for the Powercube Modules from Amtec Robotics, Berlin.
* This driver uses the official C++ driver from Amtec (Called Powercube
M5api).
! * You can download it from here after obtaining a password from the company:
* http://www.powercube.de/files/PCube_M5api_Linux/M5DLL_SUSE_10.0.tar
*
***************
*** 89,92 ****
--- 89,93 ----
// Constructor; need that
AmtecM5(ConfigFile* cf, int section);
+ ~AmtecM5(void);
// Must implement the following methods.
***************
*** 103,107 ****
int ModuleSyncMotion(int state);
float normalize_angle(float angle);
!
//This class talks to the Bus with Powercubes (CAN or RS232 bus)
//Designed with the Factory Design Pattern:
--- 104,108 ----
int ModuleSyncMotion(int state);
float normalize_angle(float angle);
!
//This class talks to the Bus with Powercubes (CAN or RS232 bus)
//Designed with the Factory Design Pattern:
***************
*** 114,124 ****
float samplingrate;
float alarmtime;
!
//Normally this should be 1. Set to -1 if you want to invert
the direction of rotation/movement of a specific joint
int directions[PLAYER_ACTARRAY_NUM_ACTUATORS];
!
//Normally, this should be set to 0. Set to a value to add an
offset to the position sent to the modules
float offsets[PLAYER_ACTARRAY_NUM_ACTUATORS];
!
//If 1, it limits the values of the commands sent to -pi rads
and pi rads. (-180-180deg) (Safer for most rotary joints).
int normalize_angles[PLAYER_ACTARRAY_NUM_ACTUATORS];
--- 115,125 ----
float samplingrate;
float alarmtime;
!
//Normally this should be 1. Set to -1 if you want to invert
the direction of rotation/movement of a specific joint
int directions[PLAYER_ACTARRAY_NUM_ACTUATORS];
!
//Normally, this should be set to 0. Set to a value to add an
offset to the position sent to the modules
float offsets[PLAYER_ACTARRAY_NUM_ACTUATORS];
!
//If 1, it limits the values of the commands sent to -pi rads
and pi rads. (-180-180deg) (Safer for most rotary joints).
int normalize_angles[PLAYER_ACTARRAY_NUM_ACTUATORS];
***************
*** 126,136 ****
//Should we show some debug messages?
int debug_level;
!
//if true, the modules have their synchro mode activated.
bool modsynchro;
!
//0 if they are off (powercubes are all halted). 1 if they
are on.
int motor_state;
!
//Controls if we tell the scheduler to give us high priority
int highpriority;
--- 127,137 ----
//Should we show some debug messages?
int debug_level;
!
//if true, the modules have their synchro mode activated.
bool modsynchro;
!
//0 if they are off (powercubes are all halted). 1 if they
are on.
int motor_state;
!
//Controls if we tell the scheduler to give us high priority
int highpriority;
***************
*** 157,164 ****
player_actarray_data_t actArray;
player_actarray_position_cmd_t lastActArrayPosCmd;
player_actarray_home_cmd_t lastActArrayHomeCmd;
-
player_point_3d_t aaBasePos;
player_orientation_3d_t aaBaseOrient;
--- 158,166 ----
player_actarray_data_t actArray;
+ player_actarray_actuator_t *actuators;
+ player_actarray_actuatorgeom_t *actuatorsGeom;
player_actarray_position_cmd_t lastActArrayPosCmd;
player_actarray_home_cmd_t lastActArrayHomeCmd;
player_point_3d_t aaBasePos;
player_orientation_3d_t aaBaseOrient;
***************
*** 214,218 ****
debug_level = cf->ReadInt(section, "debug_level", 0);
!
//Initial Speed and Acceleration
initSpeed = cf->ReadFloat(section, "initial_ramp_speed", 0.1);
--- 216,220 ----
debug_level = cf->ReadInt(section, "debug_level", 0);
!
//Initial Speed and Acceleration
initSpeed = cf->ReadFloat(section, "initial_ramp_speed", 0.1);
***************
*** 226,230 ****
//Initialize with false (modules move as soon as they get a
command)
modsynchro=false;
!
//After power-on, the modules are ready to move.
motor_state=1;
--- 228,232 ----
//Initialize with false (modules move as soon as they get a
command)
modsynchro=false;
!
//After power-on, the modules are ready to move.
motor_state=1;
***************
*** 233,241 ****
cout << "NUM_ACTUATORS is no longer 16. Remember to fix
server/drivers/actarray/amtecM5.cc accordingly.\n";
}
!
//This needs to be changed if PLAYER_ACTARRAY_NUM_ACTUATORS changes
(now it's 16)
int
numbermatched_directions=sscanf(directionString,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",&directions[0],&directions[1],&directions[2],&directions[3],&directions[4],&directions[5],&directions[6],&directions[7],&directions[8],&directions[9],&directions[10],&directions[11],&directions[12],&directions[13],&directions[14],&directions[15]);
! cout << "Found " << numbermatched_directions << " values for the
direction vector." << endl;
!
//Checking if the values for directions are correct. Only accept 1 or
-1.
for (int i = 0 ; i != numbermatched_directions ; ++i) {
--- 235,243 ----
cout << "NUM_ACTUATORS is no longer 16. Remember to fix
server/drivers/actarray/amtecM5.cc accordingly.\n";
}
!
//This needs to be changed if PLAYER_ACTARRAY_NUM_ACTUATORS changes
(now it's 16)
int
numbermatched_directions=sscanf(directionString,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",&directions[0],&directions[1],&directions[2],&directions[3],&directions[4],&directions[5],&directions[6],&directions[7],&directions[8],&directions[9],&directions[10],&directions[11],&directions[12],&directions[13],&directions[14],&directions[15]);
! cout << "Found " << numbermatched_directions << " values for the
direction vector." << endl;
!
//Checking if the values for directions are correct. Only accept 1 or
-1.
for (int i = 0 ; i != numbermatched_directions ; ++i) {
***************
*** 248,252 ****
}
!
//Vector for the direction of the modules
const char * offsetString= cf->ReadString(section, "offsets", 0);
--- 250,254 ----
}
!
//Vector for the direction of the modules
const char * offsetString= cf->ReadString(section, "offsets", 0);
***************
*** 256,260 ****
//This needs to be changed if PLAYER_ACTARRAY_NUM_ACTUATORS changes
(now it's 16)
int
numbermatched_offsets=sscanf(offsetString,"%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",&offsets[0],&offsets[1],&offsets[2],&offsets[3],&offsets[4],&offsets[5],&offsets[6],&offsets[7],&offsets[8],&offsets[9],&offsets[10],&offsets[11],&offsets[12],&offsets[13],&offsets[14],&offsets[15]);
! cout << "Found " << numbermatched_offsets << " values for the offset
vector." << endl;
--- 258,262 ----
//This needs to be changed if PLAYER_ACTARRAY_NUM_ACTUATORS changes
(now it's 16)
int
numbermatched_offsets=sscanf(offsetString,"%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",&offsets[0],&offsets[1],&offsets[2],&offsets[3],&offsets[4],&offsets[5],&offsets[6],&offsets[7],&offsets[8],&offsets[9],&offsets[10],&offsets[11],&offsets[12],&offsets[13],&offsets[14],&offsets[15]);
! cout << "Found " << numbermatched_offsets << " values for the offset
vector." << endl;
***************
*** 266,272 ****
//This needs to be changed if PLAYER_ACTARRAY_NUM_ACTUATORS changes
(now it's 16)
int
numbermatched_normalize=sscanf(normalizeString,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",&normalize_angles[0],&normalize_angles[1],&normalize_angles[2],&normalize_angles[3],&normalize_angles[4],&normalize_angles[5],&normalize_angles[6],&normalize_angles[7],&normalize_angles[8],&normalize_angles[9],&normalize_angles[10],&normalize_angles[11],&normalize_angles[12],&normalize_angles[13],&normalize_angles[14],&normalize_angles[15]);
! cout << "Found " << numbermatched_normalize << " values for the angle
normalization vector." << endl;
-
//Ids of those modules. A mapping is done using this sequence to
actuators[0], actuators[1], ...
const char * idString= cf->ReadString(section, "module_ids", 0);
--- 268,274 ----
//This needs to be changed if PLAYER_ACTARRAY_NUM_ACTUATORS changes
(now it's 16)
int
numbermatched_normalize=sscanf(normalizeString,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",&normalize_angles[0],&normalize_angles[1],&normalize_angles[2],&normalize_angles[3],&normalize_angles[4],&normalize_angles[5],&normalize_angles[6],&normalize_angles[7],&normalize_angles[8],&normalize_angles[9],&normalize_angles[10],&normalize_angles[11],&normalize_angles[12],&normalize_angles[13],&normalize_angles[14],&normalize_angles[15]);
! cout << "Found " << numbermatched_normalize << " values for the angle
normalization vector." << endl;
!
//Ids of those modules. A mapping is done using this sequence to
actuators[0], actuators[1], ...
const char * idString= cf->ReadString(section, "module_ids", 0);
***************
*** 298,307 ****
!
//Do we tell the scheduler that we need a very high priority?
highpriority = cf->ReadInt(section, "highpriority", 0);
}
////////////////////////////////////////////////////////////////////////////////
// Set up the device. Return 0 if things go well, and -1 otherwise.
--- 300,321 ----
!
//Do we tell the scheduler that we need a very high priority?
highpriority = cf->ReadInt(section, "highpriority", 0);
+ actuators = NULL;
+ acuatorsGeom = NULL;
+ }
+
+
////////////////////////////////////////////////////////////////////////////////
+ // Destructor.
+ AmtecM5::~AmtecM5(void)
+ {
+ if (actuators != NULL) {
+ delete[] actuators;
+ }
}
+
////////////////////////////////////////////////////////////////////////////////
// Set up the device. Return 0 if things go well, and -1 otherwise.
***************
*** 465,468 ****
--- 479,494 ----
}
+ //Allocate some space to send actuator data and geometry
+ actuators = new player_actarray_actuator_t[module_count];
+ if (actuators == NULL) {
+ PLAYER_ERROR("amtecM5: Failed to allocate memory to store
actuator data");
+ return(-1);
+ }
+ actuatorsGeom = new player_actarray_actuatorgeom_t[module_count];
+ if (actuatorsGeom == NULL) {
+ PLAYER_ERROR("amtecM5: Failed to allocate memory to store
actuator geometry");
+ return(-1);
+ }
+
puts("Amtec M5 powercube driver ready.");
***************
*** 482,486 ****
//Wait a little to let everything settle
sleep(1); //We've been getting some segfaults at disconnect from the
last client without this
!
// Stop and join the driver thread
StopThread();
--- 508,512 ----
//Wait a little to let everything settle
sleep(1); //We've been getting some segfaults at disconnect from the
last client without this
!
// Stop and join the driver thread
StopThread();
***************
*** 492,495 ****
--- 518,531 ----
pclDevice=0;
+ if (actuators != NULL) {
+ delete[] actuators;
+ actuators = NULL;
+ }
+
+ if (actuatorsGeom != NULL) {
+ delete[] actuatorsGeom;
+ actuatorsGeom = NULL;
+ }
+
puts("Amtec M5 Powercube driver has been shutdown");
***************
*** 517,521 ****
bool handled(false);
!
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_ACTARRAY_REQ_POWER, device_addr)) {
//Got a Power-Request
--- 553,557 ----
bool handled(false);
!
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_ACTARRAY_REQ_POWER, device_addr)) {
//Got a Power-Request
***************
*** 526,534 ****
printf("Power-Request. Value: %d\n",value);
}
!
//Since we cannot physically turn the modules off, this is
what we do:
//If power==off -> Halt all the modules
//If power==on -> Reset all the modules
!
if (value==1) {
//reset all the modules
--- 562,570 ----
printf("Power-Request. Value: %d\n",value);
}
!
//Since we cannot physically turn the modules off, this is
what we do:
//If power==off -> Halt all the modules
//If power==on -> Reset all the modules
!
if (value==1) {
//reset all the modules
***************
*** 570,573 ****
--- 606,610 ----
aaGeom.actuators_count=module_count;
+ aaGeom.actuators=actuatorsGeom;
for (unsigned int i = 0; i < module_count; i++) {
***************
*** 638,642 ****
float speed=req->speed;
! if (debug_level)
cout << "Speed_config. joint: " << joint << " speed:
" << speed << endl;
--- 675,679 ----
float speed=req->speed;
! if (debug_level)
cout << "Speed_config. joint: " << joint << " speed:
" << speed << endl;
***************
*** 658,662 ****
} else
if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,PLAYER_ACTARRAY_REQ_ACCEL,device_addr))
{
!
//printf("Accel Request. (Configuration of the acceleration
for the next movements)\n");
--- 695,699 ----
} else
if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,PLAYER_ACTARRAY_REQ_ACCEL,device_addr))
{
!
//printf("Accel Request. (Configuration of the acceleration
for the next movements)\n");
***************
*** 696,700 ****
}
!
if (handled) {
return(0);
--- 733,737 ----
}
!
if (handled) {
return(0);
***************
*** 702,706 ****
return(-1);
}
!
}
--- 739,743 ----
return(-1);
}
!
}
***************
*** 711,715 ****
bool handled(false);
!
//If the command matches any of the following, an action is taken. If
not, -1 is returned
if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,PLAYER_ACTARRAY_CMD_POS,device_addr))
{
--- 748,752 ----
bool handled(false);
!
//If the command matches any of the following, an action is taken. If
not, -1 is returned
if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,PLAYER_ACTARRAY_CMD_POS,device_addr))
{
***************
*** 717,721 ****
player_actarray_position_cmd_t * cmd =
reinterpret_cast<player_actarray_position_cmd_t*>(data);
!
int jointnumber=cmd->joint;
//The command that we send to the module is:
angleplayer/direction - offset
--- 754,758 ----
player_actarray_position_cmd_t * cmd =
reinterpret_cast<player_actarray_position_cmd_t*>(data);
!
int jointnumber=cmd->joint;
//The command that we send to the module is:
angleplayer/direction - offset
***************
*** 725,729 ****
float newposition(0.0);
!
//Do we have to limit the command to -180 and 180deg?
if (normalize_angles[jointnumber]) {
--- 762,766 ----
float newposition(0.0);
!
//Do we have to limit the command to -180 and 180deg?
if (normalize_angles[jointnumber]) {
***************
*** 732,745 ****
newposition=cmd->position/directions[jointnumber]-offsets[jointnumber];
}
!
if (debug_level) {
cout << "pos_cmd. Joint: " << cmd->joint << " pos: "
<< cmd->position << "Sent to module: " << newposition << endl;
}
!
if (can && modsynchro) {
ModuleSyncMotion(0); //Deactivate the syncmotion bit
}
!
int
error=pclDevice->moveRamp(idModuleList[jointnumber],newposition,rampSpeed[jointnumber],rampAccel[jointnumber]);
// module, pos, speed, accel
--- 769,782 ----
newposition=cmd->position/directions[jointnumber]-offsets[jointnumber];
}
!
if (debug_level) {
cout << "pos_cmd. Joint: " << cmd->joint << " pos: "
<< cmd->position << "Sent to module: " << newposition << endl;
}
!
if (can && modsynchro) {
ModuleSyncMotion(0); //Deactivate the syncmotion bit
}
!
int
error=pclDevice->moveRamp(idModuleList[jointnumber],newposition,rampSpeed[jointnumber],rampAccel[jointnumber]);
// module, pos, speed, accel
***************
*** 748,752 ****
}
handled=true;
!
} else if
(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,PLAYER_ACTARRAY_CMD_SPEED,device_addr))
{
//We received a speed-cmd
--- 785,789 ----
}
handled=true;
!
} else if
(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,PLAYER_ACTARRAY_CMD_SPEED,device_addr))
{
//We received a speed-cmd
***************
*** 788,792 ****
ModuleSyncMotion(0); //Deactivate the syncmotion bit
}
!
if (joint == -1) {
for (unsigned int i=0 ; i != idModuleList.size() ; ++i
) {
--- 825,829 ----
ModuleSyncMotion(0); //Deactivate the syncmotion bit
}
!
if (joint == -1) {
for (unsigned int i=0 ; i != idModuleList.size() ; ++i
) {
***************
*** 842,853 ****
cout << "multi_current_cmd" << endl;
}
!
if ((cmd->currents_count == idModuleList.size()) ||
(cmd->currents_count == PLAYER_ACTARRAY_NUM_ACTUATORS)) {
!
if (can && !modsynchro) {
ModuleSyncMotion(1); //activate the
syncmotion bit
}
!
for (unsigned int i=0 ; i != idModuleList.size() ;
++i ) {
pclDevice->moveCur(idModuleList[i],cmd->currents[i]/directions[i]);
--- 879,890 ----
cout << "multi_current_cmd" << endl;
}
!
if ((cmd->currents_count == idModuleList.size()) ||
(cmd->currents_count == PLAYER_ACTARRAY_NUM_ACTUATORS)) {
!
if (can && !modsynchro) {
ModuleSyncMotion(1); //activate the
syncmotion bit
}
!
for (unsigned int i=0 ; i != idModuleList.size() ;
++i ) {
pclDevice->moveCur(idModuleList[i],cmd->currents[i]/directions[i]);
***************
*** 864,868 ****
cout << "WARNING: multi_current_cmd: The # of current
commands must equal the # of modules or to the maximum # of modules
(PLAYER_ACTARRAY_NUM_ACTUATORS). Ignoring command.\n";
}
!
} else if
(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,PLAYER_ACTARRAY_CMD_MULTI_POS,device_addr))
{
//We received a position-cmd for several joints
--- 901,905 ----
cout << "WARNING: multi_current_cmd: The # of current
commands must equal the # of modules or to the maximum # of modules
(PLAYER_ACTARRAY_NUM_ACTUATORS). Ignoring command.\n";
}
!
} else if
(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,PLAYER_ACTARRAY_CMD_MULTI_POS,device_addr))
{
//We received a position-cmd for several joints
***************
*** 874,885 ****
cout << "multi_pos_cmd" << endl;
}
!
if ( (cmd->positions_count == idModuleList.size()) ||
(cmd->positions_count == PLAYER_ACTARRAY_NUM_ACTUATORS)) {
!
if (can && !modsynchro) {
ModuleSyncMotion(1); //activate the
syncmotion bit
}
!
!
//transmit the commands to the modules. if
modsynchro=1, they wait until they get StartMotionAll to move
for (unsigned int i=0 ; i != idModuleList.size() ;
++i ) {
--- 911,922 ----
cout << "multi_pos_cmd" << endl;
}
!
if ( (cmd->positions_count == idModuleList.size()) ||
(cmd->positions_count == PLAYER_ACTARRAY_NUM_ACTUATORS)) {
!
if (can && !modsynchro) {
ModuleSyncMotion(1); //activate the
syncmotion bit
}
!
!
//transmit the commands to the modules. if
modsynchro=1, they wait until they get StartMotionAll to move
for (unsigned int i=0 ; i != idModuleList.size() ;
++i ) {
***************
*** 887,891 ****
float newposition(0.0);
!
if (normalize_angles[i]) {
--- 924,928 ----
float newposition(0.0);
!
if (normalize_angles[i]) {
***************
*** 901,917 ****
}
}
!
if (can) {
pclDevice->startMotionAll(); //Tells the
modules to start moving now (All together)
}
!
handled=true;
} else {
cout << "WARNING: multi_pos_cmd: The # of position
commands must equal the # of modules, or to the maximum # of modules
(PLAYER_ACTARRAY_NUM_ACTUATORS. Ignoring command.\n";
!
}
!
} else if
(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,PLAYER_ACTARRAY_CMD_MULTI_SPEED,device_addr))
{
--- 938,954 ----
}
}
!
if (can) {
pclDevice->startMotionAll(); //Tells the
modules to start moving now (All together)
}
!
handled=true;
} else {
cout << "WARNING: multi_pos_cmd: The # of position
commands must equal the # of modules, or to the maximum # of modules
(PLAYER_ACTARRAY_NUM_ACTUATORS. Ignoring command.\n";
!
}
!
} else if
(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,PLAYER_ACTARRAY_CMD_MULTI_SPEED,device_addr))
{
***************
*** 921,929 ****
player_actarray_multi_speed_cmd_t * cmd =
reinterpret_cast<player_actarray_multi_speed_cmd_t*>(data);
!
if (debug_level) {
cout << "multi_speed_cmd" << endl;
}
!
if ((cmd->speeds_count == idModuleList.size()) ||
(cmd->speeds_count == PLAYER_ACTARRAY_NUM_ACTUATORS)) {
--- 958,966 ----
player_actarray_multi_speed_cmd_t * cmd =
reinterpret_cast<player_actarray_multi_speed_cmd_t*>(data);
!
if (debug_level) {
cout << "multi_speed_cmd" << endl;
}
!
if ((cmd->speeds_count == idModuleList.size()) ||
(cmd->speeds_count == PLAYER_ACTARRAY_NUM_ACTUATORS)) {
***************
*** 945,956 ****
pclDevice->startMotionAll(); //Tells the
modules to start moving now (All together)
}
!
handled=true;
} else {
cout << "WARNING: multi_speed_cmd: The # of speed
commands must equal the # of modules, or to the maximum number of modules
(PLAYER_ACTARRAY_NUM_ACTUATORS). Ignoring command.\n";
!
}
!
}
--- 982,993 ----
pclDevice->startMotionAll(); //Tells the
modules to start moving now (All together)
}
!
handled=true;
} else {
cout << "WARNING: multi_speed_cmd: The # of speed
commands must equal the # of modules, or to the maximum number of modules
(PLAYER_ACTARRAY_NUM_ACTUATORS). Ignoring command.\n";
!
}
!
}
***************
*** 962,966 ****
return(-1);
}
!
}
--- 999,1003 ----
return(-1);
}
!
}
***************
*** 972,976 ****
// could want to have the behaviour without limits (set 0 in the
normalize_angles vector in the config file)
float AmtecM5::normalize_angle(float angle) {
!
return (atan2( sin(angle), cos(angle) ) );
}
--- 1009,1013 ----
// could want to have the behaviour without limits (set 0 in the
normalize_angles vector in the config file)
float AmtecM5::normalize_angle(float angle) {
!
return (atan2( sin(angle), cos(angle) ) );
}
***************
*** 979,983 ****
if (can) {
!
if (state==1) {
//Activate the synchro bits in all modules
--- 1016,1020 ----
if (can) {
!
if (state==1) {
//Activate the synchro bits in all modules
***************
*** 987,991 ****
config |= CONFIGID_MOD_SYNC_MOTION;
pclDevice->setConfig(idModuleList[i],config);
!
pclDevice->getConfig(idModuleList[i],&config);
if (config & CONFIGID_MOD_SYNC_MOTION) {
--- 1024,1028 ----
config |= CONFIGID_MOD_SYNC_MOTION;
pclDevice->setConfig(idModuleList[i],config);
!
pclDevice->getConfig(idModuleList[i],&config);
if (config & CONFIGID_MOD_SYNC_MOTION) {
***************
*** 994,1000 ****
}
modsynchro=true;
!
} else {
! //deactivate the synchro bits on all modules
for (unsigned int i=0 ; i != idModuleList.size() ;
++i ) {
unsigned long config(0);
--- 1031,1037 ----
}
modsynchro=true;
!
} else {
! //deactivate the synchro bits on all modules
for (unsigned int i=0 ; i != idModuleList.size() ;
++i ) {
unsigned long config(0);
***************
*** 1002,1006 ****
config &= ~(CONFIGID_MOD_SYNC_MOTION); // ~
is the binary NOT
pclDevice->setConfig(idModuleList[i],config);
!
pclDevice->getConfig(idModuleList[i],&config);
if (!(config & CONFIGID_MOD_SYNC_MOTION)) {
--- 1039,1043 ----
config &= ~(CONFIGID_MOD_SYNC_MOTION); // ~
is the binary NOT
pclDevice->setConfig(idModuleList[i],config);
!
pclDevice->getConfig(idModuleList[i],&config);
if (!(config & CONFIGID_MOD_SYNC_MOTION)) {
***************
*** 1009,1013 ****
}
modsynchro=false;
!
}
}
--- 1046,1050 ----
}
modsynchro=false;
!
}
}
***************
*** 1054,1058 ****
gavewarning=true;
}
!
//cout << real_elapsed << "mS\n";
--- 1091,1095 ----
gavewarning=true;
}
!
//cout << real_elapsed << "mS\n";
***************
*** 1067,1070 ****
--- 1104,1108 ----
// Copy the data.
player_actarray_data_t data;
+ data.actuators = actuators;
// The number of actuators in the array.
***************
*** 1074,1078 ****
data.motor_state=motor_state; // 0 or 1
!
//Memory to record the old positions (for velocity and accel
calculation)
static float oldpos[PLAYER_ACTARRAY_NUM_ACTUATORS];
--- 1112,1116 ----
data.motor_state=motor_state; // 0 or 1
!
//Memory to record the old positions (for velocity and accel
calculation)
static float oldpos[PLAYER_ACTARRAY_NUM_ACTUATORS];
***************
*** 1081,1085 ****
static bool valid_deltapos(false); // true after the second
run. (Valid velocity calculation)
static bool valid_deltaspeed(false); // true after the third
run. (Valid accel calculation)
!
if (!init_oldposspeed) {
//Initialize with zeros the first time.
--- 1119,1123 ----
static bool valid_deltapos(false); // true after the second
run. (Valid velocity calculation)
static bool valid_deltaspeed(false); // true after the third
run. (Valid accel calculation)
!
if (!init_oldposspeed) {
//Initialize with zeros the first time.
***************
*** 1088,1093 ****
init_oldposspeed=true;
}
!
!
//Temporary variables to hold the data that we read/calculate
float pos[module_count];
--- 1126,1131 ----
init_oldposspeed=true;
}
!
!
//Temporary variables to hold the data that we read/calculate
float pos[module_count];
***************
*** 1097,1101 ****
unsigned long state[module_count];
uint8_t report_state[module_count];
!
//Zero those arrays
memset(&pos,0,sizeof(pos));
--- 1135,1139 ----
unsigned long state[module_count];
uint8_t report_state[module_count];
!
//Zero those arrays
memset(&pos,0,sizeof(pos));
***************
*** 1104,1110 ****
memset(¤t,0,sizeof(current));
memset(&report_state,0,sizeof(report_state));
!
!
!
//First read position (The most time critical)
if (can) {
--- 1142,1148 ----
memset(¤t,0,sizeof(current));
memset(&report_state,0,sizeof(report_state));
!
!
!
//First read position (The most time critical)
if (can) {
***************
*** 1114,1118 ****
pclDevice->getCur(idModuleList[i],&(current[i]));
}
!
//Now read the saved positions
for (unsigned int i=0; i<module_count ; ++i ) {
--- 1152,1156 ----
pclDevice->getCur(idModuleList[i],&(current[i]));
}
!
//Now read the saved positions
for (unsigned int i=0; i<module_count ; ++i ) {
***************
*** 1140,1144 ****
for (unsigned int i=0; i != module_count ; ++i ) {
pclDevice->getModuleState(idModuleList[i],&(state[i]));
!
float deltapos=pos[i]-oldpos[i];
if (valid_deltapos) {
--- 1178,1182 ----
for (unsigned int i=0; i != module_count ; ++i ) {
pclDevice->getModuleState(idModuleList[i],&(state[i]));
!
float deltapos=pos[i]-oldpos[i];
if (valid_deltapos) {
***************
*** 1151,1155 ****
float deltaspeed=speed[i]-oldspeed[i];
if (valid_deltaspeed) {
! accel[i]=deltaspeed/(real_elapsed/1000.0);
} else {
accel[i]=0.0;
--- 1189,1193 ----
float deltaspeed=speed[i]-oldspeed[i];
if (valid_deltaspeed) {
! accel[i]=deltaspeed/(real_elapsed/1000.0);
} else {
accel[i]=0.0;
***************
*** 1171,1176 ****
}
!
!
//Prepare the data for publication
for (unsigned int i=0; i<module_count ; ++i ) {
--- 1209,1214 ----
}
!
!
//Prepare the data for publication
for (unsigned int i=0; i<module_count ; ++i ) {
***************
*** 1212,1220 ****
}
//Data is now ready for publication.
!
// Process incoming messages. AmtecM5::ProcessMessage() is
// called on each message. (We do this after reading data, to
avoid jitter on the acquisition rate)
ProcessMessages();
!
//Tell the watchdog we are still alive:
--- 1250,1258 ----
}
//Data is now ready for publication.
!
// Process incoming messages. AmtecM5::ProcessMessage() is
// called on each message. (We do this after reading data, to
avoid jitter on the acquisition rate)
ProcessMessages();
!
//Tell the watchdog we are still alive:
-------------------------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc.
Still grepping through log files to find problems? Stop.
Now Search log events and configuration files using AJAX and a browser.
Download your FREE copy of Splunk now >> http://get.splunk.com/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
