double
compute_new_velocity(carmen_simulator_ackerman_config_t *simulator_config)
{
double minimum_time_to_reach_desired_velocity;
double acceleration;
int signal_target_v, signal_v, command_signal;
double target_v, time, time_rest;
signal_target_v = simulator_config->target_v >= 0 ? 1 : -1;
signal_v = simulator_config->v >= 0 ? 1 : -1;
target_v = simulator_config->target_v;
if (signal_target_v != signal_v)
{
target_v = 0;
}
acceleration = get_acceleration(simulator_config->v, target_v, simulator_config);
minimum_time_to_reach_desired_velocity = fabs((target_v - simulator_config->v) / acceleration); // s = v / a, i.e., s = (m/s) / (m/s^2)
command_signal = simulator_config->v < target_v ? 1 : -1;
time = fmin(simulator_config->delta_t, minimum_time_to_reach_desired_velocity);
simulator_config->v += command_signal * acceleration * time;
if (signal_target_v != signal_v)
{
time_rest = simulator_config->delta_t - time;
target_v = simulator_config->target_v;
acceleration = get_acceleration(simulator_config->v, target_v, simulator_config);
minimum_time_to_reach_desired_velocity = fabs((target_v - simulator_config->v) / acceleration); // s = v / a, i.e., s = (m/s) / (m/s^2)
command_signal = simulator_config->v < target_v ? 1 : -1;
time = fmin(time_rest, minimum_time_to_reach_desired_velocity);
simulator_config->v += command_signal * acceleration * time;
}
simulator_config->v = carmen_clamp(
-simulator_config->maximum_speed_reverse,
simulator_config->v,
simulator_config->maximum_speed_forward);
return (simulator_config->v);
}
//equation 1-4 respectively calculates kinimatic equations 1-4
void equation1(float *result) {
float a, b, c; //input storage
a = get_velocity_initial(); //get inputs
b = get_acceleration();
c = get_time();
out = a + (b*c); //calculation
*result = out; //the passed pointer result point to out which stores the calculated result
}
void equation2(float *result) {
float a, b, c, d; //input storage
a = get_position_initial();
b = get_velocity_initial();
c = get_time();
d = get_acceleration();
out = a + (b*c) + (c*c*d/2);
*result = out; //the passed pointer result point to out which stores the calculated result
}
void equation3(float *result) {
float a, b, c, d; //input storage
a = get_velocity_initial();
b = get_acceleration();
c = get_position_final();
d = get_position_initial();
out = sqrt((a*a) + (2*b*(c-d)));
*result = out; //the passed pointer result point to out which stores the calculated result
}
void start_scheduler(void) {
while (1) {
//timer interrupt
if (sampling_flag == 1) {
timer_rst();
if (curr_channel == 1) {
// used to indicate scheduler cycle in GPIO pin RA5
update_status(SYSTEM_START);
PORTAbits.RA5 = !PORTAbits.RA5;
} else if (curr_channel == 100) {
update_status(SYSTEM_GYRO);
done = gyro_task();
} else if (curr_channel == 200) {
update_status(SYSTEM_ACCELERO);
done = get_acceleration();
} else if (curr_channel == 300) {
update_status(SYSTEM_COMPASS);
done = get_compass();
// //done = Compass_test_single();
// } else if (curr_channel == 250) {
} else if (curr_channel == 400) {
update_status(SYSTEM_FUSION);
ComplementaryFilter();
fusion_final();
} else if (curr_channel == 700) {
update_status(SYSTEM_UART);
//done = uart_task();
}
} else {
//update status if a task terminate
if (done && (check_status() == SYSTEM_UART) && TXSTAbits.TRMT) {
update_status(SYSTEM_IDLE);
done = 0;
} else if (done && ((check_status() == SYSTEM_GYRO) ||
check_status() == SYSTEM_ACCELERO ||
check_status() == SYSTEM_COMPASS ||
check_status() == SYSTEM_FUSION
)) {
update_status(SYSTEM_IDLE);
done = 0;
}
}
}
}
static double
get_motion_command_v(double v, double desired_v, double time)
{
double max_acceleration;
double new_v, acceleration; // @@@ nao estou considerando frenagens...
if (time == 0)
return v;
max_acceleration = get_acceleration(v, desired_v, &carmen_robot_ackerman_config);
acceleration = (desired_v - v) / time; // @@@ estou supondo que a velocidade esta sempre abaixo da maxima...
if (fabs(acceleration) > max_acceleration) // @@@ tem que tratar o sinal de acceleration (quande de uma re)
acceleration = max_acceleration * (acceleration / fabs(acceleration));
new_v = v + acceleration * time;
return (new_v);
}
int main(int argc, char **argv){
/*positions of all particles*/
FLOAT *x;
FLOAT *y;
FLOAT *z;
/*velocities of all particles*/
FLOAT *v_x;
FLOAT *v_y;
FLOAT *v_z;
/*accelerations of all particles*/
FLOAT *a_x;
FLOAT *a_y;
FLOAT *a_z;
/*terms for runge-kutta*/
FLOAT *a_x_old;
FLOAT *a_y_old;
FLOAT *a_z_old;
FLOAT *k_1_x;
FLOAT *k_1_y;
FLOAT *k_1_z;
FLOAT *k_1_v_x;
FLOAT *k_1_v_y;
FLOAT *k_1_v_z;
FLOAT *k_2_x;
FLOAT *k_2_y;
FLOAT *k_2_z;
FLOAT *k_2_v_x;
FLOAT *k_2_v_y;
FLOAT *k_2_v_z;
FLOAT *k_3_x;
FLOAT *k_3_y;
FLOAT *k_3_z;
FLOAT *k_3_v_x;
FLOAT *k_3_v_y;
FLOAT *k_3_v_z;
FLOAT *k_4_x;
FLOAT *k_4_y;
FLOAT *k_4_z;
FLOAT *k_4_v_x;
FLOAT *k_4_v_y;
FLOAT *k_4_v_z;
FLOAT *xtemp;
FLOAT *ytemp;
FLOAT *ztemp;
FLOAT *v_xtemp;
FLOAT *v_ytemp;
FLOAT *v_ztemp;
FLOAT *a_xtemp;
FLOAT *a_ytemp;
FLOAT *a_ztemp;
/*masses*/
FLOAT *mass;
/*timestep variables*/
FLOAT h= 0.001;
int n_steps = (int)(100/h);
int n_points = 3;
FLOAT radius = 100.0;
FLOAT unit_mass = 1.0;
FLOAT vel_initial = sqrt((11.0/3.0) * G_GRAV * unit_mass / (sqrt(3.0)*radius));
FLOAT kinetic;
FLOAT potential;
int i,j,k;
/*memory allocation*/
x = get_memory(n_points);
y = get_memory(n_points);
z = get_memory(n_points);
v_x = get_memory(n_points);
v_y = get_memory(n_points);
v_z = get_memory(n_points);
a_x = get_memory(n_points);
a_y = get_memory(n_points);
a_z = get_memory(n_points);
mass = get_memory(n_points);
k_1_x = get_memory(n_points);
k_1_y = get_memory(n_points);
k_1_z = get_memory(n_points);
k_1_v_x = get_memory(n_points);
k_1_v_y = get_memory(n_points);
k_1_v_z = get_memory(n_points);
k_2_x = get_memory(n_points);
k_2_y = get_memory(n_points);
k_2_z = get_memory(n_points);
k_2_v_x = get_memory(n_points);
k_2_v_y = get_memory(n_points);
k_2_v_z = get_memory(n_points);
k_3_x = get_memory(n_points);
k_3_y = get_memory(n_points);
k_3_z = get_memory(n_points);
k_3_v_x = get_memory(n_points);
k_3_v_y = get_memory(n_points);
k_3_v_z = get_memory(n_points);
k_4_x = get_memory(n_points);
k_4_y = get_memory(n_points);
k_4_z = get_memory(n_points);
k_4_v_x = get_memory(n_points);
k_4_v_y = get_memory(n_points);
k_4_v_z = get_memory(n_points);
xtemp = get_memory(n_points);
ytemp = get_memory(n_points);
ztemp = get_memory(n_points);
v_xtemp = get_memory(n_points);
v_ytemp = get_memory(n_points);
v_ztemp = get_memory(n_points);
a_xtemp = get_memory(n_points);
a_ytemp = get_memory(n_points);
a_ztemp = get_memory(n_points);
initialize_pos(x,y,z, n_points, radius);
initialize_vel(v_x,v_y,v_z, n_points, vel_initial, radius);
initialize_mass(mass, n_points, unit_mass);
/*implementation of a second order runge kutta integration*/
FILE *in;
in = fopen("3cuerpos.dat","w");
for(i=0;i<n_steps;i++){
a_x_old = a_x;
a_y_old = a_y;
a_z_old = a_z;
get_acceleration(a_x, a_y, a_z, x, y, z, mass, n_points);
for(j=0;j<n_points;j++){
k_1_x[j] = v_x[j];
k_1_y[j] = v_y[j];
k_1_z[j] = v_z[j];
k_1_v_x[j] = a_x[j];
k_1_v_y[j] = a_y[j];
k_1_v_z[j] = a_z[j];
//FIRST STEP
xtemp[j] = x[j] + (h/2.0)*k_1_x[j];
ytemp[j] = y[j] + (h/2.0)*k_1_y[j];
ztemp[j] = z[j] + (h/2.0)*k_1_z[j];
v_xtemp[j] = v_x[j] + (h/2.0)*k_1_v_x[j];
v_ytemp[j] = v_y[j] + (h/2.0)*k_1_v_y[j];
v_ztemp[j] = v_z[j] + (h/2.0)*k_1_v_z[j];
k_2_x[j] = v_xtemp[j];
k_2_y[j] = v_ytemp[j];
k_2_z[j] = v_ztemp[j];
}
get_acceleration(a_xtemp,a_ytemp,a_ztemp,xtemp,ytemp,ztemp, mass, n_points);
for(j=0;j<n_points;j++){
k_2_v_x[j] = a_xtemp[j];
k_2_v_y[j] = a_ytemp[j];
k_2_v_z[j] = a_ztemp[j];
//SECOND STEP
xtemp[j] = x[j] + (h/2.0)*k_2_x[j];
ytemp[j] = y[j] + (h/2.0)*k_2_y[j];
ztemp[j] = z[j] + (h/2.0)*k_2_z[j];
v_xtemp[j] = v_x[j] + (h/2.0)*k_2_v_x[j];
v_ytemp[j] = v_y[j] + (h/2.0)*k_2_v_y[j];
v_ztemp[j] = v_z[j] + (h/2.0)*k_2_v_z[j];
k_3_x[j] = v_xtemp[j];
k_3_y[j] = v_ytemp[j];
k_3_z[j] = v_ztemp[j];
}
get_acceleration(a_xtemp,a_ytemp,a_ztemp,xtemp,ytemp,ztemp, mass, n_points);
for(j=0;j<n_points;j++){
k_3_v_x[j] = a_xtemp[j];
k_3_v_y[j] = a_ytemp[j];
k_3_v_z[j] = a_ztemp[j];
//THIRD STEP
xtemp[j] = x[j] + h*k_3_x[j];
ytemp[j] = y[j] + h*k_3_y[j];
ztemp[j] = z[j] + h*k_3_z[j];
v_xtemp[j] = v_x[j] + h*k_3_v_x[j];
v_ytemp[j] = v_y[j] + h*k_3_v_y[j];
v_ztemp[j] = v_z[j] + h*k_3_v_z[j];
k_4_x[j] = v_xtemp[j];
k_4_y[j] = v_ytemp[j];
k_4_z[j] = v_ztemp[j];
}
get_acceleration(a_xtemp,a_ytemp,a_ztemp,xtemp,ytemp,ztemp, mass, n_points);
for(j=0;j<n_points;j++){
k_4_v_x[j] = a_xtemp[j];
k_4_v_y[j] = a_ytemp[j];
k_4_v_z[j] = a_ztemp[j];
//FOURTH STEP
x[j] = x[j] + h*(1.0/6.0)*(k_1_x[j]+2*k_2_x[j]+2*k_3_x[j]+k_4_x[j]);
y[j] = y[j] + h*(1.0/6.0)*(k_1_y[j]+2*k_2_y[j]+2*k_3_y[j]+k_4_y[j]);
z[j] = z[j] + h*(1.0/6.0)*(k_1_z[j]+2*k_2_z[j]+2*k_3_z[j]+k_4_z[j]);
v_x[j] = v_x[j] + h*(1.0/6.0)*(k_1_v_x[j]+2*k_2_v_x[j]+2*k_3_v_x[j]+k_4_v_x[j]);
v_y[j] = v_y[j] + h*(1.0/6.0)*(k_1_v_y[j]+2*k_2_v_y[j]+2*k_3_v_y[j]+k_4_v_y[j]);
v_z[j] = v_z[j] + h*(1.0/6.0)*(k_1_v_z[j]+2*k_2_v_z[j]+2*k_3_v_z[j]+k_4_v_z[j]);
}
for(k=0;k<n_points;k++){
fprintf(in," %f %f %f ", x[k], y[k], z[k]);
}
kinetic = get_kinetic(x, y, z, v_x, v_y, v_z, a_x, a_y, a_z, mass, n_points);
potential = get_potential(x, y, z, v_x, v_y, v_z, a_x, a_y, a_z, mass, n_points);
fprintf(in,"%f %f \n",kinetic, potential);
}
fclose(in);
}
