This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/Motion/motion.cpp
- Committer:
- rsavitski
- Date:
- 2013-04-09
- Revision:
- 24:50805ef8c499
- Child:
- 25:b16f1045108f
File content as of revision 24:50805ef8c499:
//////////////////////////////////////////////////////////////////////////////// // Motion control unit //////////////////////////////////////////////////////////////////////////////// // Takes current state of the robot and target waypoint, // calculates desired forward and angular velocities and requests those from the motor control layer. //////////////////////////////////////////////////////////////////////////////// #include "motion.h" namespace motion { void motionlayer(void const *dummy) { //TODO: current_waypoint global in AI layer //TODO: threshold for deciding that the waypoint has been achieved // get target waypoint from AI Waypoint target_waypoint = *AI::current_waypoint; // get current state from Kalman State current_state = Kalman::getState(); float delta_x = target_waypoint.x - current_state.x; float delta_y = target_waypoint.y - current_state.y; float distance_err = sqrt(delta_x*delta_x + delta_y*delta_y); float angle_err = atan2(delta_y, delta_x); // angular velocity controller const float p_gain_av = 1.0; //TODO: tune const float max_av = PI; // radians per sec //TODO: tune // angle error [-pi, pi] float angular_v = p_gain_av * angle_err; // constrain range if (angular_v > max_av) angular_v = max_av; else if (angular_v < -max_av) angular_v = -max_av; // forward velocity controller const float p_gain_fv = 1.0; //TODO: tune float max_fv = 1.0; // meters per sec //TODO: tune const float angle_envelope_exponent = 8.0; //TODO: tune // control, distance_err in meters float forward_v = p_gain_fv * distance_err; // control the forward velocity envelope based on angular error max_fv = max_fv * pow(cos(angle_err/2), angle_envelope_exponent); // constrain range if (forward_v > max_fv) forward_v = max_fv; else if (forward_v < -max_fv) forward_v = -max_fv; //TODO: put into motor control MotorControl::set_fwdcmd(forward_v); MotorControl::set_thetacmd(angular_v); } } //namespace