void Physics::RK4_step(real_t dt, real_t fraction, real_t weight) {
set_forces(dt); //param actually unnecessary?
for (size_t i = 0; i < num_spheres(); i++) {
spheres[i]->state.dx += weight
* spheres[i]->step_position(dt, fraction, collision_damping);
spheres[i]->state.dv += weight * dt
* spheres[i]->get_acceleration();
spheres[i]->state.dax += weight
* spheres[i]->step_orientation(dt, fraction, collision_damping);
spheres[i]->state.dav += weight * dt
* spheres[i]->get_angular_acceleration();
}
}
static void read_tables(FILE *fp,const char *fn,
int ntab,int angle,t_tabledata td[])
{
char *libfn;
char buf[STRLEN];
double **yy=NULL,start,end,dx0,dx1,ssd,vm,vp,f,numf;
int k,i,nx,nx0=0,ny,nny,ns;
gmx_bool bAllZero,bZeroV,bZeroF;
double tabscale;
nny = 2*ntab+1;
libfn = gmxlibfn(fn);
nx = read_xvg(libfn,&yy,&ny);
if (ny != nny)
gmx_fatal(FARGS,"Trying to read file %s, but nr columns = %d, should be %d",
libfn,ny,nny);
if (angle == 0) {
if (yy[0][0] != 0.0)
gmx_fatal(FARGS,
"The first distance in file %s is %f nm instead of %f nm",
libfn,yy[0][0],0.0);
} else {
if (angle == 1)
start = 0.0;
else
start = -180.0;
end = 180.0;
if (yy[0][0] != start || yy[0][nx-1] != end)
gmx_fatal(FARGS,"The angles in file %s should go from %f to %f instead of %f to %f\n",
libfn,start,end,yy[0][0],yy[0][nx-1]);
}
tabscale = (nx-1)/(yy[0][nx-1] - yy[0][0]);
if (fp) {
fprintf(fp,"Read user tables from %s with %d data points.\n",libfn,nx);
if (angle == 0)
fprintf(fp,"Tabscale = %g points/nm\n",tabscale);
}
bAllZero = TRUE;
for(k=0; k<ntab; k++) {
bZeroV = TRUE;
bZeroF = TRUE;
for(i=0; (i < nx); i++) {
if (i >= 2) {
dx0 = yy[0][i-1] - yy[0][i-2];
dx1 = yy[0][i] - yy[0][i-1];
/* Check for 1% deviation in spacing */
if (fabs(dx1 - dx0) >= 0.005*(fabs(dx0) + fabs(dx1))) {
gmx_fatal(FARGS,"In table file '%s' the x values are not equally spaced: %f %f %f",fn,yy[0][i-2],yy[0][i-1],yy[0][i]);
}
}
if (yy[1+k*2][i] != 0) {
bZeroV = FALSE;
if (bAllZero) {
bAllZero = FALSE;
nx0 = i;
}
if (yy[1+k*2][i] > 0.01*GMX_REAL_MAX ||
yy[1+k*2][i] < -0.01*GMX_REAL_MAX) {
gmx_fatal(FARGS,"Out of range potential value %g in file '%s'",
yy[1+k*2][i],fn);
}
}
if (yy[1+k*2+1][i] != 0) {
bZeroF = FALSE;
if (bAllZero) {
bAllZero = FALSE;
nx0 = i;
}
if (yy[1+k*2+1][i] > 0.01*GMX_REAL_MAX ||
yy[1+k*2+1][i] < -0.01*GMX_REAL_MAX) {
gmx_fatal(FARGS,"Out of range force value %g in file '%s'",
yy[1+k*2+1][i],fn);
}
}
}
if (!bZeroV && bZeroF) {
set_forces(fp,angle,nx,1/tabscale,yy[1+k*2],yy[1+k*2+1],k);
} else {
/* Check if the second column is close to minus the numerical
* derivative of the first column.
*/
ssd = 0;
ns = 0;
for(i=1; (i < nx-1); i++) {
vm = yy[1+2*k][i-1];
vp = yy[1+2*k][i+1];
f = yy[1+2*k+1][i];
if (vm != 0 && vp != 0 && f != 0) {
/* Take the centered difference */
numf = -(vp - vm)*0.5*tabscale;
ssd += fabs(2*(f - numf)/(f + numf));
ns++;
}
}
if (ns > 0) {
ssd /= ns;
sprintf(buf,"For the %d non-zero entries for table %d in %s the forces deviate on average %d%% from minus the numerical derivative of the potential\n",ns,k,libfn,(int)(100*ssd+0.5));
if (debug)
fprintf(debug,"%s",buf);
if (ssd > 0.2) {
if (fp)
fprintf(fp,"\nWARNING: %s\n",buf);
fprintf(stderr,"\nWARNING: %s\n",buf);
}
}
}
}
if (bAllZero && fp) {
fprintf(fp,"\nNOTE: All elements in table %s are zero\n\n",libfn);
}
for(k=0; (k<ntab); k++) {
init_table(fp,nx,nx0,tabscale,&(td[k]),TRUE);
for(i=0; (i<nx); i++) {
td[k].x[i] = yy[0][i];
td[k].v[i] = yy[2*k+1][i];
td[k].f[i] = yy[2*k+2][i];
}
}
for(i=0; (i<ny); i++)
sfree(yy[i]);
sfree(yy);
sfree(libfn);
}
void *control_stabilizer(void *thread_id)
{
printf("in control stabilizer \n");
U_trim.thrust = 10;
ros::NodeHandle nh;
//ros::Publisher cmd_pub;
ros::Subscriber sonar_sub;
ros::Subscriber imu_sub;
ros::Subscriber xbee_sub;
//cmd_pub = nh.advertise<quad_msgs::MotorCommands>("controller/cmd_motors",1);
sonar_sub = nh.subscribe<quad_msgs::SonarData>("sonar/sonar_data",1,sonarCallback);
imu_sub = nh.subscribe<quad_msgs::ImuData>("imu/imu_data",1,imuCallback);
xbee_sub = nh.subscribe<quad_msgs::XbeeData>("xbee/xbee_cmds",1,xbeeCallback);
dynamic_reconfigure::Server<controller::controllerConfig> server;
dynamic_reconfigure::Server<controller::controllerConfig>::CallbackType f;
f = boost::bind(&gainsCallback, _1, _2);
server.setCallback(f);
ros::Time start;
ros::Time end;
ros::Duration dur;
//ros::AsyncSpinner spinner(6); // Use 4 threads
//spinner.start();
double freq;
Distances sonar_distances;
Distances repulsive_forces;
State_Error vicon_error = {0.0};
Vicon desired_velocity = {0.0};
times.delta.tv_nsec = delta_time; //500000;
if(DEBUG){
nh.setParam("m0_cmd",700);
nh.setParam("m1_cmd",700);
nh.setParam("m2_cmd",700);
nh.setParam("m3_cmd",700);
}
while(ros::ok())
{
start = ros::Time::now();
if (VICON_OR_JOY == 1)
{
printf("in vicon part");
if(new_xbee_data)
{
//calculate error from vicon
//xbee.error_vicon(vicon_error, desired_velocity, desired_positions);
//calculate desired attitude (phi theta phi) in desired_angles
desired_angles_calc(desired_angles, vicon_error, gains);
}
}
else
{
Angles old_desired_angles = desired_angles;
ros::spinOnce();
/*if(KILL_MOTORS){
quad_msgs::MotorCommands mcs;
mcs.m0 = 0;
mcs.m1 = 0;
mcs.m2 = 0;
mcs.m3 = 0;
cmd_pub.publish(mcs);
ROS_INFO("KILLING MOTORS: KILL_MOTORS = true\n");
}*/
/*if(new_xbee_data < 0) ; //printf("joystick not ready to read: old data");
//check flight mode
if(ESTOP)
{
if(flight_mode < 11.0)
{
//printf("FLIGHT MODE: OFF %i \n", flight_mode);
CONTROLLER_RUN = false;
}
else if (flight_mode > 15.0 && flight_mode < 17.0) AUTO_HEIGHT = false;
else if (flight_mode > 17.0) AUTO_HEIGHT = true;
} */
}
//calculate error from imu (in radians) between desired and measured state
State imu_error = error_imu(desired_angles);
//calculate thrust and desired acceleration
U = thrust(imu_error,vicon_error, U_trim, joystick_thrust, gains);
motor_commands = set_forces(U,Ct,d);
/*quad_msgs::MotorCommands mcs;
//calculate the forces of each motor and change force on motor objects and send via i2c
if(!DEBUG) {
double* motor_commands;
motor_commands = set_forces(U,Ct,d);
mcs.header.stamp = ros::Time::now();
mcs.m0 = motor_commands[0];
mcs.m1 = motor_commands[1];
mcs.m2 = motor_commands[2];
mcs.m3 = motor_commands[3];
} else {
double m0c, m1c, m2c,m3c;
nh.getParam("m0_cmd",m0c);
nh.getParam("m1_cmd",m1c);
nh.getParam("m2_cmd",m2c);
nh.getParam("m3_cmd",m3c);
mcs.header.stamp = ros::Time::now();
mcs.m0 = m0c;
mcs.m1 = m1c;
mcs.m2 = m2c;
mcs.m3 = m3c;
} */
//cmd_pub.publish(mcs);
end = ros::Time::now();
dur = end - start;
freq = 1/(dur.toSec());
//ROS_INFO("%f", freq);
if(freq < 200) ROS_WARN("frequency below 200: %f \n ", freq); //check frequencie
}
printf("EXIT CONTROL_STABILIZER\n");
}
