Update of /cvsroot/playerstage/code/player/server/drivers/mixed/erratic
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv31130
Modified Files:
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.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** robot_params.cc 22 Sep 2006 07:08:15 -0000 1.2
--- robot_params.cc 18 Dec 2006 06:03:27 -0000 1.3
***************
*** 23,27 ****
--- 23,29 ----
#include "robot_params.h"
+ #include <math.h>
+ #define DTOR(x) (M_PI * x / 180.0)
RobotParams_t erratic_params =
***************
*** 73,89 ****
20,
1.20482,
! {
! { 73, 105, 90 },
! { 130, 78, 41 },
! { 154, 30, 15 },
! { 154, -30, -15 },
! { 130, -78, -41 },
! { 73, -105, -90 },
! { -146, -60, -145 },
! { -146, 60, 145 },
},
2,
{
! {0.1, 0.1, 0},
{0.1, -0.1, 0},
}
--- 75,92 ----
20,
1.20482,
! 8,
! { // sonar poses, in m and rads
! { 0.180, 0.110, DTOR(90) },
! { 0.200, 0.85, DTOR(0) },
! { 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,
{
! {0.1, 0.1, 0}, // ir poses in m and deg
{0.1, -0.1, 0},
}
Index: robot_params.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/robot_params.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** robot_params.h 22 Sep 2006 07:08:15 -0000 1.2
--- robot_params.h 18 Dec 2006 06:03:27 -0000 1.3
***************
*** 89,95 ****
int Vel2Divisor; //
double VelConvFactor; //
sonar_pose_t sonar_pose[32];
! int NumIR;
! player_pose_t IRPose[8];
} RobotParams_t;
--- 89,96 ----
int Vel2Divisor; //
double VelConvFactor; //
+ int NumSonars;
sonar_pose_t sonar_pose[32];
! int NumIR;
! player_pose_t IRPose[8];
} RobotParams_t;
Index: erratic.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.cc,v
retrieving revision 1.10
retrieving revision 1.11
diff -C2 -d -r1.10 -r1.11
*** erratic.cc 18 Dec 2006 04:49:56 -0000 1.10
--- erratic.cc 18 Dec 2006 06:03:27 -0000 1.11
***************
*** 52,60 ****
- @ref interface_aio
! - Returns data from analog input pins
- @ref interface_ir
- Returns ranges from IR sensors, assuming they're connected to the analog
input pins
@par Supported configuration requests
--- 52,63 ----
- @ref interface_aio
! - Returns data from analog and digital input pins
- @ref interface_ir
- Returns ranges from IR sensors, assuming they're connected to the analog
input pins
+ - @ref interface_sonar
+ - Returns ranges from sonar sensors
+
@par Supported configuration requests
***************
*** 66,69 ****
--- 69,74 ----
- PLAYER_POSITION_VELOCITY_MODE_REQ
- @ref interface_ir :
+ - PLAYER_IR_POSE_REQ
+ - @ref interface_sonar :
- PLAYER_SONAR_GET_GEOM_REQ
***************
*** 808,811 ****
--- 813,818 ----
for (i_voltage = 0; i_voltage <
erratic_data.aio.voltages_count ;i_voltage++)
{
+ if (i_voltage >=
PLAYER_AIO_MAX_INPUTS)
+ continue;
erratic_data.aio.voltages[i_voltage] = (packet.packet[5+i_voltage*2]
+
256*packet.packet[6+i_voltage*2]) * (1.0 / 1024.0) * CPU_VOLTAGE;
***************
*** 814,817 ****
--- 821,840 ----
erratic_data.ir.ranges[i_voltage] =
IRRangeFromVoltage(erratic_data.ir.voltages[i_voltage]);
}
+
+ // add digital inputs, four E port bits
+ for (int i=0; i < 4; i++)
+ {
+ if (i_voltage >=
PLAYER_AIO_MAX_INPUTS)
+ continue;
+
erratic_data.aio.voltages[i_voltage+i] =
+
(packet.packet[5+i_voltage*2] & (0x1 << i+4)) ? 1 : 0;
+
erratic_data.ir.voltages[i_voltage+1] =
+
(packet.packet[5+i_voltage*2] & (0x1 << i+4)) ? 1 : 0;
+
erratic_data.ir.ranges[i_voltage+i] =
+ IRRangeFromVoltage(erratic_data.ir.voltages[i_voltage+i]);
+ }
+
+
+
PublishAIn();
PublishIR();
Index: erratic.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.h,v
retrieving revision 1.6
retrieving revision 1.7
diff -C2 -d -r1.6 -r1.7
*** erratic.h 16 Oct 2006 00:23:30 -0000 1.6
--- erratic.h 18 Dec 2006 06:03:27 -0000 1.7
***************
*** 110,114 ****
! #define DEFAULT_VIDERE_PORT "/dev/ttyS0"
typedef struct player_erratic_data
--- 110,114 ----
! #define DEFAULT_VIDERE_PORT "/dev/erratic"
typedef struct player_erratic_data
***************
*** 116,121 ****
player_position2d_data_t position;
player_power_data_t power;
! player_aio_data_t aio;
! player_ir_data ir;
} __attribute__ ((packed)) player_erratic_data_t;
--- 116,121 ----
player_position2d_data_t position;
player_power_data_t power;
! player_aio_data_t aio;
! player_ir_data ir;
} __attribute__ ((packed)) player_erratic_data_t;
***************
*** 129,235 ****
class Erratic : public Driver
{
! private:
int mcount;
! player_erratic_data_t erratic_data;
! player_devaddr_t position_id;
! player_devaddr_t power_id;
! player_devaddr_t aio_id;
! player_devaddr_t ir_id;
! int position_subscriptions;
! int aio_ir_subscriptions;
! //ErraticMotorPacket* sippacket;
! ErraticMotorPacket *motor_packet;
! pthread_mutex_t motor_packet_mutex;
! int Connect();
! int Disconnect();
! void ResetRawPositions();
! void ToggleMotorPower(unsigned char val);
! void ToggleAIn(unsigned char val);
! int HandleConfig(MessageQueue* resp_queue, player_msghdr * hdr, void*
data);
! int HandleCommand(player_msghdr * hdr, void * data);
! void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
! void HandleCarCommand(player_position2d_cmd_car_t position_cmd);
! void PublishAllData();
! void PublishPosition2D();
! void PublishPower();
! void PublishAIn();
! void PublishIR();
! float IRRangeFromVoltage(float voltage);
! void StartThreads();
! void StopThreads();
! void Send(ErraticPacket *packet);
! void SendThread();
! static void *SendThreadDummy(void *driver);
! void ReceiveThread();
! static void *ReceiveThreadDummy(void *driver);
! int read_fd, write_fd;
! const char* psos_serial_port;
! player_position2d_cmd_vel_t last_position_cmd;
! player_position2d_cmd_car_t last_car_cmd;
! std::queue<ErraticPacket *> send_queue;
! pthread_mutex_t send_queue_mutex;
! pthread_cond_t send_queue_cond;
! pthread_t send_thread;
! pthread_t receive_thread;
! // Parameters
! bool direct_wheel_vel_control;
! bool print_all_packets;
! bool print_status_summary;
! bool save_settings_in_robot;
! int param_idx; // index in the RobotParams table for this robot
! // Max motor speeds (mm/sec,deg/sec)
! int motor_max_speed;
! int motor_max_turnspeed;
! // Customized control settings for the robot
! int16_t pid_trans_p, pid_trans_v, pid_trans_i;
! int16_t pid_rot_p, pid_rot_v, pid_rot_i;
! // This is a fairly low-level setting that is exposed
! uint16_t motor_pwm_frequency, motor_pwm_max_on;
! // Bound the command velocities
! bool use_vel_band;
! // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
! short motor_max_trans_accel, motor_max_trans_decel;
! short motor_max_rot_accel, motor_max_rot_decel;
! public:
! Erratic(ConfigFile* cf, int section);
! virtual int Subscribe(player_devaddr_t id);
! virtual int Unsubscribe(player_devaddr_t id);
! /* the main thread */
! virtual void Main();
! virtual int Setup();
! virtual int Shutdown();
! // MessageHandler
! virtual int ProcessMessage(MessageQueue * resp_queue, player_msghdr *
hdr, void * data);
};
--- 129,235 ----
class Erratic : public Driver
{
! private:
int mcount;
! player_erratic_data_t erratic_data;
! player_devaddr_t position_id;
! player_devaddr_t power_id;
! player_devaddr_t aio_id;
! player_devaddr_t ir_id;
! int position_subscriptions;
! int aio_ir_subscriptions;
! //ErraticMotorPacket* sippacket;
! ErraticMotorPacket *motor_packet;
! pthread_mutex_t motor_packet_mutex;
! int Connect();
! int Disconnect();
! void ResetRawPositions();
! void ToggleMotorPower(unsigned char val);
! void ToggleAIn(unsigned char val);
! int HandleConfig(MessageQueue* resp_queue, player_msghdr * hdr, void* data);
! int HandleCommand(player_msghdr * hdr, void * data);
! void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
! void HandleCarCommand(player_position2d_cmd_car_t position_cmd);
! void PublishAllData();
! void PublishPosition2D();
! void PublishPower();
! void PublishAIn();
! void PublishIR();
! float IRRangeFromVoltage(float voltage);
! void StartThreads();
! void StopThreads();
! void Send(ErraticPacket *packet);
! void SendThread();
! static void *SendThreadDummy(void *driver);
! void ReceiveThread();
! static void *ReceiveThreadDummy(void *driver);
! int read_fd, write_fd;
! const char* psos_serial_port;
! player_position2d_cmd_vel_t last_position_cmd;
! player_position2d_cmd_car_t last_car_cmd;
! std::queue<ErraticPacket *> send_queue;
! pthread_mutex_t send_queue_mutex;
! pthread_cond_t send_queue_cond;
! pthread_t send_thread;
! pthread_t receive_thread;
! // Parameters
! bool direct_wheel_vel_control;
! bool print_all_packets;
! bool print_status_summary;
! bool save_settings_in_robot;
! int param_idx; // index in the RobotParams table for this robot
! // Max motor speeds (mm/sec,deg/sec)
! int motor_max_speed;
! int motor_max_turnspeed;
! // Customized control settings for the robot
! int16_t pid_trans_p, pid_trans_v, pid_trans_i;
! int16_t pid_rot_p, pid_rot_v, pid_rot_i;
! // This is a fairly low-level setting that is exposed
! uint16_t motor_pwm_frequency, motor_pwm_max_on;
! // Bound the command velocities
! bool use_vel_band;
! // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
! short motor_max_trans_accel, motor_max_trans_decel;
! short motor_max_rot_accel, motor_max_rot_decel;
! public:
! Erratic(ConfigFile* cf, int section);
! virtual int Subscribe(player_devaddr_t id);
! virtual int Unsubscribe(player_devaddr_t id);
! /* the main thread */
! virtual void Main();
! virtual int Setup();
! virtual int Shutdown();
! // MessageHandler
! virtual int ProcessMessage(MessageQueue * resp_queue, player_msghdr * hdr,
void * data);
};
-------------------------------------------------------------------------
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
