2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/Motion/motion.cpp
- Committer:
- madcowswe
- Date:
- 2013-04-16
- Revision:
- 85:b0858346d838
- Parent:
- 81:ef1ce4f5322b
- Child:
- 88:8850373c3f0d
File content as of revision 85:b0858346d838:
//////////////////////////////////////////////////////////////////////////////// // Motion control unit //////////////////////////////////////////////////////////////////////////////// // Takes current state and motion command (set via accessors) and actuates it via motor layer calls //////////////////////////////////////////////////////////////////////////////// //todo: check everything is properly initialised and not a race condition #include "motion.h" #include "math.h" #include "Kalman.h" #include "MotorControl.h" #include "supportfuncs.h" #include "AvoidDstSensor.h" namespace motion { // private function prototypes void setWaypointReached(); void clearMotionCmd(); void clearWaypoint(); } namespace motion { volatile int collavoiden = 0; // TODO: kill oskar's code AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR); //TODO: kill oskar's hack // motion commands supported enum motion_type_t { motion_waypoint }; struct motion_cmd { motion_type_t motion_type; osThreadId setter_tid; bool motion_done; union { Waypoint *wp_ptr; }; }; // local copy of the current active motion motion_cmd current_motion = {motion_waypoint, 0, false, NULL}; Waypoint target_waypoint = {0,0,0,0,0}; //local wp copy, TODO: fix and make a shared local memory pool for any movement cmd to be copied to Mutex waypoint_flag_mutex; // local to waypoint motion handler bool wp_d_reached = false; bool wp_a_reached = false; void motionlayer(void const *dummy) { switch (current_motion.motion_type) { case motion_waypoint: waypoint_motion_handler(); break; default: break; } } void waypoint_motion_handler() { // save target waypoint //Waypoint target_waypoint = *current_motion.wp_ptr; // 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; //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y); float distance_err = hypot(delta_x, delta_y); float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta); // reversing if close to target and more optimal than forward movement bool reversing = false; if ((abs(angle_err) > PI/2) && (distance_err < 0.2)) //TODO: parameterise and tune 0.2 meters hardcoded { reversing = true; angle_err = constrainAngle(angle_err + PI); distance_err = -distance_err; } 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 // is the waypoint reached waypoint_flag_mutex.lock(); //TODO: consider refactoring, mutexes suck, switch to semaphore style or more contained mutexes (think of several producers as well); race conditions not checked atm either if (abs(distance_err) < ((wp_d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) { wp_d_reached = true; angle_err = constrainAngle(target_waypoint.theta - current_state.theta); if (abs(angle_err) < target_waypoint.angle_threshold) { wp_a_reached = true; } } // 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 if (wp_d_reached && wp_a_reached) { setWaypointReached(); } waypoint_flag_mutex.unlock(); // angular velocity controller const float p_gain_av = 1;//0.7; //TODO: tune const float max_av = 1;//0.5; // 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 = 0.85;//0.7; //TODO: tune float max_fv = 0.3;//0.2; // meters per sec //TODO: tune float max_fv_reverse = 0.03; //TODO: tune const float angle_envelope_exponent = 32;//512; //8.0; //TODO: tune // control, distance_err in meters float forward_v = p_gain_fv * distance_err; // if reversing, robot is less stable hence a different cap is used if (reversing) max_fv = max_fv_reverse; // control the forward velocity envelope based on angular error max_fv = max_fv * pow(cos(angle_err_saved/2), /*angle_envelope_exponent*/target_waypoint.angle_exponent); //temp hack // constrain range if (forward_v > max_fv) forward_v = max_fv; else if (forward_v < -max_fv) forward_v = -max_fv; //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v); //TODO: remove oskar's avoidance hack if(collavoiden) { float d = ADS.Distanceincm(); if(d > 10) { forward_v *= max(min((d-15)*(1.0f/25.0f),1.0f),-0.1f); } } // end of Oskar hack // pass values to the motor control MotorControl::set_fwdcmd(forward_v); MotorControl::set_omegacmd(angular_v); } bool checkMotionStatus() { return current_motion.motion_done; } void setNewWaypoint(osThreadId setter_tid_in, Waypoint *new_wp) { clearMotionCmd(); current_motion.setter_tid = setter_tid_in; current_motion.motion_type = motion_waypoint; target_waypoint = *new_wp; } void setWaypointReached() { current_motion.motion_done = true; osSignalSet(current_motion.setter_tid,0x1); } void clearMotionCmd() { current_motion.motion_done = false; osSignalClear(current_motion.setter_tid, 0x1); switch (current_motion.motion_type) { case motion_waypoint: clearWaypoint(); break; default: break; } } void clearWaypoint() { wp_d_reached = false; wp_a_reached = false; } } //namespace