Update of /cvsroot/playerstage/code/player/server/drivers/mixed/erratic
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv32694/server/drivers/mixed/erratic
Modified Files:
Tag: release-2-0-patches
erratic.cc erratic.h motorpacket.cc 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.3
retrieving revision 1.1.2.4
diff -C2 -d -r1.1.2.3 -r1.1.2.4
*** robot_params.cc 22 Sep 2006 20:46:43 -0000 1.1.2.3
--- robot_params.cc 19 Dec 2006 18:55:56 -0000 1.1.2.4
***************
*** 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.1.2.3
retrieving revision 1.1.2.4
diff -C2 -d -r1.1.2.3 -r1.1.2.4
*** robot_params.h 22 Sep 2006 20:46:43 -0000 1.1.2.3
--- robot_params.h 19 Dec 2006 18:55:56 -0000 1.1.2.4
***************
*** 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: motorpacket.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/motorpacket.cc,v
retrieving revision 1.1.2.1
retrieving revision 1.1.2.2
diff -C2 -d -r1.1.2.1 -r1.1.2.2
*** motorpacket.cc 22 Sep 2006 20:46:43 -0000 1.1.2.1
--- motorpacket.cc 19 Dec 2006 18:55:56 -0000 1.1.2.2
***************
*** 107,111 ****
//if (debug_mode)
! // printf("Just parsed, new xpos: %i ypos: %i\n", xpos, ypos);
angle = (short)
--- 107,111 ----
//if (debug_mode)
! // printf("Just parsed, new xpos: %i ypos: %i\n", xpos, ypos);
angle = (short)
Index: erratic.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.cc,v
retrieving revision 1.1.2.5
retrieving revision 1.1.2.6
diff -C2 -d -r1.1.2.5 -r1.1.2.6
*** erratic.cc 25 Sep 2006 15:49:54 -0000 1.1.2.5
--- erratic.cc 19 Dec 2006 18:55:56 -0000 1.1.2.6
***************
*** 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
***************
*** 215,218 ****
--- 220,224 ----
// intialise members
motor_packet = NULL;
+ mcount = 0;
// Do we create a robot position interface?
***************
*** 804,814 ****
erratic_data.ir.voltages_count =
RobotParams[this->param_idx]->NumIR;
erratic_data.ir.ranges_count = 2;
! for (unsigned int i_voltage = 0; i_voltage <
erratic_data.aio.voltages_count ;i_voltage++) {
! 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;
! erratic_data.ir.voltages[i_voltage] =
(packet.packet[5+i_voltage*2]
! +
256*packet.packet[6+i_voltage*2]) * (1.0 / 1024.0) * CPU_VOLTAGE;
! erratic_data.ir.ranges[i_voltage] =
IRRangeFromVoltage(erratic_data.ir.voltages[i_voltage]);
! }
PublishAIn();
PublishIR();
--- 810,840 ----
erratic_data.ir.voltages_count =
RobotParams[this->param_idx]->NumIR;
erratic_data.ir.ranges_count = 2;
! unsigned int i_voltage;
! 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;
!
erratic_data.ir.voltages[i_voltage] = (packet.packet[5+i_voltage*2]
! +
256*packet.packet[6+i_voltage*2]) * (1.0 / 1024.0) * CPU_VOLTAGE;
!
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();
***************
*** 891,894 ****
--- 917,923 ----
unsigned char erraticcommand[4];
+ //**************************
+ // printf("Reset raw odometry\n");
+
if(this->motor_packet) {
pkt = new ErraticPacket();
***************
*** 897,900 ****
--- 926,932 ----
this->motor_packet->xpos = 0;
this->motor_packet->ypos = 0;
+ this->motor_packet->x_offset = 0;
+ this->motor_packet->y_offset = 0;
+ this->motor_packet->angle_offset = 0;
erraticcommand[0] = (command_e)reset_origo;
erraticcommand[1] = (argtype_e)argint;
***************
*** 1078,1081 ****
--- 1110,1118 ----
- this->motor_packet->angle;
+ //**************************
+ // printf("Reset odometry: %f %f %f\n",
set_odom_req->pose.px, set_odom_req->pose.py, set_odom_req->pose.pa);
+ // printf("Reset odometry: %d %d %d\n",
motor_packet->x_offset, motor_packet->y_offset, motor_packet->angle_offset);
+
+
this->Publish(this->position_id, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_SET_ODOM);
***************
*** 1244,1247 ****
--- 1281,1292 ----
+ // function to get the time in ms
+ int getms()
+ {
+ struct timeval tv;
+ gettimeofday(&tv,NULL);
+ return tv.tv_sec*1000 + tv.tv_usec/1000;
+ }
+
// Handles one Player command detailing a velocity
***************
*** 1260,1263 ****
--- 1305,1315 ----
//turnRateDemand = 768;
+ // throttle back on commands
+ int ms = getms();
+ if (mcount == 0) mcount = ms-200;
+ if (ms < mcount + 50) // at least 100
ms have to elapse
+ return;
+ mcount = ms;
+
//if (debug_mode)
// printf("Will VW, %i and %i\n", speedDemand, turnRateDemand);
Index: erratic.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.h,v
retrieving revision 1.1.2.5
retrieving revision 1.1.2.6
diff -C2 -d -r1.1.2.5 -r1.1.2.6
*** erratic.h 25 Sep 2006 15:49:54 -0000 1.1.2.5
--- erratic.h 19 Dec 2006 18:55:56 -0000 1.1.2.6
***************
*** 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,234 ****
class Erratic : public Driver
{
! private:
! 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
