actuator.cpp

00001 /*
00002  * Author    : Garudago KRAI ITB 2019
00003  * Developer : Calmantara Sumpono Putra 
00004  * Version   : 1.0.0 
00005  */
00006 
00007 #include "mbed.h"
00008 #include "actuator.h"
00009 #include <garudago_conf/config.h>
00010 
00011 Base::Base(){
00012     //motor and encoder distance from center
00013     _motor_distance_from_center =  motor_distance_from_center;
00014     _encoder_distance_from_center = encoder_distance_from_center;
00015     //limitation and tolerance
00016     _position_tolerance = position_tolerance;
00017     _orientation_tolerance = orientation_tolerance;
00018     _max_velocity_magnitude = max_velocity_magnitude;
00019     _max_rotation_magnitude = max_rotation_magnitude;
00020     _cutoff_velocity_magnitude = cutoff_velocity_magnitude;
00021     _cutoff_rotation_magnitude = cutoff_rotation_magnitude;
00022     //limitation 
00023     _max_linear_acceleration_magnitude = max_linear_acceleration_magnitude;
00024     _max_linear_deceleration_magnitude = max_linear_deceleration_magnitude;
00025     _max_angular_acceleration_magnitude = max_angular_acceleration_magnitude;
00026     _max_angular_deceleration_magnitude = max_angular_deceleration_magnitude;
00027     //feedforward constant
00028     _linear_acceleration_feedforward_factor = linear_acceleration_feedforward_factor;
00029     _angular_acceleration_feedforward_factor = angular_acceleration_feedforward_factor;
00030     _linear_deceleration_feedforward_factor = linear_deceleration_feedforward_factor;
00031     _angular_deceleration_feedforward_factor = angular_deceleration_feedforward_factor;
00032 }
00033 
00034 float *Base::update_state(float wheel_right_position,
00035                         float wheel_left_position,
00036                         float wheel_back_position){
00037 //procedure to get current position of robot
00038     float *_get_geometry; 
00039     static float _geometry[3];
00040     _get_geometry = _base_position_from_wheels(wheel_right_position,
00041                                                 wheel_left_position,
00042                                                 wheel_back_position);
00043     _geometry[0] = _get_geometry[0];
00044     _geometry[1] = _get_geometry[1];
00045     _geometry[2] = _get_geometry[2];
00046     return _geometry;
00047 }
00048 
00049 float *Base::compute_action(float x_robot,
00050                             float y_robot,
00051                             float theta, 
00052                             float x_target,
00053                             float y_target,
00054                             float theta_target){
00055 //function to get set point value for each motor
00056     float _linear_velocity;
00057     float _rotation;
00058     float _alpha;
00059     float *_get_wheels;
00060     static float _wheels[3];
00061 
00062     _linear_velocity = compute_velocity(x_robot, 
00063                                         y_robot,
00064                                         x_target, 
00065                                         y_target);
00066 
00067     _rotation = compute_rotation(theta, theta_target);
00068 
00069     _alpha = _compute_alpha(x_robot, 
00070                             y_target,
00071                             theta,
00072                             x_target,
00073                             y_target);
00074 
00075     _get_wheels = _wheels_from_base(_linear_velocity,
00076                                     _rotation,
00077                                     _alpha);
00078 
00079     _wheels[0] = _get_wheels[0];
00080     _wheels[1] = _get_wheels[1];
00081     _wheels[2] = _get_wheels[2];
00082     _prev_feedforward_rotation_command = _rotation;
00083     _prev_feedforward_linear_command = _linear_velocity;
00084 
00085     return _wheels;
00086 }
00087 
00088 float *Base::manual_movement(float theta, 
00089                             float theta_target,
00090                             float linear,
00091                             float alpha){
00092     float _rotation;
00093     float *_get_wheels;
00094     static float _wheels[4];
00095 
00096     _rotation = compute_rotation(theta, theta_target);
00097     _get_wheels = _wheels_from_base(linear, _rotation, alpha);
00098 
00099     _wheels[0] = _get_wheels[0];
00100     _wheels[1] = _get_wheels[1];
00101     _wheels[2] = _get_wheels[2];
00102     _wheels[3] = _get_wheels[3];
00103 
00104     return _wheels;
00105 }
00106 
00107 float Base::compute_velocity(float x_robot,
00108                             float y_robot,
00109                             float x_target,
00110                             float y_target){
00111 //function to compute linear velocity of auto mode robot
00112     return 0;
00113 }
00114 
00115 float Base::compute_rotation(float theta,
00116                              float theta_target){
00117 //function to compute rotation of auto mode robot
00118     if(fabs(theta_target - theta)<=orientation_tolerance &&
00119        (fabs(_prev_feedforward_rotation_command) < _cutoff_rotation_magnitude)){
00120         _feedforward_rotation_command = 0;
00121         _last_orientation = theta;
00122     } 
00123     
00124     else{
00125         //compute velocity from origin and target position
00126         if(theta_target > theta){
00127             _rotation_from_s1 = sqrt(2 * _max_angular_acceleration_magnitude * 
00128                                 fabs(theta - _last_orientation));
00129             _rotation_from_s2 = sqrt(2 * _max_angular_deceleration_magnitude * 
00130                                 fabs(theta_target - theta));
00131         } else{
00132             _rotation_from_s1 = -sqrt(2 * _max_angular_acceleration_magnitude * 
00133                                 fabs(theta - _last_orientation));
00134             _rotation_from_s2 = -sqrt(2 * _max_angular_deceleration_magnitude * 
00135                                 fabs(theta_target - theta));
00136         }
00137 
00138         //acceleration or deceleration
00139         _prev_target_rotation_command = _target_rotation_command;
00140         if(_rotation_from_s1 <= _rotation_from_s2){ 
00141             _target_rotation_command = _rotation_from_s1;
00142         }
00143         else if(_rotation_from_s2 < _rotation_from_s1){
00144             _target_rotation_command = _rotation_from_s2;
00145         }
00146 
00147         //feedforward command
00148         float _delta_rotation_command = fabs(_target_rotation_command) - 
00149                                         fabs(_prev_target_rotation_command);
00150         if(_delta_rotation_command > 0){
00151             _feedforward_rotation_command = _prev_target_rotation_command + 
00152                 (angular_acceleration_feedforward_factor * _delta_rotation_command);
00153         } else{
00154             _feedforward_rotation_command = _prev_target_rotation_command + 
00155                 (angular_deceleration_feedforward_factor * _delta_rotation_command);
00156         }
00157 
00158         //limitation for command
00159         if(fabs(_feedforward_rotation_command) > max_angular_acceleration_magnitude){
00160             _feedforward_rotation_command = max_angular_acceleration_magnitude;
00161         }
00162 
00163         //condition to make correct sign
00164         if(theta_target < theta){
00165             _feedforward_rotation_command = -_feedforward_rotation_command;
00166         }
00167     }
00168     return _feedforward_rotation_command;   
00169 }
00170 
00171 float Base::_compute_alpha(float x_robot,
00172                         float y_robot,
00173                         float theta,
00174                         float x_target,
00175                         float y_target){
00176 //function to compute robot movement orientation
00177     float _alpha = atan((y_target-y_robot)/(x_target-x_robot)) - theta;
00178     
00179     if(x_target < x_robot)  return _alpha + PI;
00180     else    return _alpha;
00181 }
00182 
00183 float *Base::_base_position_from_wheels(float wheel_right,
00184                                         float wheel_left,
00185                                         float wheel_back){
00186 //funstion to transform robot position from three wheels
00187     static float _geometry[3];
00188     float _y = sqrt(3.0) / 3.0 * (wheel_right - wheel_left);
00189     float _x = (1.0 / 3.0 * (wheel_left + wheel_right)) - (2.0 / 3.0 * wheel_back);
00190     float _theta = (1.0 / (3.0 * _encoder_distance_from_center)) * \
00191                     (wheel_right + wheel_back + wheel_left);
00192 
00193     x_actual += cos(_theta)*_x - sin(_theta)*_y;
00194     y_actual += sin(_theta)*_x + cos(_theta)*_y;
00195     theta_actual += _theta;
00196     
00197     _geometry[0] = x_actual;
00198     _geometry[1] = y_actual;
00199     _geometry[2] = theta_actual;
00200     return _geometry;
00201 }
00202 
00203 float *Base::_base_velocity_from_wheels(float wheel_right_top,
00204                                         float wheel_left_top,
00205                                         float wheel_right_back,
00206                                         float wheel_left_back){
00207 
00208     return 0;
00209 }
00210 
00211 float *Base::_wheels_from_base(float linear,
00212                             float rotation,
00213                             float alpha){
00214 //funtion to compute set point for each motor
00215 
00216     static float _motor[4];
00217     _motor[0] = (-linear*cos(alpha)*1.4142/2) - (linear*sin(alpha)*1.4142/2) + \
00218                (rotation*_motor_distance_from_center);   //for left top motor
00219     _motor[1] = (-linear*cos(alpha)*1.4142/2) + (linear*sin(alpha)*1.4142/2) + \
00220                (rotation*_motor_distance_from_center);   //for right top motor
00221     _motor[2] = (linear*cos(alpha)*1.4142/2) - (linear*sin(alpha)*1.4142/2) + \
00222                (rotation*_motor_distance_from_center);   //for left back motor
00223     _motor[3] = (linear*cos(alpha)*1.4142/2) + (linear*sin(alpha)*1.4142/2) + \
00224                (rotation*_motor_distance_from_center);   //for right back motor
00225     
00226     return _motor;
00227 }