void Global_Variable_Init(void) { odometry(1); Check_Battery(1); /* Variable Initialize */ New_Spline_Point_Arrived = 0; Lost_msg = 0; Lost_msg_Slow_Down = 0; send_clock = 0; Time_Out_Slow_Down = 1; }
void main(void) { unsigned int channel = CHANNEL; unsigned char data = 0x07; // DBGU output configuration TRACE_CONFIGURE(DBGU_STANDARD, 115200, BOARD_MCK); // Configuration PIT (Periodic Interrupt Timer) ConfigurePit(); // Configuration TC (Timer Counter) ConfigureTc(); // Configuration PIO (Paralell In and Out port), Init Interrupt on PIO ConfigureButtons(); ConfigureLeds(); // Configuration Radio Module nRF24L (PIO and SPI), ConfigureButtons must be executed before ConfigureNRF24L(); ConfigureUSART0(); ConfigureUSART1(); //initialize proximity sensor ir_init(); Global_Variable_Init(); while(Timer0Tick<2); // wait until NRF24L01 power up nrf24l01_power_up(True); while(Timer0Tick<4); // wait until NRF24L01 stand by Timer0Tick = 0; //initialize the 24L01 to the debug configuration as RX and auto-ack disabled nrf24l01_initialize_debug(True, nrf_TX_RX_SIZE, False); nrf24l01_write_register(0x06, &data, 1); nrf24l01_set_as_rx(True); Delay_US(130); nrf24l01_set_rf_ch(channel); nrf24l01_flush_rx(); Delay_US(300); while (1) { if(Timer0Tick!=0){ Timer0Tick = 0; Check_Battery(0); odometry(0); ProxRead_m(); Send_Coord(); Delay_US(10000);//give time for the coming message feedbackController(goalx, goaly, goaldist); } Check_Wireless(); }//while }//main
int deadReckoning(void) { unsigned int t = TMR1; // Timing function volatile coordinate_t delta; float WheelSpL = parameter_unicycle.radius_l * get_motor_measures(MOTOR_ZERO).position; float WheelSpR = parameter_unicycle.radius_r * get_motor_measures(MOTOR_ONE).position; float SumSp = WheelSpR + WheelSpL; // Calcolo della somma degli spostamenti delle ruote float DifSp = WheelSpR - WheelSpL; // Calcolo della differenza degli spostamenti delle ruote //PulsEncL = 0; // Flush variabile //PulsEncR = 0; // Flush variabile if (fabs(DifSp) <= parameter_unicycle.sp_min) { delta.theta = 0; delta.space = WheelSpR; delta.x = delta.space * cosTh_old; delta.y = delta.space * sinTh_old; } else if (fabs(SumSp) <= parameter_unicycle.sp_min) { delta.theta = DifSp / parameter_unicycle.wheelbase; coordinate.theta = fmodf(coordinate.theta + delta.theta, 2 * PI); // Angolo normalizzato tra [0,2*PI] sinTh_old = sinf(coordinate.theta); cosTh_old = cosf(coordinate.theta); delta.x = 0; delta.y = 0; delta.space = 0; } else { delta.theta = DifSp / parameter_unicycle.wheelbase; coordinate.theta = fmodf(coordinate.theta + delta.theta, 2 * PI); // Angolo normalizzato tra [0,2*PI] float cosTh_new = cosf(coordinate.theta); float sinTh_new = sinf(coordinate.theta); delta.space = SumSp / 2; float radius = wheel_m * (SumSp / DifSp); delta.x = radius * (sinTh_new - sinTh_old); delta.y = radius * (cosTh_old - cosTh_new); sinTh_old = sinTh_new; cosTh_old = cosTh_new; } // Calculate odometry odometry(delta); return TMR1 - t; // Time of execution }
// Find the fitness for obstacle avoidance of the passed controller double fitfunc(double waypoints[DATASIZE],int its) { double fitness = 0.0; // Fitness of waypoint set int j,i; int left_encoder, right_encoder, old_left_encoder = 0, old_right_encoder = 0; double theta = 0.0, old_theta = 0.0; double x = 0.0, y = 0.0; int k = 0; int ds_l, ds_r, lspeed, rspeed; double fit_ds, fit_speed, fit_totalds, fitness_distance, fitness_stuck, fitness_goal, fitness_waypoints; fit_speed = 0.0; fit_ds = 0.0; fit_totalds = 0.0; int count_stuck = 0; int count_waypoints = 0; int count_waypoints_actual = 0; fitness_goal = 0.333; double waypoints_reached[DATASIZE]; for (i=0; i<DATASIZE; i++) { waypoints_reached[i] = 0.0; } wb_differential_wheels_set_encoders(0,0); // Evaluate fitness repeatedly for (j=0; j<its; j++) { // if close enough to current waypoint select next waypoint if ((x - waypoints[k])*(x - waypoints[k])+(y - waypoints[k+1])*(y - waypoints[k+1]) < 0.0001) { //printf("WAYPOINT REACHED (%.2f,%.2f) YESSS \n",waypoints[k], waypoints[k+1]); count_waypoints += 1; count_waypoints_actual += 1; waypoints_reached[k] = waypoints[k]; waypoints_reached[k+1] = waypoints[k+1]; k += 2; loc_state = ALIGN; if (k == DATASIZE) { k = 0; } } // compute wheel speeds compute_wheel_speeds(x,y,theta,waypoints[k],waypoints[k+1],&lspeed,&rspeed); wb_differential_wheels_set_speed(lspeed,rspeed); robot_step(128); // run one step // compute current position left_encoder = wb_differential_wheels_get_left_encoder(); right_encoder = wb_differential_wheels_get_right_encoder(); ds_l = left_encoder - old_left_encoder; ds_r = right_encoder - old_right_encoder; odometry(ds_l, ds_r, x, y, old_theta, &x, &y, &theta, &fit_ds); old_left_encoder = left_encoder; old_right_encoder = right_encoder; old_theta = theta; fit_speed += (fabs(lspeed) + fabs(rspeed))/(2.0*MAX_SPEED); fit_totalds += fit_ds; if(fit_ds < 0.0001) { count_stuck += 1; } if (y > 1.2) { // when this coordinate is reached the robot is considered escaped and the evaluation stops printf("\nROBOT ESCAPED!! :)\n"); for (i=0; i<DATASIZE; i++) { printf("%f ",waypoints_reached[i]); } printf("\n"); count_waypoints = 5; //this acts as a bonus for reaching the goal fitness_goal = 1.0; break; } } fit_speed /= its; fitness_distance = 1/(1+exp(fit_totalds-4)); fitness_stuck = (1 - (double)count_stuck/its); fitness_goal = fitness_goal; fitness_waypoints = pow((((double)count_waypoints/10)+0.5),2); //1.2 are assumed to be the length of sides of the world fitness = pow(fitness_distance,0.75)*pow(fitness_stuck,1.5)*fitness_goal*fitness_waypoints; /* if (fitness > fitness_best) { fitness_best = fitness; printf("\nFITNESS: %f\nTotal Distance: %.2f\nStuck Ratio: %.2f\n\n", fitness, fit_totalds, fitness_stuck); } */ printf("FITNESS: %f waypoints: %d ditance: %.2f stuck: %.2f\n", fitness, count_waypoints_actual, fit_totalds, fitness_stuck); return fitness; }
int main() { char motor_aux_info[] = {LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT}; //Only one robot can be created at the time Robot *p_robot = new Ev3(PERIOD, TRACK, ENCODER_SCALE_FACTOR, motor_aux_info); //Odometry only //Robot *p_robot = new Xg1300lGyro(PERIOD, TRACK, ENCODER_SCALE_FACTOR, motor_aux_info, (char *)&GYRO_PORT); //Microinfinity XG1300L gyro //Robot *p_robot = new LegoGyro(PERIOD, TRACK, ENCODER_SCALE_FACTOR, motor_aux_info, (char *)&GYRO_PORT); //Lego EV3 gyro Odometry odometry(p_robot); Control control(&odometry); InputKeys *p_keyboard = new Keyboard; //Create and initialize speed variables float speed = 0; float rate = 0; bool quit_program = false; //Enter main loop while(!quit_program) { //Read sensors p_robot->readSensors(); //Compute position odometry.updatePosition(); //Control instructions //User defined instructions using keyboard switch(p_keyboard->getKey()) { case MOVE_FORWARD: speed += INC_SPEED_MM_SECOND; break; case MOVE_BACKWARDS: speed -= INC_SPEED_MM_SECOND; break; case TURN_LEFT: rate += INC_RATE_RAD_SECOND; break; case TURN_RIGHT: rate -= INC_RATE_RAD_SECOND; break; case ENABLE_CONTROL: control.enable(); break; case EXIT: quit_program = true; case RESET: odometry.reset(); control.reset(); case STOP_ROBOT: speed = 0; rate = 0; break; } //High level control instructions control.getTargetSpeedRate(speed, rate); //Execute the instructions p_robot->setActuators(speed, rate); } //Free used memory before exiting delete p_keyboard; delete p_robot; return 1; }
// This gets called by the world update start event. void GazeboOdometryPlugin::OnUpdate(const common::UpdateInfo& _info) { // C denotes child frame, P parent frame, and W world frame. // Further C_pose_W_P denotes pose of P wrt. W expressed in C. math::Pose W_pose_W_C = link_->GetWorldCoGPose(); math::Vector3 C_linear_velocity_W_C = link_->GetRelativeLinearVel(); math::Vector3 C_angular_velocity_W_C = link_->GetRelativeAngularVel(); math::Vector3 gazebo_linear_velocity = C_linear_velocity_W_C; math::Vector3 gazebo_angular_velocity = C_angular_velocity_W_C; math::Pose gazebo_pose = W_pose_W_C; if (parent_frame_id_ != "world") { math::Pose W_pose_W_P = parent_link_->GetWorldPose(); math::Vector3 P_linear_velocity_W_P = parent_link_->GetRelativeLinearVel(); math::Vector3 P_angular_velocity_W_P = parent_link_->GetRelativeAngularVel(); math::Pose C_pose_P_C_ = W_pose_W_C - W_pose_W_P; math::Vector3 C_linear_velocity_P_C; // \prescript{}{C}{\dot{r}}_{PC} = -R_{CP} // \cdot \prescript{}{P}{\omega}_{WP} \cross \prescript{}{P}{r}_{PC} // + \prescript{}{C}{v}_{WC} // - R_{CP} \cdot \prescript{}{P}{v}_{WP} C_linear_velocity_P_C = - C_pose_P_C_.rot.GetInverse() * P_angular_velocity_W_P.Cross(C_pose_P_C_.pos) + C_linear_velocity_W_C - C_pose_P_C_.rot.GetInverse() * P_linear_velocity_W_P; // \prescript{}{C}{\omega}_{PC} = \prescript{}{C}{\omega}_{WC} // - R_{CP} \cdot \prescript{}{P}{\omega}_{WP} gazebo_angular_velocity = C_angular_velocity_W_C - C_pose_P_C_.rot.GetInverse() * P_angular_velocity_W_P; gazebo_linear_velocity = C_linear_velocity_P_C; gazebo_pose = C_pose_P_C_; } bool publish_odometry = true; // First, determine whether we should publish a odometry. if (covariance_image_.data != NULL) { // We have an image. // Image is always centered around the origin: int width = covariance_image_.cols; int height = covariance_image_.rows; int x = static_cast<int>(std::floor(gazebo_pose.pos.x / covariance_image_scale_)) + width / 2; int y = static_cast<int>(std::floor(gazebo_pose.pos.y / covariance_image_scale_)) + height / 2; if (x >= 0 && x < width && y >= 0 && y < height) { uint8_t pixel_value = covariance_image_.at<uint8_t>(y, x); if (pixel_value == 0) { publish_odometry = false; // TODO: covariance scaling, according to the intensity values could be implemented here. } } } if (gazebo_sequence_ % measurement_divisor_ == 0) { nav_msgs::Odometry odometry; odometry.header.frame_id = parent_frame_id_; odometry.header.seq = odometry_sequence_++; odometry.header.stamp.sec = (world_->GetSimTime()).sec + ros::Duration(unknown_delay_).sec; odometry.header.stamp.nsec = (world_->GetSimTime()).nsec + ros::Duration(unknown_delay_).nsec; odometry.child_frame_id = namespace_; copyPosition(gazebo_pose.pos, &odometry.pose.pose.position); odometry.pose.pose.orientation.w = gazebo_pose.rot.w; odometry.pose.pose.orientation.x = gazebo_pose.rot.x; odometry.pose.pose.orientation.y = gazebo_pose.rot.y; odometry.pose.pose.orientation.z = gazebo_pose.rot.z; odometry.twist.twist.linear.x = gazebo_linear_velocity.x; odometry.twist.twist.linear.y = gazebo_linear_velocity.y; odometry.twist.twist.linear.z = gazebo_linear_velocity.z; odometry.twist.twist.angular.x = gazebo_angular_velocity.x; odometry.twist.twist.angular.y = gazebo_angular_velocity.y; odometry.twist.twist.angular.z = gazebo_angular_velocity.z; if (publish_odometry) odometry_queue_.push_back(std::make_pair(gazebo_sequence_ + measurement_delay_, odometry)); } // Is it time to publish the front element? if (gazebo_sequence_ == odometry_queue_.front().first) { nav_msgs::OdometryPtr odometry(new nav_msgs::Odometry); odometry->header = odometry_queue_.front().second.header; odometry->child_frame_id = odometry_queue_.front().second.child_frame_id; odometry->pose.pose = odometry_queue_.front().second.pose.pose; odometry->twist.twist.linear = odometry_queue_.front().second.twist.twist.linear; odometry->twist.twist.angular = odometry_queue_.front().second.twist.twist.angular; odometry_queue_.pop_front(); // Calculate position distortions. Eigen::Vector3d pos_n; pos_n << position_n_[0](random_generator_) + position_u_[0](random_generator_), position_n_[1](random_generator_) + position_u_[1](random_generator_), position_n_[2](random_generator_) + position_u_[2](random_generator_); geometry_msgs::Point& p = odometry->pose.pose.position; p.x += pos_n[0]; p.y += pos_n[1]; p.z += pos_n[2]; // Calculate attitude distortions. Eigen::Vector3d theta; theta << attitude_n_[0](random_generator_) + attitude_u_[0](random_generator_), attitude_n_[1](random_generator_) + attitude_u_[1](random_generator_), attitude_n_[2](random_generator_) + attitude_u_[2](random_generator_); Eigen::Quaterniond q_n = QuaternionFromSmallAngle(theta); q_n.normalize(); geometry_msgs::Quaternion& q_W_L = odometry->pose.pose.orientation; Eigen::Quaterniond _q_W_L(q_W_L.w, q_W_L.x, q_W_L.y, q_W_L.z); _q_W_L = _q_W_L * q_n; q_W_L.w = _q_W_L.w(); q_W_L.x = _q_W_L.x(); q_W_L.y = _q_W_L.y(); q_W_L.z = _q_W_L.z(); // Calculate linear velocity distortions. Eigen::Vector3d linear_velocity_n; linear_velocity_n << linear_velocity_n_[0](random_generator_) + linear_velocity_u_[0](random_generator_), linear_velocity_n_[1](random_generator_) + linear_velocity_u_[1](random_generator_), linear_velocity_n_[2](random_generator_) + linear_velocity_u_[2](random_generator_); geometry_msgs::Vector3& linear_velocity = odometry->twist.twist.linear; linear_velocity.x += linear_velocity_n[0]; linear_velocity.y += linear_velocity_n[1]; linear_velocity.z += linear_velocity_n[2]; // Calculate angular veocity distortions. Eigen::Vector3d angular_velocity_n; angular_velocity_n << angular_velocity_n_[0](random_generator_) + angular_velocity_u_[0](random_generator_), angular_velocity_n_[1](random_generator_) + angular_velocity_u_[1](random_generator_), angular_velocity_n_[2](random_generator_) + angular_velocity_u_[2](random_generator_); geometry_msgs::Vector3& angular_velocity = odometry->twist.twist.angular; angular_velocity.x += angular_velocity_n[0]; angular_velocity.y += angular_velocity_n[1]; angular_velocity.z += angular_velocity_n[2]; odometry->pose.covariance = pose_covariance_matrix_; odometry->twist.covariance = twist_covariance_matrix_; // Publish all the topics, for which the topic name is specified. if (pose_pub_.getNumSubscribers() > 0) { geometry_msgs::PoseStampedPtr pose(new geometry_msgs::PoseStamped); pose->header = odometry->header; pose->pose = odometry->pose.pose; pose_pub_.publish(pose); } if (pose_with_covariance_pub_.getNumSubscribers() > 0) { geometry_msgs::PoseWithCovarianceStampedPtr pose_with_covariance( new geometry_msgs::PoseWithCovarianceStamped); pose_with_covariance->header = odometry->header; pose_with_covariance->pose.pose = odometry->pose.pose; pose_with_covariance->pose.covariance = odometry->pose.covariance; pose_with_covariance_pub_.publish(pose_with_covariance); } if (position_pub_.getNumSubscribers() > 0) { geometry_msgs::PointStampedPtr position(new geometry_msgs::PointStamped); position->header = odometry->header; position->point = p; position_pub_.publish(position); } if (transform_pub_.getNumSubscribers() > 0) { geometry_msgs::TransformStampedPtr transform(new geometry_msgs::TransformStamped); transform->header = odometry->header; geometry_msgs::Vector3 translation; translation.x = p.x; translation.y = p.y; translation.z = p.z; transform->transform.translation = translation; transform->transform.rotation = q_W_L; transform_pub_.publish(transform); } if (odometry_pub_.getNumSubscribers() > 0) { odometry_pub_.publish(odometry); } tf::Quaternion tf_q_W_L(q_W_L.x, q_W_L.y, q_W_L.z, q_W_L.w); tf::Vector3 tf_p(p.x, p.y, p.z); tf_ = tf::Transform(tf_q_W_L, tf_p); transform_broadcaster_.sendTransform(tf::StampedTransform(tf_, odometry->header.stamp, parent_frame_id_, namespace_)); // std::cout << "published odometry with timestamp " << odometry->header.stamp << "at time t" << world_->GetSimTime().Double() // << "delay should be " << measurement_delay_ << "sim cycles" << std::endl; } ++gazebo_sequence_; }