Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/Motion/motion.cpp@54:99d3158c9207, 2013-04-12 (annotated)

Committer:
rsavitski
Date:
Fri Apr 12 20:58:59 2013 +0000
Revision:
54:99d3158c9207
Parent:
52:bffe5f7c39a3
Child:
57:d434ceab6892
crappy movement around cake

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rsavitski 24:50805ef8c499 1 ////////////////////////////////////////////////////////////////////////////////
rsavitski 24:50805ef8c499 2 // Motion control unit
rsavitski 24:50805ef8c499 3 ////////////////////////////////////////////////////////////////////////////////
rsavitski 24:50805ef8c499 4 // Takes current state of the robot and target waypoint,
rsavitski 24:50805ef8c499 5 // calculates desired forward and angular velocities and requests those from the motor control layer.
rsavitski 24:50805ef8c499 6 ////////////////////////////////////////////////////////////////////////////////
rsavitski 24:50805ef8c499 7
rsavitski 24:50805ef8c499 8 #include "motion.h"
rsavitski 38:c9058a401410 9
rsavitski 24:50805ef8c499 10 namespace motion
rsavitski 24:50805ef8c499 11 {
rsavitski 24:50805ef8c499 12
rsavitski 39:44d3dea4adcc 13 Mutex waypoint_flag_mutex;
rsavitski 39:44d3dea4adcc 14
rsavitski 39:44d3dea4adcc 15 Waypoint *current_waypoint = NULL;
rsavitski 39:44d3dea4adcc 16
rsavitski 39:44d3dea4adcc 17 bool waypoint_reached = false; // is current waypoint reached
rsavitski 40:fefdbb9b5968 18 bool d_reached = false;
rsavitski 40:fefdbb9b5968 19 bool a_reached = false;
rsavitski 39:44d3dea4adcc 20
rsavitski 24:50805ef8c499 21 void motionlayer(void const *dummy)
rsavitski 30:791739422122 22 {
rsavitski 39:44d3dea4adcc 23 // save target waypoint
rsavitski 39:44d3dea4adcc 24 Waypoint target_waypoint = *current_waypoint;
rsavitski 24:50805ef8c499 25
rsavitski 24:50805ef8c499 26 // get current state from Kalman
rsavitski 24:50805ef8c499 27 State current_state = Kalman::getState();
rsavitski 24:50805ef8c499 28
rsavitski 24:50805ef8c499 29 float delta_x = target_waypoint.x - current_state.x;
rsavitski 24:50805ef8c499 30 float delta_y = target_waypoint.y - current_state.y;
rsavitski 24:50805ef8c499 31
madcowswe 25:b16f1045108f 32 //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y);
rsavitski 24:50805ef8c499 33
madcowswe 25:b16f1045108f 34 float distance_err = hypot(delta_x, delta_y);
madcowswe 25:b16f1045108f 35
madcowswe 25:b16f1045108f 36 float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
rsavitski 24:50805ef8c499 37
rsavitski 52:bffe5f7c39a3 38 // reversing
rsavitski 52:bffe5f7c39a3 39 bool reversing = false;
rsavitski 52:bffe5f7c39a3 40 if ((abs(angle_err) > PI/2) && (distance_err < 0.2))
rsavitski 52:bffe5f7c39a3 41 {
rsavitski 52:bffe5f7c39a3 42 reversing = true;
rsavitski 52:bffe5f7c39a3 43 angle_err = constrainAngle(angle_err + PI);
rsavitski 52:bffe5f7c39a3 44 distance_err = -distance_err;
rsavitski 52:bffe5f7c39a3 45 }
rsavitski 52:bffe5f7c39a3 46
rsavitski 52:bffe5f7c39a3 47 float angle_err_saved = angle_err; // actuated angle error can be overriden by the final turning code, but forward speed envelope should be controlled with the atan angle
rsavitski 52:bffe5f7c39a3 48
rsavitski 30:791739422122 49 // is the waypoint reached
rsavitski 39:44d3dea4adcc 50 waypoint_flag_mutex.lock();
rsavitski 52:bffe5f7c39a3 51 if (abs(distance_err) < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
rsavitski 30:791739422122 52 {
rsavitski 37:6ecf0d21e492 53 d_reached = true;
rsavitski 52:bffe5f7c39a3 54 //distance_err = 0;
rsavitski 38:c9058a401410 55
rsavitski 52:bffe5f7c39a3 56 angle_err = constrainAngle(target_waypoint.theta - current_state.theta);
rsavitski 38:c9058a401410 57
rsavitski 38:c9058a401410 58 if (abs(angle_err) < target_waypoint.angle_threshold)
madcowswe 33:a49197572737 59 {
rsavitski 37:6ecf0d21e492 60 a_reached = true;
rsavitski 39:44d3dea4adcc 61 //angle_err = 0;
madcowswe 33:a49197572737 62 }
rsavitski 35:f8e7f0a72a3d 63 }
rsavitski 39:44d3dea4adcc 64 waypoint_flag_mutex.unlock();
rsavitski 30:791739422122 65
rsavitski 39:44d3dea4adcc 66 waypoint_flag_mutex.lock(); // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
rsavitski 37:6ecf0d21e492 67 if (d_reached && a_reached)
rsavitski 30:791739422122 68 {
rsavitski 39:44d3dea4adcc 69 setWaypointReached();
rsavitski 30:791739422122 70 }
rsavitski 39:44d3dea4adcc 71 waypoint_flag_mutex.unlock();
rsavitski 24:50805ef8c499 72
rsavitski 24:50805ef8c499 73 // angular velocity controller
madcowswe 34:e1678450feec 74 const float p_gain_av = 0.5; //TODO: tune
rsavitski 24:50805ef8c499 75
madcowswe 33:a49197572737 76 const float max_av = 0.5*PI; // radians per sec //TODO: tune
rsavitski 24:50805ef8c499 77
rsavitski 24:50805ef8c499 78 // angle error [-pi, pi]
rsavitski 24:50805ef8c499 79 float angular_v = p_gain_av * angle_err;
rsavitski 24:50805ef8c499 80
rsavitski 24:50805ef8c499 81 // constrain range
rsavitski 24:50805ef8c499 82 if (angular_v > max_av)
rsavitski 24:50805ef8c499 83 angular_v = max_av;
rsavitski 24:50805ef8c499 84 else if (angular_v < -max_av)
rsavitski 24:50805ef8c499 85 angular_v = -max_av;
rsavitski 24:50805ef8c499 86
rsavitski 24:50805ef8c499 87
rsavitski 24:50805ef8c499 88 // forward velocity controller
rsavitski 54:99d3158c9207 89 const float p_gain_fv = 0.25; //TODO: tune
rsavitski 24:50805ef8c499 90
madcowswe 25:b16f1045108f 91 float max_fv = 0.2; // meters per sec //TODO: tune
rsavitski 52:bffe5f7c39a3 92 float max_fv_reverse = 0.05; //TODO: tune
rsavitski 54:99d3158c9207 93 const float angle_envelope_exponent = 512;//32.0; //8.0; //TODO: tune
rsavitski 24:50805ef8c499 94
rsavitski 24:50805ef8c499 95 // control, distance_err in meters
rsavitski 24:50805ef8c499 96 float forward_v = p_gain_fv * distance_err;
rsavitski 24:50805ef8c499 97
rsavitski 52:bffe5f7c39a3 98 // if reversing, robot is less stable hence a different cap is used
rsavitski 52:bffe5f7c39a3 99 if (reversing)
rsavitski 52:bffe5f7c39a3 100 max_fv = max_fv_reverse;
rsavitski 52:bffe5f7c39a3 101
rsavitski 24:50805ef8c499 102 // control the forward velocity envelope based on angular error
rsavitski 52:bffe5f7c39a3 103 max_fv = max_fv * pow(cos(angle_err_saved/2), angle_envelope_exponent);
rsavitski 24:50805ef8c499 104
rsavitski 24:50805ef8c499 105 // constrain range
rsavitski 24:50805ef8c499 106 if (forward_v > max_fv)
rsavitski 24:50805ef8c499 107 forward_v = max_fv;
rsavitski 24:50805ef8c499 108 else if (forward_v < -max_fv)
rsavitski 24:50805ef8c499 109 forward_v = -max_fv;
madcowswe 25:b16f1045108f 110
madcowswe 25:b16f1045108f 111 //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
rsavitski 24:50805ef8c499 112
rsavitski 30:791739422122 113 // pass values to the motor control
rsavitski 24:50805ef8c499 114 MotorControl::set_fwdcmd(forward_v);
madcowswe 25:b16f1045108f 115 MotorControl::set_omegacmd(angular_v);
rsavitski 24:50805ef8c499 116 }
rsavitski 24:50805ef8c499 117
rsavitski 39:44d3dea4adcc 118 void setNewWaypoint(Waypoint *new_wp)
rsavitski 39:44d3dea4adcc 119 {
rsavitski 39:44d3dea4adcc 120 current_waypoint = new_wp;
rsavitski 39:44d3dea4adcc 121 }
rsavitski 39:44d3dea4adcc 122
rsavitski 39:44d3dea4adcc 123 void setWaypointReached()
rsavitski 39:44d3dea4adcc 124 {
rsavitski 39:44d3dea4adcc 125 waypoint_reached = true;
rsavitski 39:44d3dea4adcc 126 }
rsavitski 39:44d3dea4adcc 127
rsavitski 39:44d3dea4adcc 128 void clearWaypointReached()
rsavitski 39:44d3dea4adcc 129 {
rsavitski 39:44d3dea4adcc 130 waypoint_reached = false;
rsavitski 40:fefdbb9b5968 131 d_reached = false;
rsavitski 40:fefdbb9b5968 132 a_reached = false;
rsavitski 39:44d3dea4adcc 133 }
rsavitski 39:44d3dea4adcc 134
rsavitski 39:44d3dea4adcc 135 bool checkWaypointStatus()
rsavitski 39:44d3dea4adcc 136 {
rsavitski 39:44d3dea4adcc 137 return waypoint_reached;
rsavitski 39:44d3dea4adcc 138 }
rsavitski 39:44d3dea4adcc 139
rsavitski 24:50805ef8c499 140 } //namespace