volksbot.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Osnabrueck University
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  * * Redistributions of source code must retain the above copyright notice,
00009  *   this list of conditions and the following disclaimer.
00010  * * Redistributions in binary form must reproduce the above copyright notice,
00011  *   this list of conditions and the following disclaimer in the documentation
00012  *   and/or other materials provided with the distribution.
00013  * * Neither the name of the Osnabrueck University nor the names of its
00014  *   contributors may be used to endorse or promote products derived from this
00015  *   software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/console.h>
00031 
00032 #include <algorithm>
00033 #include <cmath>
00034 #include <cstdio>
00035 #include <limits>
00036 #include <stdint.h>
00037 
00038 #include "comm.h"
00039 #include "volksbot.h"
00040 
00041 Volksbot::Volksbot(
00042         Comm &comm,
00043         double wheel_radius,
00044         double axis_length,
00045         double turning_adaptation,
00046         int gear_ratio,
00047         int max_vel_l,
00048         int max_vel_r,
00049         int max_acc_l,
00050         int max_acc_r,
00051         bool drive_backwards) :
00052       epos2_left_(drive_backwards ? 0x01 : 0x02),
00053       epos2_right_(drive_backwards ? 0x02 : 0x01),
00054       comm_(comm),
00055       wheel_radius_(wheel_radius),
00056       axis_length_(axis_length),
00057       turning_adaptation_(turning_adaptation),
00058       gear_ratio_(gear_ratio),
00059       max_vel_l_(max_vel_l),
00060       max_vel_r_(max_vel_r),
00061       max_acc_l_(max_acc_l),
00062       max_acc_r_(max_acc_r)
00063 {
00064   epos2_left_.init();
00065   epos2_right_.init();
00066 
00067   epos2_left_.enableController();
00068   epos2_right_.enableController();
00069 
00070   epos2_left_.enableMotor(epos2_left_.VELOCITY);
00071   epos2_right_.enableMotor(epos2_right_.VELOCITY);
00072 
00073   epos2_left_.setProfileData(
00074       0, // Velocity
00075       max_vel_l_, // Max Velocity
00076       0, // Acceleration
00077       0, // Deceleration
00078       0, // QS Decel
00079       max_acc_l_, // Max acc
00080       0 // Type: Trapecoidal
00081       );
00082 
00083   epos2_right_.setProfileData(
00084       0, // Velocity
00085       max_vel_r_, // Max Velocity
00086       0, // Acceleration
00087       0, // Deceleration
00088       0, // QS Decel
00089       max_acc_r_, // Max acc
00090       0 // Type: Trapecoidal
00091       );
00092 
00093   epos2_left_.setOperationMode(epos2_left_.VELOCITY);
00094   epos2_right_.setOperationMode(epos2_right_.VELOCITY);
00095 }
00096 
00097 Volksbot::~Volksbot()
00098 {
00099   epos2_left_.setTargetVelocity(0.0);
00100   epos2_right_.setTargetVelocity(0.0);
00101   epos2_left_.close();
00102   epos2_right_.close();
00103 }
00104 
00105 double Volksbot::get_max_vel(){
00106   return (max_vel_r_ + max_vel_l_) * M_PI * wheel_radius_ / (60.0 * gear_ratio_);
00107 }
00108 
00109 void Volksbot::set_wheel_speed(double _v_l_soll, double _v_r_soll)
00110 {
00111   epos2_left_.setTargetVelocity(_v_l_soll / ( 2.0 * M_PI * wheel_radius_) * 60.0 * gear_ratio_);
00112   epos2_right_.setTargetVelocity(-1.0 * _v_r_soll / ( 2.0 * M_PI * wheel_radius_) * 60.0 * gear_ratio_);
00113 }
00114 
00115 void Volksbot::odometry()
00116 {
00117   static double x = 0.0;
00118   static double y = 0.0;
00119   static double theta = 0.0;
00120   static long enc_left_last = epos2_left_.readEncoderCounter();
00121   static long enc_right_last = epos2_right_.readEncoderCounter();
00122   static long enc_per_turn_left = 4 * epos2_left_.getEncoderPulseNumber() * gear_ratio_;
00123   static long enc_per_turn_right = 4 * epos2_right_.getEncoderPulseNumber() * gear_ratio_;
00124 
00125   long enc_left = epos2_left_.readEncoderCounter();
00126   long enc_right = epos2_right_.readEncoderCounter();
00127   long wheel_l = enc_left - enc_left_last;
00128   long wheel_r = enc_right - enc_right_last;
00129 
00130   // handle overflow (> 10000 required to ensure we don't do this on zero crossings)
00131   if((abs(enc_left) > 10000) && (std::signbit(enc_left) != std::signbit(enc_left_last)))
00132   {
00133     if(std::signbit(enc_left))
00134       wheel_l = std::numeric_limits<int32_t>::max() - enc_left_last - std::numeric_limits<int32_t>::min() + enc_left;
00135     else
00136       wheel_l = std::numeric_limits<int32_t>::max() - enc_left - std::numeric_limits<int32_t>::min() + enc_left_last;
00137   }
00138 
00139   if((abs(enc_right) > 10000) && (std::signbit(enc_right) != std::signbit(enc_right_last)))
00140   {
00141     if(std::signbit(enc_right))
00142       wheel_r = std::numeric_limits<int32_t>::max() - enc_right_last - std::numeric_limits<int32_t>::min() + enc_right;
00143     else
00144       wheel_r = std::numeric_limits<int32_t>::max() - enc_right - std::numeric_limits<int32_t>::min() + enc_right_last;
00145   }
00146 
00147   enc_left_last = enc_left;
00148   enc_right_last = enc_right;
00149 
00150   double wheel_L = 2.0 * M_PI * wheel_radius_ * wheel_l / enc_per_turn_left;
00151   double wheel_R = -2.0 * M_PI * wheel_radius_ * wheel_r / enc_per_turn_right;
00152 
00153   double dtheta = (wheel_R - wheel_L) / axis_length_ * turning_adaptation_;
00154   double hypothenuse = 0.5 * (wheel_L + wheel_R);
00155 
00156   x += hypothenuse * cos(theta + dtheta * 0.5);
00157   y += hypothenuse * sin(theta + dtheta * 0.5);
00158   theta += dtheta;
00159 
00160   if (theta > M_PI)
00161     theta -= 2.0 * M_PI;
00162   if (theta < -M_PI)
00163     theta += 2.0 * M_PI;
00164 
00165   double v_left = epos2_left_.readVelocity() / 60.0 / gear_ratio_ * 2.0 * M_PI * wheel_radius_;
00166   double v_right = -epos2_right_.readVelocity() / 60.0 / gear_ratio_ * 2.0 * M_PI * wheel_radius_;
00167   double v_x = (v_left + v_right) * 0.5;
00168   double v_theta = (v_right - v_left) / axis_length_ * turning_adaptation_;
00169 
00170   double wheelpos_l = 2.0 * M_PI * (enc_left % enc_per_turn_left) / enc_per_turn_left;
00171   if (wheelpos_l > M_PI)
00172     wheelpos_l -= 2.0 * M_PI;
00173   if (wheelpos_l < -M_PI)
00174     wheelpos_l += 2.0 * M_PI;
00175 
00176   double wheelpos_r = 2.0 * M_PI * (enc_right % enc_per_turn_right) / enc_per_turn_right;
00177   if (wheelpos_r > M_PI)
00178     wheelpos_r -= 2.0 * M_PI;
00179   if (wheelpos_r < -M_PI)
00180     wheelpos_r += 2.0 * M_PI;
00181 
00182   comm_.send_odometry(x, y, theta, v_x, v_theta, wheelpos_l, wheelpos_r);
00183 }


volksbot_driver
Author(s): Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 19:17:33