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