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); }