Update of /cvsroot/playerstage/code/player/server/drivers/mixed/erratic
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv10048/server/drivers/mixed/erratic
Added Files:
.cvsignore Makefile.am erratic.cc erratic.h packet.cc packet.h
robot_params.cc robot_params.h sip.cc sip.h
Log Message:
added erratic driver
--- NEW FILE: erratic.cc ---
// -*- mode:C++; tab-width:2; c-basic-offset:2; indent-tabs-mode:1; -*-
/**
* Videre Erratic robot driver for Player
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
USA
**/
[...1165 lines suppressed...]
}
motorpacket = new ErraticPacket();
motorpacket->Build(motorcommand, 4);
Send(motorpacket);
}
}
// Switchboard for robot commands, called from ProcessMessage
int Erratic::HandleCommand(player_msghdr * hdr, void* data) {
if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL, this->position_id))
{
player_position2d_cmd_vel_t position_cmd =
*(player_position2d_cmd_vel_t*)data;
this->HandlePositionCommand(position_cmd);
} else
return(-1);
return(0);
}
--- NEW FILE: erratic.h ---
// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:1; -*-
/**
* Erratic Erratic robot driver for Player
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
USA
**/
#ifndef _P2OSDEVICE_H
#define _P2OSDEVICE_H
#include <pthread.h>
#include <sys/time.h>
#include <queue>
#include <libplayercore/playercore.h>
#include <replace/replace.h>
#include "packet.h"
#include "robot_params.h"
#include <stdint.h>
// Version
#define ERRATIC_VERSION "1.0b"
#define ERRATIC_DATE "2006-05-07"
// Default max speeds
#define MOTOR_DEF_MAX_SPEED 0.5
#define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
// This merely sets a delay policy in the initial connection
#define ROBOT_CYCLETIME 20000
/* Erratic constants */
#define VIDERE_NOMINAL_VOLTAGE 12.0
// Commands for the robot
typedef enum command {
pulse = 0,
open_controller = 1,
close_controller = 2,
enable_motors = 4,
set_max_trans_acc = 5,
set_max_position_velocity = 6,
reset_origo = 7,
trans_vel = 11,
configuration = 18,
rot_vel = 21,
set_max_rot_acc = 23,
stop = 29,
wheel_vel = 32,
set_analog = 71,
save_config = 72,
set_pwm_freq = 73,
set_pwm_max_on = 74,
set_pid_trans_p = 80,
set_pid_trans_v = 81,
set_pid_trans_i = 82,
set_pid_rot_p = 83,
set_pid_rot_v = 84,
set_pid_rot_i = 85,
} command_e;
// Argument types used in robot commands
typedef enum argtype {
argint = 0x3B,
argnint = 0x1B,
argstr = 0x2B
} argtype_e;
// Types of replies from the robot
typedef enum reply {
debug = 0x15,
config = 0x20,
stopped = 0x32,
moving = 0x33,
motor = 0x80,
encoder = 0x90,
ain = 0x9a
} reply_e;
#define DEFAULT_VIDERE_PORT "/dev/ttyS0"
typedef struct player_erratic_data
{
player_position2d_data_t position;
player_power_data_t power;
player_aio_data_t aio;
} __attribute__ ((packed)) player_erratic_data_t;
// this is here because we need the above typedef's before including it.
#include "sip.h"
extern bool debug_mode;
class erSIP;
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;
int position_subscriptions;
int aio_subscriptions;
//erSIP* sippacket;
erSIP *motor_packet;
pthread_mutex_t motor_packet_mutex;
int Connect();
int Disconnect();
int SendReceiveOLD(ErraticPacket* pkt, bool publish_data=true);
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 PublishAllData();
void PublishPosition2D();
void PublishPower();
void PublishAIn();
void StartThreads();
void StopThreads();
void Send(ErraticPacket *packet);
void SendThread();
static void *SendThreadDummy(void *driver);
void ReceiveThread();
static void *ReceiveThreadDummy(void *driver);
int direct_wheel_vel_control; // false -> separate trans and rot vel
int read_fd, write_fd;
const char* psos_serial_port;
struct timeval lastblob_tv;
player_position2d_cmd_vel_t last_position_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 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);
};
#endif
--- NEW FILE: packet.h ---
/*
* Copyright (C) 2006
* Videre Design
* Copyright (C) 2000
* Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#ifndef _PACKET_H
#define _PACKET_H
#include <string.h>
#include <stdint.h>
#define PACKET_LEN 256
enum receive_result_e {
success = 0,
failure = 1,
timeout = 2
};
class ErraticPacket
{
public:
unsigned char packet[PACKET_LEN];
unsigned char size;
uint16_t CalcChkSum();
void Print();
void PrintHex();
int Build( unsigned char *data, unsigned char datasize );
int Send( int fd );
int Receive( int fd, uint16_t wait = 30 );
bool Check();
bool operator!= ( ErraticPacket p ) {
if ( size != p.size) return(true);
if ( memcmp( packet, p.packet, size ) != 0 ) return (true);
return(false);
}
};
#endif
--- NEW FILE: sip.h ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000
* Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/*
* $Id: sip.h,v 1.1 2006/05/30 20:39:30 gerkey Exp $
*
* part of the P2OS parser. methods for filling and parsing server
* information packets (SIPs)
*/
#ifndef _SIP_H
#define _SIP_H
#include <limits.h>
#include "erratic.h"
class erSIP
{
private:
int PositionChange( unsigned short, unsigned short );
int param_idx; // index of our robot's data in the parameter table
public:
// these values are returned in every standard SIP
bool lwstall, rwstall;
unsigned char status, battery;
unsigned short ptu, timer, rawxpos;
unsigned short rawypos;
short angle, lvel, rvel, control;
int xpos, ypos;
int x_offset,y_offset,angle_offset;
bool Parse( unsigned char *buffer, int length );
void Print();
void Fill(player_erratic_data_t* data);
erSIP(int idx)
{
param_idx = idx;
xpos = INT_MAX;
ypos = INT_MAX;
}
};
#endif
--- NEW FILE: packet.cc ---
// -*- mode:C++; tab-width:2; c-basic-offset:2; indent-tabs-mode:1; -*-
/**
* Player - One Hell of a Robot Server
* Copyright (C) 2000
* Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
USA
*
**/
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include "packet.h"
#include <unistd.h>
#include <stdlib.h> /* for exit() */
//#include <sys/poll.h>
#include "erratic.h"
void ErraticPacket::Print() {
if (packet) {
printf("\"");
for(int i=0;i<size;i++) {
printf("%u ", packet[i]);
}
puts("\"");
}
}
void ErraticPacket::PrintHex() {
if (packet) {
printf("\"Hex: ");
for(int i=0;i<size;i++) {
printf("%.2x ", packet[i]);
}
puts("\"");
}
}
bool ErraticPacket::Check() {
uint16_t chksum = CalcChkSum();
uint16_t received_chksum = packet[size-2] << 8 | packet[size-1];
if ( chksum == received_chksum )
return(true);
if (debug_mode) {
printf("This packet failed checksum control (%i instead of %i):
", received_chksum, chksum);
PrintHex();
}
return(false);
}
uint16_t ErraticPacket::CalcChkSum() {
uint8_t *buffer = &packet[3];
uint16_t n = size - 5;
uint16_t c = 0;
while (n > 1) {
c+= (*(buffer)<<8) | *(buffer+1);
n -= 2;
buffer += 2;
}
if (n > 0) c = c ^ (uint16_t)*buffer;
return(c);
}
// You can supply a timeout in milliseconds
int ErraticPacket::Receive( int fd, uint16_t wait ) {
uint8_t prefix[3];
uint32_t skipped;
uint16_t cnt;
memset(packet,0,sizeof(packet));
struct pollfd readpoll;
readpoll.fd = fd; readpoll.events = POLLIN | POLLERR | POLLHUP |
POLLNVAL; readpoll.revents = 0;
// This block will terminate or move on when there is data to read
if (wait) {
while (1) {
int8_t stuff = poll(&readpoll, 1, wait);
if (stuff < 0) {
if (errno == EINTR) {
continue;
}
return 1;
}
if (stuff == 0) {
return (receive_result_e)timeout;
}
if (readpoll.revents & POLLIN)
break;
printf("Serial port error\n");
return (receive_result_e)failure;
}
}
do {
memset(prefix,0,sizeof(prefix));
skipped = 0;
while(1) {
cnt = 0;
// Once we've started receiving a packet, we have a
tight timeout
while( cnt!=1 ) {
if (wait) {
while (1) {
int8_t stuff = poll(&readpoll,
1, 3);
if (stuff < 0) {
if (errno == EINTR) {
continue;
}
return 1;
}
if (stuff == 0) {
return
(receive_result_e)timeout;
}
if (readpoll.revents & POLLIN)
break;
printf("Serial port error\n");
return
(receive_result_e)failure;
}
}
int newcnt = read( fd, &prefix[2], 1 );
if (newcnt < 0 && errno == EAGAIN)
continue;
else if (newcnt < 0) {
perror("Erratic::Receive:read:");
return(1);
}
cnt++;
}
if (prefix[0]==0xFA && prefix[1]==0xFB) break;
prefix[0]=prefix[1];
prefix[1]=prefix[2];
skipped++;
if (skipped > 200) return (receive_result_e)timeout;
}
if (skipped>2 && debug_mode) printf("Skipped %d bytes\n",
skipped);
size = prefix[2]+3;
memcpy( packet, prefix, 3);
cnt = 0;
while( cnt!=prefix[2] )
{
if (wait) {
while (1) {
int8_t stuff = poll(&readpoll, 1, 3);
if (stuff < 0) {
if (errno == EINTR) {
continue;
}
return 1;
}
if (stuff == 0) {
return
(receive_result_e)timeout;
}
if (readpoll.revents & POLLIN)
break;
printf("Serial port error\n");
return (receive_result_e)failure;
}
}
int newcnt = read( fd, &packet[3+cnt], prefix[2]-cnt );
if (newcnt < 0 && errno == EAGAIN)
continue;
else if (newcnt < 0) {
perror("Erratic::Receive:read:");
return(1);
}
cnt += newcnt;
}
} while (!Check());
return(0);
}
int ErraticPacket::Build( unsigned char *data, unsigned char datasize ) {
uint16_t chksum;
size = datasize + 5;
/* header */
packet[0]=0xFA;
packet[1]=0xFB;
if ( size > 198 ) {
printf("Erratic driver error: Packet to robot can't be larger
than 200 bytes");
return(1);
}
packet[2] = datasize + 2;
memcpy( &packet[3], data, datasize );
chksum = CalcChkSum();
//if (chksum < 0) chksum += 0x8000;
packet[3+datasize] = chksum >> 8;
packet[3+datasize+1] = chksum;// & 0xFF;
/*if (debug_mode) {
int16_t last = packet[3+datasize+1];
int32_t test = (packet[3+datasize] << 8) | last;
printf("chksum that will be sent:%i , received: %i\n", chksum,
test);
}*/
return(0);
}
int ErraticPacket::Send( int fd) {
int cnt=0;
//printf("Send(): ");
//if (packet[3] != 0x0b && packet[3] != 0x15)
// PrintHex();
//if (debug_mode)
// Print();
while(cnt!=size)
{
if((cnt += write( fd, packet+cnt, size-cnt )) < 0)
{
perror("Send");
return(1);
}
}
return(0);
}
--- NEW FILE: robot_params.cc ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000
* Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include "robot_params.h"
RobotParams_t erratic_params =
{
0.001534,
"Videre",
0.011,
0.780, //This is the empirically correct value, but doesn't match wheel size
calculation
0,
1.626,
1,
1,
0,
0,
0,
"",
"",
0,
1,
0,
0,
0,
300,
1000,
0,
5,
5,
1,
0,
0,
0,
120,
330,
180,
279,
0,
0,
0,
1,
1,
8,
"erratic",
38400,
0,
0,
0,
0,
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 },
}
};
RobotParams_t *RobotParams[1] = {&erratic_params};
--- NEW FILE: robot_params.h ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000
* Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#ifndef _ROBOT_PARAMS_H
#define _ROBOT_PARAMS_H
void initialize_robot_params(void);
#define PLAYER_NUM_ROBOT_TYPES 30
typedef struct
{
double x;
double y;
double th;
} sonar_pose_t;
typedef struct
{
double AngleConvFactor; //
char* Class;
double DiffConvFactor; //
double DistConvFactor; //
int FrontBumpers; //
double GyroScaler; //
int HasMoveCommand; //
int Holonomic; //
int IRNum; //
int IRUnit; //
int LaserFlipped; //
char* LaserIgnore;
char* LaserPort;
int LaserPossessed; //
int LaserPowerControlled; //
int LaserTh; //
int LaserX; //
int LaserY; //
int MaxRVelocity; //
int MaxVelocity; //
int NewTableSensingIR; //
int NumFrontBumpers; //
int NumRearBumpers; //
double RangeConvFactor; //
int RearBumpers; //
int RequestEncoderPackets; //
int RequestIOPackets; //
int RobotDiagonal; //
int RobotLength; //
int RobotRadius; //
int RobotWidth; //
int RotAccel; //
int RotDecel; //
int RotVelMax; //
int SettableAccsDecs; //
int SettableVelMaxes; //
int SonarNum; //
char* Subclass;
int SwitchToBaudRate; //
int TableSensingIR; //
int TransAccel; //
int TransDecel; //
int TransVelMax; //
int Vel2Divisor; //
double VelConvFactor; //
sonar_pose_t sonar_pose[32];
} RobotParams_t;
//extern RobotParams_t PlayerRobotParams[];
//extern RobotParams_t erratic_params;
extern RobotParams_t *RobotParams[];
#endif
--- NEW FILE: Makefile.am ---
noinst_LTLIBRARIES =
if INCLUDE_ERRATIC
noinst_LTLIBRARIES += liberratic.la
endif
AM_CPPFLAGS = -Wall -I$(top_srcdir)
liberratic_la_SOURCES = erratic.cc erratic.h \
packet.h packet.cc \
robot_params.cc robot_params.h \
sip.cc sip.h replace.h
--- NEW FILE: .cvsignore ---
Makefile
Makefile.in
--- NEW FILE: sip.cc ---
// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:1; -*-
/**
* Videre Erratic robot driver for Player
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
USA
*
**/
#include <stdio.h>
#include <limits.h>
#include <math.h> /* rint(3) */
#include <sys/types.h>
#include <netinet/in.h>
#include "sip.h"
#include <stdlib.h> /* for abs() */
#include <unistd.h>
int erSIP::PositionChange( unsigned short from, unsigned short to )
{
int diff1, diff2;
/* find difference in two directions and pick shortest */
if ( to > from ) {
diff1 = to - from;
diff2 = - ( from + 4096 - to );
}
else {
diff1 = to - from;
diff2 = 4096 - from + to;
}
if ( abs(diff1) < abs(diff2) )
return(diff1);
else
return(diff2);
}
void erSIP::Print()
{
printf("lwstall:%d rwstall:%d\n", lwstall, rwstall);
printf("status: 0x%x", status);
printf("battery: %d\n", battery);
printf("xpos: %d ypos:%d ptu:%hu timer:%hu\n", xpos, ypos, ptu, timer);
printf("angle: %d lvel: %d rvel: %d control: %d\n", angle, lvel, rvel,
control);
}
// Parses and absorbs a standard packet from the robot
bool erSIP::Parse( unsigned char *buffer, int length )
{
int cnt = 0, change;
unsigned short newxpos, newypos;
// Check type and length
if (length < 20) return false;
status = buffer[cnt];
cnt += sizeof(unsigned char);
// This driver does its own integration, and only cares for the lower bits of
the odometry updates
// Integers from robot are little-endian
newxpos = buffer[cnt] + 0x100*(buffer[cnt+1]&0x0F);
if (xpos!=INT_MAX) {
change = (int) rint(PositionChange( rawxpos, newxpos ) *
RobotParams[param_idx]->DistConvFactor);
//printf("xchange: %i ", change);
if (abs(change)>100)
;//PLAYER_WARN1("invalid odometry change [%d]; odometry values are
tainted", change);
else
xpos += change;
}
else {
//printf("xpos was INT_MAX\n");
xpos = 0;
}
rawxpos = newxpos;
cnt += 3;
newypos = buffer[cnt] + 0x100*(buffer[cnt+1]&0x0F);
if (ypos!=INT_MAX) {
change = (int) rint(PositionChange( rawypos, newypos ) *
RobotParams[param_idx]->DistConvFactor);
if (abs(change)>100)
;//PLAYER_WARN1("invalid odometry change [%d]; odometry values are
tainted", change);
else
ypos += change;
}
else
ypos = 0;
rawypos = newypos;
cnt += 3;
//if (debug_mode)
// printf("Just parsed, new xpos: %i ypos: %i\n", xpos, ypos);
angle = (short)
rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
M_PI/2048.0 * 180.0/M_PI);
cnt += sizeof(short);
lvel = (short)
rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
RobotParams[param_idx]->VelConvFactor);
cnt += sizeof(short);
rvel = (short)
rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
RobotParams[param_idx]->VelConvFactor);
cnt += sizeof(short);
battery = buffer[cnt];
cnt += sizeof(unsigned char);
lwstall = buffer[cnt] & 0x01;
cnt += sizeof(unsigned char);
rwstall = buffer[cnt] & 0x01;
cnt += sizeof(unsigned char);
control = (short)
rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
RobotParams[param_idx]->AngleConvFactor);
cnt += sizeof(short);
return true;
}
// Spits out information that was previously parsed
void erSIP::Fill(player_erratic_data_t* data)
{
// Odometry data
{
// initialize position to current offset
data->position.pos.px = this->x_offset / 1e3;
data->position.pos.py = this->y_offset / 1e3;
// now transform current position by rotation if there is one
// and add to offset
if(this->angle_offset != 0)
{
double rot = DTOR(this->angle_offset); // convert
rotation to radians
data->position.pos.px += ((this->xpos/1e3) * cos(rot)
-
(this->ypos/1e3) * sin(rot));
data->position.pos.py += ((this->xpos/1e3) * sin(rot)
+
(this->ypos/1e3) * cos(rot));
data->position.pos.pa = DTOR(this->angle_offset +
angle);
}
else
{
data->position.pos.px += this->xpos / 1e3;
data->position.pos.py += this->ypos / 1e3;
data->position.pos.pa = DTOR(this->angle);
}
data->position.vel.px = (((this->lvel) + (this->rvel) ) / 2) /
1e3;
data->position.vel.py = 0.0;
data->position.vel.pa = (0.596*(double)(this->rvel -
this->lvel) /
(2.0/RobotParams[param_idx]->DiffConvFactor));
data->position.stall = (unsigned char)(this->lwstall ||
this->rwstall);
}
// Battery data
{
data->power.valid = PLAYER_POWER_MASK_VOLTS |
PLAYER_POWER_MASK_PERCENT;
data->power.volts = this->battery / 1e1;
data->power.percent = 1e2 * (data->power.volts /
VIDERE_NOMINAL_VOLTAGE);
}
}
-------------------------------------------------------
All the advantages of Linux Managed Hosting--Without the Cost and Risk!
Fully trained technicians. The highest number of Red Hat certifications in
the hosting industry. Fanatical Support. Click to learn more
http://sel.as-us.falkag.net/sel?cmd=lnk&kid=107521&bid=248729&dat=121642
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
