Update of /cvsroot/playerstage/code/player/server/drivers/mixed/erratic
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv11614
Modified Files:
Tag: release-2-0-patches
erratic.cc erratic.h motorpacket.cc
Log Message:
Index: erratic.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.cc,v
retrieving revision 1.1.2.23
retrieving revision 1.1.2.24
diff -C2 -d -r1.1.2.23 -r1.1.2.24
*** erratic.cc 11 Oct 2007 05:21:25 -0000 1.1.2.23
--- erratic.cc 26 Oct 2007 02:33:12 -0000 1.1.2.24
***************
*** 1252,1265 ****
(player_position2d_set_odom_req_t*)data;
! this->motor_packet->x_offset = ((int)rint(set_odom_req->pose.px*1e3))
! - this->motor_packet->xpos;
this->motor_packet->y_offset = ((int)rint(set_odom_req->pose.py*1e3))
! - this->motor_packet->ypos;
! this->motor_packet->angle_offset =
((int)rint(RTOD(set_odom_req->pose.pa)))
! - 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);
--- 1252,1265 ----
(player_position2d_set_odom_req_t*)data;
! this->motor_packet->x_offset = ((int )rint(set_odom_req->pose.px*1e3))
! + this->motor_packet->xpos;
this->motor_packet->y_offset = ((int)rint(set_odom_req->pose.py*1e3))
! + this->motor_packet->ypos;
! this->motor_packet->angle_offset =
((int)rint(RTOA(set_odom_req->pose.pa)))
! + 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);
***************
*** 1291,1295 ****
}
! ResetRawPositions();
this->Publish(this->position_id, resp_queue,
--- 1291,1299 ----
}
! this->motor_packet->x_offset = this->motor_packet->xpos;
! this->motor_packet->y_offset = this->motor_packet->ypos;;
! this->motor_packet->angle_offset = this->motor_packet->angle;;
! printf("Resetting odometry offsets: %d %d %d\n",
motor_packet->xpos,
! motor_packet->ypos,
motor_packet->angle);
this->Publish(this->position_id, resp_queue,
***************
*** 1419,1426 ****
speedDemand = (int)rint(cmd.velocity * 1e3); // convert to mm/s
angleDemand = (int)rint(RTOD(cmd.angle)); // convert to deg heading
! angleDemand -= this->motor_packet->angle_offset; // check for angle offset
of odometry
! if (angleDemand > 360) // normalize
angleDemand -= 360;
! if (angleDemand < 0)
angleDemand += 360;
--- 1423,1430 ----
speedDemand = (int)rint(cmd.velocity * 1e3); // convert to mm/s
angleDemand = (int)rint(RTOD(cmd.angle)); // convert to deg heading
! angleDemand += (int)rint(ATOD(this->motor_packet->angle_offset)); // check
for angle offset of odometry
! while (angleDemand > 360) // normalize
angleDemand -= 360;
! while (angleDemand < 0)
angleDemand += 360;
Index: erratic.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.h,v
retrieving revision 1.1.2.10
retrieving revision 1.1.2.11
diff -C2 -d -r1.1.2.10 -r1.1.2.11
*** erratic.h 12 Feb 2007 03:31:21 -0000 1.1.2.10
--- erratic.h 26 Oct 2007 02:33:12 -0000 1.1.2.11
***************
*** 49,52 ****
--- 49,57 ----
#define CPU_VOLTAGE 3.5
+ // angular constants, angular units are 4096 / rev
+ #define ATOR(x) (M_PI * ((double)(x)) / 2048.0)
+ #define ATOD(x) (180.0 * ((double)(x)) / 2048.0)
+ #define RTOA(x) ((short)((x) * 2048.0) / M_PI)
+
// Default max speeds
#define MOTOR_DEF_MAX_SPEED 0.5
Index: motorpacket.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/motorpacket.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
*** motorpacket.cc 8 Aug 2007 06:53:20 -0000 1.1.2.3
--- motorpacket.cc 26 Oct 2007 02:33:13 -0000 1.1.2.4
***************
*** 110,116 ****
// 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);
--- 110,118 ----
// 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);
! angle = (short)(buffer[cnt] | (buffer[cnt+1] << 8)); // keep as 4096 / full
turn
!
cnt += sizeof(short);
***************
*** 145,183 ****
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);
! }
}
--- 147,182 ----
void ErraticMotorPacket::Fill(player_erratic_data_t* data)
{
! // Odometry data
! {
! // initialize position to current offset
! data->position.pos.px = (double)(this->xpos - this->x_offset) / 1e3;
! data->position.pos.py = (double)(this->ypos - 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 = ATOR(this->angle_offset); // convert rotation to radians
! double ax = data->position.pos.px;
! double ay = data->position.pos.py;
! data->position.pos.px = ax * cos(rot) + ay * sin(rot);
! data->position.pos.py = -ax * sin(rot) + ay * cos(rot);
! data->position.pos.pa = ATOR(angle - this->angle_offset);
! }
! else
! data->position.pos.pa = ATOR(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);
! }
}
-------------------------------------------------------------------------
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
