Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/Motion/motion.cpp@40:fefdbb9b5968, 2013-04-11 (annotated)

Committer:
rsavitski
Date:
Thu Apr 11 18:49:11 2013 +0000
Revision:
40:fefdbb9b5968
Parent:
39:44d3dea4adcc
Child:
52:bffe5f7c39a3
hysteresis

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 30:791739422122 38 // is the waypoint reached
rsavitski 39:44d3dea4adcc 39 waypoint_flag_mutex.lock();
rsavitski 40:fefdbb9b5968 40 if (distance_err < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
rsavitski 30:791739422122 41 {
rsavitski 37:6ecf0d21e492 42 d_reached = true;
rsavitski 30:791739422122 43 distance_err = 0;
rsavitski 38:c9058a401410 44
rsavitski 39:44d3dea4adcc 45 angle_err = 0.5*constrainAngle(target_waypoint.theta - current_state.theta);
rsavitski 38:c9058a401410 46
rsavitski 38:c9058a401410 47 if (abs(angle_err) < target_waypoint.angle_threshold)
madcowswe 33:a49197572737 48 {
rsavitski 37:6ecf0d21e492 49 a_reached = true;
rsavitski 39:44d3dea4adcc 50 //angle_err = 0;
madcowswe 33:a49197572737 51 }
rsavitski 35:f8e7f0a72a3d 52 }
rsavitski 39:44d3dea4adcc 53 waypoint_flag_mutex.unlock();
rsavitski 30:791739422122 54
rsavitski 39:44d3dea4adcc 55 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 56 if (d_reached && a_reached)
rsavitski 30:791739422122 57 {
rsavitski 39:44d3dea4adcc 58 setWaypointReached();
rsavitski 30:791739422122 59 }
rsavitski 39:44d3dea4adcc 60 waypoint_flag_mutex.unlock();
rsavitski 24:50805ef8c499 61
rsavitski 24:50805ef8c499 62 // angular velocity controller
madcowswe 34:e1678450feec 63 const float p_gain_av = 0.5; //TODO: tune
rsavitski 24:50805ef8c499 64
madcowswe 33:a49197572737 65 const float max_av = 0.5*PI; // radians per sec //TODO: tune
rsavitski 24:50805ef8c499 66
rsavitski 24:50805ef8c499 67 // angle error [-pi, pi]
rsavitski 24:50805ef8c499 68 float angular_v = p_gain_av * angle_err;
rsavitski 24:50805ef8c499 69
rsavitski 24:50805ef8c499 70 // constrain range
rsavitski 24:50805ef8c499 71 if (angular_v > max_av)
rsavitski 24:50805ef8c499 72 angular_v = max_av;
rsavitski 24:50805ef8c499 73 else if (angular_v < -max_av)
rsavitski 24:50805ef8c499 74 angular_v = -max_av;
rsavitski 24:50805ef8c499 75
rsavitski 24:50805ef8c499 76
rsavitski 24:50805ef8c499 77 // forward velocity controller
madcowswe 34:e1678450feec 78 const float p_gain_fv = 0.5; //TODO: tune
rsavitski 24:50805ef8c499 79
madcowswe 25:b16f1045108f 80 float max_fv = 0.2; // meters per sec //TODO: tune
rsavitski 24:50805ef8c499 81 const float angle_envelope_exponent = 8.0; //TODO: tune
rsavitski 24:50805ef8c499 82
rsavitski 24:50805ef8c499 83 // control, distance_err in meters
rsavitski 24:50805ef8c499 84 float forward_v = p_gain_fv * distance_err;
rsavitski 24:50805ef8c499 85
rsavitski 24:50805ef8c499 86 // control the forward velocity envelope based on angular error
rsavitski 24:50805ef8c499 87 max_fv = max_fv * pow(cos(angle_err/2), angle_envelope_exponent);
rsavitski 24:50805ef8c499 88
rsavitski 24:50805ef8c499 89 // constrain range
rsavitski 24:50805ef8c499 90 if (forward_v > max_fv)
rsavitski 24:50805ef8c499 91 forward_v = max_fv;
rsavitski 24:50805ef8c499 92 else if (forward_v < -max_fv)
rsavitski 24:50805ef8c499 93 forward_v = -max_fv;
madcowswe 25:b16f1045108f 94
madcowswe 25:b16f1045108f 95 //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
rsavitski 24:50805ef8c499 96
rsavitski 30:791739422122 97 // pass values to the motor control
rsavitski 24:50805ef8c499 98 MotorControl::set_fwdcmd(forward_v);
madcowswe 25:b16f1045108f 99 MotorControl::set_omegacmd(angular_v);
rsavitski 24:50805ef8c499 100 }
rsavitski 24:50805ef8c499 101
rsavitski 39:44d3dea4adcc 102 void setNewWaypoint(Waypoint *new_wp)
rsavitski 39:44d3dea4adcc 103 {
rsavitski 39:44d3dea4adcc 104 current_waypoint = new_wp;
rsavitski 39:44d3dea4adcc 105 }
rsavitski 39:44d3dea4adcc 106
rsavitski 39:44d3dea4adcc 107 void setWaypointReached()
rsavitski 39:44d3dea4adcc 108 {
rsavitski 39:44d3dea4adcc 109 waypoint_reached = true;
rsavitski 39:44d3dea4adcc 110 }
rsavitski 39:44d3dea4adcc 111
rsavitski 39:44d3dea4adcc 112 void clearWaypointReached()
rsavitski 39:44d3dea4adcc 113 {
rsavitski 39:44d3dea4adcc 114 waypoint_reached = false;
rsavitski 40:fefdbb9b5968 115 d_reached = false;
rsavitski 40:fefdbb9b5968 116 a_reached = false;
rsavitski 39:44d3dea4adcc 117 }
rsavitski 39:44d3dea4adcc 118
rsavitski 39:44d3dea4adcc 119 bool checkWaypointStatus()
rsavitski 39:44d3dea4adcc 120 {
rsavitski 39:44d3dea4adcc 121 return waypoint_reached;
rsavitski 39:44d3dea4adcc 122 }
rsavitski 39:44d3dea4adcc 123
rsavitski 24:50805ef8c499 124 } //namespace