float CalculateManipulatedValue(PidParameter* const pid)
{
const float error = pid->desiredValue - pid->currentValue;
const float dt = pid->dt;
const float maxLimitValue = pid->manipulatedValueLimit;
const float minLimitValue = -maxLimitValue;
float proportionalTerm = pid->proportionalGain * error;
float integralTerm = pid->integralTerm + pid->integralGain * error * dt;
float derivativeTerm = pid->derivativeGain * (error - pid->previousError)
/ dt;
//proportionalTerm = BOUNDARY(proportionalTerm, minLimitValue, maxLimitValue);
integralTerm = BOUNDARY(integralTerm, minLimitValue, maxLimitValue);
//derivativeTerm = BOUNDARY(derivativeTerm, minLimitValue, maxLimitValue);
float manipulatedValue = proportionalTerm + integralTerm + derivativeTerm;
manipulatedValue = BOUNDARY(manipulatedValue, minLimitValue, maxLimitValue);
//for debuging//
pid->error = error;
pid->proportionalTerm = proportionalTerm;
pid->derivativeTerm = derivativeTerm;
pid->mainpulatedValue = manipulatedValue;
////////////////
pid->integralTerm = integralTerm;
pid->previousError = error;
return manipulatedValue;
}
void SetMotorPwmTo(uint16_t id, float motor0, float motor1)
{
int motor0Pwm = INTEGER(motor0);
int motor1Pwm = INTEGER(motor1);
motor0Pwm = BOUNDARY(motor0Pwm, -999, 999);
motor1Pwm = BOUNDARY(motor1Pwm, -999, 999);
motor0Pwm = abs(motor0Pwm);
motor1Pwm = abs(motor1Pwm);
static uint8_t message[13];
message[0] = '<';
message[1] = '0';
message[2] = 'R';
message[3] = motor0 > 0 ? '1' : '0';
message[4] = (motor0Pwm / 100) % 10 + '0';
message[5] = (motor0Pwm / 10) % 10 + '0';
message[6] = (motor0Pwm) % 10 + '0';
message[7] = 'L';
message[8] = motor1 > 0 ? '1' : '0';
message[9] = (motor1Pwm / 100) % 10 + '0';
message[10] = (motor1Pwm / 10) % 10 + '0';
message[11] = (motor1Pwm) % 10 + '0';
message[12] = '>';
if (IsI2c2OnGoing() == TRUE)
return;
I2c2WriteRequest(id, message, sizeof(message));
}
void
end_send_port( int i, int j, int k,
const grid_t * g ) {
int port = BOUNDARY( i, j, k), dst = g->bc[port];
if( dst<0 || dst>=world_size ) return;
mp_end_send( g->mp, BOUNDARY(i,j,k) );
}
void
begin_recv_port( int i, int j, int k,
int size,
const grid_t * g ) {
int port = BOUNDARY(-i,-j,-k), src = g->bc[port];
if( src<0 || src>=world_size ) return;
mp_size_recv_buffer( g->mp, BOUNDARY(-i,-j,-k), size );
mp_begin_recv( g->mp, port, size, src, BOUNDARY(i,j,k) );
}
end_recv_port( int i, int j, int k,
const grid_t * g ) {
int port = BOUNDARY(-i,-j,-k), src = g->bc[port];
if( src<0 || src>=world_size ) return NULL;
mp_end_recv( g->mp, port );
return mp_recv_buffer( g->mp, port );
}
void
begin_send_port( int i, int j, int k,
int size,
const grid_t * g ) {
int port = BOUNDARY( i, j, k), dst = g->bc[port];
if( dst<0 || dst>=world_size ) return;
mp_begin_send( g->mp, port, size, dst, port );
}
size_send_port( int i, int j, int k,
int size,
const grid_t * g ) {
int port = BOUNDARY( i, j, k), dst = g->bc[port];
if( dst<0 || dst>=world_size ) return NULL;
mp_size_send_buffer( g->mp, port, size );
return mp_send_buffer( g->mp, port );
}
int BlurEngine::transfer_pixels(pixel_f *src1,
pixel_f *src2,
pixel_f *src,
double *radius,
pixel_f *dest,
int size)
{
int i;
double sum;
// printf("BlurEngine::transfer_pixels %d %d %d %d\n",
// plugin->config.r,
// plugin->config.g,
// plugin->config.b,
// plugin->config.a);
for(i = 0; i < size; i++)
{
sum = src1[i].r + src2[i].r;
BOUNDARY(sum);
dest[i].r = sum;
sum = src1[i].g + src2[i].g;
BOUNDARY(sum);
dest[i].g = sum;
sum = src1[i].b + src2[i].b;
BOUNDARY(sum);
dest[i].b = sum;
sum = src1[i].a + src2[i].a;
BOUNDARY(sum);
dest[i].a = sum;
// if(radius[i] < 2)
// {
// double scale = 2.0 - (radius[i] * radius[i] - 2.0);
// dest[i].r /= scale;
// dest[i].g /= scale;
// dest[i].b /= scale;
// dest[i].a /= scale;
// }
}
return 0;
}
grid_t *
new_grid( void ) {
grid_t *g;
int i;
MALLOC( g, 1 );
CLEAR( g, 1 );
for( i=0; i<27; i++ ) g->bc[i] = anti_symmetric_fields;
g->bc[BOUNDARY(0,0,0)] = world_rank;
g->mp = new_mp( 27 );
REGISTER_OBJECT( g, checkpt_grid, restore_grid, NULL );
return g;
}
/* Clear all exits for one room */
void clear_room( int x, int y )
{
int dir, exitx, exity;
/* Cycle through the four directions */
for( dir = 0; dir < 4; dir++ )
{
/* Find next coord in this direction */
get_exit_dir( dir, &exitx, &exity, x, y );
/* If coord is valid, clear it */
if ( !BOUNDARY( exitx, exity ) ) clear_coord( exitx, exity );
}
}
void
partition_periodic_box( grid_t * g,
double gx0, double gy0, double gz0,
double gx1, double gy1, double gz1,
int gnx, int gny, int gnz,
int gpx, int gpy, int gpz ) {
double f;
int rank, px, py, pz;
// Make sure the grid can be setup
if( !g ) ERROR(( "NULL grid" ));
if( gpx<1 || gpy<1 || gpz<1 || gpx*gpy*gpz!=world_size )
ERROR(( "Bad domain decompostion (%ix%ix%i)", gpx, gpy, gpz ));
if( gnx<1 || gny<1 || gnz<1 || gnx%gpx!=0 || gny%gpy!=0 || gnz%gpz!=0 )
ERROR(( "Bad resolution (%ix%ix%i) for domain decomposition",
gnx, gny, gnz, gpx, gpy, gpz ));
// Setup basic variables
RANK_TO_INDEX( world_rank, px,py,pz );
g->dx = (gx1-gx0)/(double)gnx;
g->dy = (gy1-gy0)/(double)gny;
g->dz = (gz1-gz0)/(double)gnz;
g->dV = ((gx1-gx0)/(double)gnx)*
((gy1-gy0)/(double)gny)*
((gz1-gz0)/(double)gnz);
g->rdx = (double)gnx/(gx1-gx0);
g->rdy = (double)gny/(gy1-gy0);
g->rdz = (double)gnz/(gz1-gz0);
g->r8V = ((double)gnx/(gx1-gx0))*
((double)gny/(gy1-gy0))*
((double)gnz/(gz1-gz0))*0.125;
f = (double) px /(double)gpx; g->x0 = gx0*(1-f) + gx1*f;
f = (double) py /(double)gpy; g->y0 = gy0*(1-f) + gy1*f;
f = (double) pz /(double)gpz; g->z0 = gz0*(1-f) + gz1*f;
f = (double)(px+1)/(double)gpx; g->x1 = gx0*(1-f) + gx1*f;
f = (double)(py+1)/(double)gpy; g->y1 = gy0*(1-f) + gy1*f;
f = (double)(pz+1)/(double)gpz; g->z1 = gz0*(1-f) + gz1*f;
// Size the local grid
size_grid(g,gnx/gpx,gny/gpy,gnz/gpz);
// Join the grid to neighbors
INDEX_TO_RANK(px-1,py, pz, rank); join_grid(g,BOUNDARY(-1, 0, 0),rank);
INDEX_TO_RANK(px, py-1,pz, rank); join_grid(g,BOUNDARY( 0,-1, 0),rank);
INDEX_TO_RANK(px, py, pz-1,rank); join_grid(g,BOUNDARY( 0, 0,-1),rank);
INDEX_TO_RANK(px+1,py, pz, rank); join_grid(g,BOUNDARY( 1, 0, 0),rank);
INDEX_TO_RANK(px, py+1,pz, rank); join_grid(g,BOUNDARY( 0, 1, 0),rank);
INDEX_TO_RANK(px, py, pz+1,rank); join_grid(g,BOUNDARY( 0, 0, 1),rank);
}
/*
* rexmit: is the specified segment a retransmit?
* returns: number of retransmitted bytes in segment, 0 if not a rexmit
* *pout_order to to TRUE if segment is out of order
*/
int
rexmit (tcb * ptcb, seqnum seq, seglen len, Bool * pout_order,
u_short this_ip_id)
{
seqspace *sspace = ptcb->ss;
seqnum seq_last = seq + len - 1;
quadrant *pquad;
int rexlen = 0;
/* unless told otherwise, it's IN order */
*pout_order = FALSE;
/* see which quadrant it starts in */
pquad = whichquad (sspace, seq);
/* add the new segment into the segment database */
if (BOUNDARY (seq, seq_last))
{
/* lives in two different quadrants (can't be > 2) */
seqnum seq1, seq2;
u_long len1, len2;
/* in first quadrant */
seq1 = seq;
len1 = LAST_SEQ (QUADNUM (seq1)) - seq1 + 1;
rexlen = addseg (ptcb, pquad, seq1, len1, pout_order, this_ip_id);
/* in second quadrant */
seq2 = FIRST_SEQ (QUADNUM (seq_last));
len2 = len - len1;
rexlen +=
addseg (ptcb, pquad->next, seq2, len2, pout_order, this_ip_id);
}
else
{
rexlen = addseg (ptcb, pquad, seq, len, pout_order, this_ip_id);
}
return (rexlen);
}
void MotorControlProcess()
{
if (isOnTimeForMotorControlProcess == FALSE)
return;
else
isOnTimeForMotorControlProcess = FALSE;
float manipulatedValues[CONTROL_MOTOR_COUNT] =
{ 0.0f, 0.0f };
int64_t currentEcnoderValues[CONTROL_MOTOR_COUNT] =
{ motorEncoders[0].encoderCount, motorEncoders[1].encoderCount };
int i;
for (i = 0; i < CONTROL_MOTOR_COUNT; i++)
{
const float controlDesiredValue =
motorControlTypes[i].controlDeisredValue;
const float dt = motorControlTypes[i].pid.dt;
const float acceleration =
motorControlTypes[i].velocityParameter.acceleration;
const float maxVelocity =
motorControlTypes[i].velocityParameter.maximumVelocity;
const float velocityUnit = acceleration * dt;
const float pulsePerRotation = motorControlTypes[i].pulsePerRotation;
const float currentVelocity = (currentEcnoderValues[i]
- motorControlTypes[i].previousEncoder) * 360.0f
/ (pulsePerRotation * dt); //degree/s
const float averageVelocity = CalculateMovingAverage(
&motorControlTypes[i].movingAverageVelocitySamples,
currentVelocity); //degree/s
const uint32_t time = motorControlTypes[i].time;
switch (motorControlTypes[i].controlMode)
{
case CONTROL_VELOCITY:
{
float startVelocity = motorControlTypes[i].startVelocity;
uint32_t desiredTime = motorControlTypes[i].desiredTime;
if (time == 0)
{
startVelocity = averageVelocity;
desiredTime = fabsf(controlDesiredValue - startVelocity)
/ acceleration / dt;
motorControlTypes[i].startVelocity = startVelocity;
motorControlTypes[i].desiredTime = desiredTime;
}
float acceledDesiredVelocity = 0.0f;
if (time < desiredTime)
{
acceledDesiredVelocity = startVelocity
+ (controlDesiredValue - startVelocity > 0 ? 1.0 : -1.0)
* velocityUnit * time;
}
else
{
acceledDesiredVelocity = controlDesiredValue;
}
motorControlTypes[i].pid.desiredValue = acceledDesiredVelocity;
motorControlTypes[i].pid.currentValue = averageVelocity;
manipulatedValues[i] = CalculateManipulatedValue(
&motorControlTypes[i].pid);
}
break;
case CONTROL_POSITION:
{
const float currentPosition = currentEcnoderValues[i] * 360.0f
/ pulsePerRotation;
const float positionError = controlDesiredValue - currentPosition;
const float absOfPositionError = fabsf(positionError);
const float positionErrorSign = positionError > 0.0f ? 1.0f : -1.0f;
uint32_t desiredTime = motorControlTypes[i].desiredTime;
float acceledDesiredVelocity = 0.0f;
if (time > desiredTime)
{
acceledDesiredVelocity =
absOfPositionError
< motorControlTypes[i].positionErrorLimit ?
0.0 :
positionErrorSign
* sqrtf(
2 * acceleration
* absOfPositionError);
acceledDesiredVelocity =
BOUNDARY(acceledDesiredVelocity, -maxVelocity, maxVelocity);
}
else
{
float startVelocity = motorControlTypes[i].startVelocity;
if (time == 0)
{
const float desiredVelocity = positionErrorSign
* maxVelocity;
startVelocity =
BOUNDARY(averageVelocity, -maxVelocity, maxVelocity);
uint32_t startAreaDistance = fabsf(
(desiredVelocity - startVelocity)
* (desiredVelocity + startVelocity)
/ (2 * acceleration));
uint32_t endAreaDistance = desiredVelocity * desiredVelocity
/ (2 * acceleration);
if (absOfPositionError
< startAreaDistance + endAreaDistance)
{
float desiredFloatTime =
fabsf(
(-2 * startVelocity
+ sqrtf(
2 * startVelocity
* startVelocity
+ 4
* acceleration
* absOfPositionError))
/ (2 * acceleration) / dt);
desiredTime = INTEGER(desiredFloatTime);
}
else
{
float desiredFloatTime = fabsf(
desiredVelocity - startVelocity) / acceleration
/ dt;
desiredTime = INTEGER(desiredFloatTime);
}
motorControlTypes[i].startVelocity = startVelocity;
motorControlTypes[i].desiredTime = desiredTime;
}
acceledDesiredVelocity = startVelocity
+ positionErrorSign * velocityUnit * time;
}
motorControlTypes[i].pid.desiredValue = acceledDesiredVelocity;
motorControlTypes[i].pid.currentValue = averageVelocity;
manipulatedValues[i] = CalculateManipulatedValue(
&motorControlTypes[i].pid);
}
break;
case CONTROL_PID_TUNING:
{
motorControlTypes[i].pid.desiredValue = controlDesiredValue;
motorControlTypes[i].pid.currentValue = currentVelocity;
manipulatedValues[i] = CalculateManipulatedValue(
&motorControlTypes[i].pid);
}
break;
default:
manipulatedValues[i] = 0.0f;
break;
}
if (motorControlTypes[i].time != 0xFFFFFFFF)
motorControlTypes[i].time += 1;
motorControlTypes[i].previousEncoder = currentEcnoderValues[i];
motorControlTypes[i].averageVelocity = averageVelocity;
}
//manipulatedValues[0] = -100;
SetMotorPwmTo(NT_S_DCDM1210_0_I2C_ADRESS, manipulatedValues[0],
manipulatedValues[1]);
//SetMotorPwmTo(0x08, manipulatedValues[2], manipulatedValues[3]);
}
/* This function is recursive, ie it calls itself */
void map_exits( CHAR_DATA * ch, ROOM_INDEX_DATA * pRoom, int x, int y, int depth )
{
static char map_chars[11] = "|-|-UD/\\\\/";
int door;
int exitx = 0, exity = 0;
int roomx = 0, roomy = 0;
EXIT_DATA *pExit;
/*
* Setup this coord as a room - Change any symbols that can't be displayed here
*/
dmap[x][y].sector = pRoom->sector_type;
switch ( pRoom->sector_type )
{
case SECT_INSIDE:
dmap[x][y].tegn = 'O';
dmap[x][y].sector = -1;
break;
case SECT_CITY:
dmap[x][y].tegn = ':';
break;
case SECT_FIELD:
case SECT_FOREST:
case SECT_HILLS:
dmap[x][y].tegn = '*';
break;
case SECT_MOUNTAIN:
dmap[x][y].tegn = '@';
break;
case SECT_WATER_SWIM:
case SECT_WATER_NOSWIM:
dmap[x][y].tegn = '=';
break;
case SECT_AIR:
dmap[x][y].tegn = '~';
break;
case SECT_DESERT:
dmap[x][y].tegn = '+';
break;
default:
dmap[x][y].tegn = 'O';
dmap[x][y].sector = -1;
bug( "%s: Bad sector type (%d) in room %d.", __FUNCTION__, pRoom->sector_type, pRoom->vnum );
break;
}
dmap[x][y].vnum = pRoom->vnum;
dmap[x][y].depth = depth;
// dmap[x][y].info = pRoom->room_flags;
dmap[x][y].can_see = room_is_dark( pRoom );
/*
* Limit recursion
*/
if ( depth > MAXDEPTH )
return;
/*
* This room is done, deal with it's exits
*/
for ( door = 0; door < 10; ++door )
{
/*
* Skip if there is no exit in this direction
*/
if ( !( pExit = get_exit( pRoom, door ) ) )
continue;
/*
* Skip up and down until I can figure out a good way to display it
*/
if ( door == 4 || door == 5 )
continue;
/*
* Get the coords for the next exit and room in this direction
*/
get_exit_dir( door, &exitx, &exity, x, y );
get_exit_dir( door, &roomx, &roomy, exitx, exity );
/*
* Skip if coords fall outside map
*/
if ( BOUNDARY( exitx, exity ) || BOUNDARY( roomx, roomy ) )
continue;
/*
* Skip if there is no room beyond this exit
*/
if ( !pExit->to_room )
continue;
/*
* Ensure there are no clashes with previously defined rooms
*/
if ( ( dmap[roomx][roomy].vnum != 0 ) && ( dmap[roomx][roomy].vnum != pExit->to_room->vnum ) )
{
/*
* Use the new room if the depth is higher
*/
if ( dmap[roomx][roomy].depth <= depth )
continue;
/*
* It is so clear the old room
*/
clear_room( roomx, roomy );
}
/*
* No exits at MAXDEPTH
*/
if ( depth == MAXDEPTH )
continue;
/*
* No need for exits that are already mapped
*/
if ( dmap[exitx][exity].depth > 0 )
continue;
/*
* Fill in exit
*/
dmap[exitx][exity].depth = depth;
dmap[exitx][exity].vnum = pExit->to_room->vnum;
// dmap[exitx][exity].info = pExit->exit_info;
dmap[exitx][exity].tegn = map_chars[door];
dmap[exitx][exity].sector = -1;
/*
* More to do? If so we recurse
*/
if ( depth < MAXDEPTH && ( ( dmap[roomx][roomy].vnum == pExit->to_room->vnum ) || ( dmap[roomx][roomy].vnum == 0 ) ) )
{
/*
* Depth increases by one each time
*/
map_exits( ch, pExit->to_room, roomx, roomy, depth + 1 );
}
}
}
void
partition_absorbing_box( grid_t * g,
double gx0, double gy0, double gz0,
double gx1, double gy1, double gz1,
int gnx, int gny, int gnz,
int gpx, int gpy, int gpz,
int pbc ) {
int px, py, pz;
partition_periodic_box( g,
gx0, gy0, gz0,
gx1, gy1, gz1,
gnx, gny, gnz,
gpx, gpy, gpz );
// Override periodic boundary conditions
RANK_TO_INDEX( world_rank, px,py,pz );
if( px==0 && gnx>1 ) {
set_fbc(g,BOUNDARY(-1,0,0),absorb_fields);
set_pbc(g,BOUNDARY(-1,0,0),pbc);
}
if( px==gpx-1 && gnx>1 ) {
set_fbc(g,BOUNDARY( 1,0,0),absorb_fields);
set_pbc(g,BOUNDARY( 1,0,0),pbc);
}
if( py==0 && gny>1 ) {
set_fbc(g,BOUNDARY(0,-1,0),absorb_fields);
set_pbc(g,BOUNDARY(0,-1,0),pbc);
}
if( py==gpy-1 && gny>1 ) {
set_fbc(g,BOUNDARY(0, 1,0),absorb_fields);
set_pbc(g,BOUNDARY(0, 1,0),pbc);
}
if( pz==0 && gnz>1 ) {
set_fbc(g,BOUNDARY(0,0,-1),absorb_fields);
set_pbc(g,BOUNDARY(0,0,-1),pbc);
}
if( pz==gpz-1 && gnz>1 ) {
set_fbc(g,BOUNDARY(0,0, 1),absorb_fields);
set_pbc(g,BOUNDARY(0,0, 1),pbc);
}
}
/* This function is recursive, ie it calls itself */
void map_exits(CHAR_DATA *ch, ROOM_INDEX_DATA *pRoom, int x, int y, int depth)
{
static char map_chars [4] = "|-|-";
int door;
int exitx = 0, exity = 0;
int roomx = 0, roomy = 0;
char buf[200]; // bugs
EXIT_DATA *pExit;
/* Setup this coord as a room */
switch(pRoom->sector_type)
{
case SECT_CITY:
case SECT_INSIDE:
case SECT_UNUSED:
map[x][y].tegn = 'O';
break;
case SECT_FIELD:
case SECT_FOREST:
case SECT_HILLS:
map[x][y].tegn = '*';
break;
case SECT_MOUNTAIN:
map[x][y].tegn = '@';
break;
case SECT_WATER_SWIM:
case SECT_WATER_NOSWIM:
map[x][y].tegn = '=';
break;
case SECT_AIR:
map[x][y].tegn = '~';
break;
case SECT_DESERT:
map[x][y].tegn = '+';
break;
default:
map[x][y].tegn = 'O';
xprintf(buf, "Map_exits: Bad sector type (%d) in room %d.",
pRoom->sector_type, pRoom->vnum);
bug(buf, 0);
break;
}
map[x][y].vnum = pRoom->vnum;
map[x][y].depth = depth;
map[x][y].info = pRoom->room_flags;
map[x][y].can_see = room_is_dark( pRoom );
/* Limit recursion */
if ( depth > MAXDEPTH ) return;
/* This room is done, deal with it's exits */
for( door = 0; door < 4; door++ )
{
/* Skip if there is no exit in this direction */
if ( ( pExit = pRoom->exit[door] ) == NULL ) continue;
/* Get the coords for the next exit and room in this direction */
get_exit_dir( door, &exitx, &exity, x, y );
get_exit_dir( door, &roomx, &roomy, exitx, exity );
/* Skip if coords fall outside map */
if ( BOUNDARY( exitx, exity ) || BOUNDARY( roomx, roomy )) continue;
/* Skip if there is no room beyond this exit */
if ( pExit->to_room == NULL ) continue;
/* Ensure there are no clashes with previously defined rooms */
if ( ( map[roomx][roomy].vnum != 0 ) &&
( map[roomx][roomy].vnum != pExit->to_room->vnum ))
{
/* Use the new room if the depth is higher */
if ( map[roomx][roomy].depth <= depth ) continue;
/* It is so clear the old room */
clear_room( roomx, roomy );
}
/* No exits at MAXDEPTH */
if ( depth == MAXDEPTH ) continue;
/* No need for exits that are already mapped */
if ( map[exitx][exity].depth > 0 ) continue;
/* Fill in exit */
map[exitx][exity].depth = depth;
map[exitx][exity].vnum = pExit->to_room->vnum;
map[exitx][exity].info = pExit->exit_info;
map[exitx][exity].tegn = map_chars[door];
/* More to do? If so we recurse */
if ( ( depth < MAXDEPTH ) &&
( ( map[roomx][roomy].vnum == pExit->to_room->vnum ) ||
( map[roomx][roomy].vnum == 0 ) ) )
{
/* Depth increases by one each time */
map_exits( ch, pExit->to_room, roomx, roomy, depth + 1 );
}
}
}