Update of /cvsroot/playerstage/code/player/server/drivers/mixed/erratic
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv19401
Added Files:
motorpacket.cc motorpacket.h
Log Message:
--- NEW FILE: motorpacket.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 "motorpacket.h"
#include <stdlib.h> /* for abs() */
#include <unistd.h>
int ErraticMotorPacket::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 ErraticMotorPacket::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 ErraticMotorPacket::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 ErraticMotorPacket::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);
}
}
--- NEW FILE: motorpacket.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: motorpacket.h,v 1.1 2006/09/22 07:09:27 kkonolige Exp $
*
* part of the P2OS parser. methods for filling and parsing server
* information packets (ErraticMotorPackets)
*/
#ifndef _ErraticMotorPacket_H
#define _ErraticMotorPacket_H
#include <limits.h>
#include "erratic.h"
class ErraticMotorPacket
{
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 ErraticMotorPacket
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);
ErraticMotorPacket(int idx)
{
param_idx = idx;
xpos = INT_MAX;
ypos = INT_MAX;
}
};
#endif
-------------------------------------------------------------------------
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
