Colour sensors calibrated
Dependencies: mbed-rtos mbed Servo QEI
Fork of ICRSEurobot13 by
Revision 43:6504d85d85b4, committed 2013-04-11
- Comitter:
- madcowswe
- Date:
- Thu Apr 11 19:49:46 2013 +0000
- Parent:
- 42:26e5f24b55b3
- Parent:
- 41:fefdbb9b5968
- Child:
- 44:555136de33e4
- Commit message:
- CpuUsage working. Currently 4%
Changed in this revision
Processes/Printing/Printing.h | Show annotated file Show diff for this revision Revisions of this file |
System/system.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/Processes/AI/ai.cpp Thu Apr 11 18:49:08 2013 +0000 +++ b/Processes/AI/ai.cpp Thu Apr 11 19:49:46 2013 +0000 @@ -3,16 +3,10 @@ namespace AI { -Mutex waypoint_flag_mutex; - -Waypoint* current_waypoint = NULL; //global scope - -bool waypoint_reached = false; // is current waypoint reached - void ailayer(void const *dummy) { - current_waypoint = new Waypoint[5]; - + Waypoint *current_waypoint = new Waypoint[5]; +/* current_waypoint[0].x = 1; current_waypoint[0].y = 1; current_waypoint[0].theta = 0.0; @@ -26,7 +20,21 @@ current_waypoint[1].angle_threshold = 0.05*PI; current_waypoint[2].x = -999; +*/ + current_waypoint[0].x = 0.5; + current_waypoint[0].y = 1.85; + current_waypoint[0].theta = 0.0; + current_waypoint[0].pos_threshold = 0.05; + current_waypoint[0].angle_threshold = 0.05*PI; + + current_waypoint[1].x = 1.2; + current_waypoint[1].y = 0.18; + current_waypoint[1].theta = 0; + current_waypoint[1].pos_threshold = 0.01; + current_waypoint[1].angle_threshold = 0.00001; + + current_waypoint[2].x = -999; /* //TODO: temp current waypoint hack current_waypoint = new Waypoint; @@ -43,35 +51,24 @@ secondwp->pos_threshold = 0.01; secondwp->angle_threshold = 0.00001; */ - + motion::setNewWaypoint(current_waypoint); while(1) { Thread::wait(50); - waypoint_flag_mutex.lock(); - if (checkWaypointStatus()) + motion::waypoint_flag_mutex.lock(); + if (motion::checkWaypointStatus()) { - clearWaypointReached(); - if ((current_waypoint+1)->x != -999) + + if ((current_waypoint+1)->x != -999) + { + motion::clearWaypointReached(); current_waypoint++; + motion::setNewWaypoint(current_waypoint); + } } - waypoint_flag_mutex.unlock(); + motion::waypoint_flag_mutex.unlock(); } } -void setWaypointReached() -{ - waypoint_reached = true; -} - -void clearWaypointReached() -{ - waypoint_reached = false; -} - -bool checkWaypointStatus() -{ - return waypoint_reached; -} - } //namespace \ No newline at end of file
--- a/Processes/AI/ai.h Thu Apr 11 18:49:08 2013 +0000 +++ b/Processes/AI/ai.h Thu Apr 11 19:49:46 2013 +0000 @@ -3,20 +3,13 @@ #include "rtos.h" #include "globals.h" +#include "motion.h" namespace AI { void ailayer(void const *dummy); -void setWaypointReached(); -void clearWaypointReached(); -bool checkWaypointStatus(); - - -extern Waypoint *current_waypoint; -extern Mutex waypoint_flag_mutex; - } #endif \ No newline at end of file
--- a/Processes/Motion/motion.cpp Thu Apr 11 18:49:08 2013 +0000 +++ b/Processes/Motion/motion.cpp Thu Apr 11 19:49:46 2013 +0000 @@ -10,10 +10,18 @@ namespace motion { +Mutex waypoint_flag_mutex; + +Waypoint *current_waypoint = NULL; + +bool waypoint_reached = false; // is current waypoint reached +bool d_reached = false; +bool a_reached = false; + void motionlayer(void const *dummy) { - // get target waypoint from AI - Waypoint target_waypoint = *AI::current_waypoint; + // save target waypoint + Waypoint target_waypoint = *current_waypoint; // get current state from Kalman State current_state = Kalman::getState(); @@ -27,30 +35,29 @@ float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta); - bool d_reached = false; - bool a_reached = false; - // is the waypoint reached - if (distance_err < target_waypoint.pos_threshold) + waypoint_flag_mutex.lock(); + if (distance_err < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) { d_reached = true; distance_err = 0; - angle_err = 0.2*constrainAngle(target_waypoint.theta - current_state.theta); + angle_err = 0.5*constrainAngle(target_waypoint.theta - current_state.theta); if (abs(angle_err) < target_waypoint.angle_threshold) { a_reached = true; - angle_err = 0; + //angle_err = 0; } } + waypoint_flag_mutex.unlock(); - AI::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 + 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 if (d_reached && a_reached) { - AI::setWaypointReached(); + setWaypointReached(); } - AI::waypoint_flag_mutex.unlock(); + waypoint_flag_mutex.unlock(); // angular velocity controller const float p_gain_av = 0.5; //TODO: tune @@ -92,4 +99,26 @@ MotorControl::set_omegacmd(angular_v); } +void setNewWaypoint(Waypoint *new_wp) +{ + current_waypoint = new_wp; +} + +void setWaypointReached() +{ + waypoint_reached = true; +} + +void clearWaypointReached() +{ + waypoint_reached = false; + d_reached = false; + a_reached = false; +} + +bool checkWaypointStatus() +{ + return waypoint_reached; +} + } //namespace \ No newline at end of file
--- a/Processes/Motion/motion.h Thu Apr 11 18:49:08 2013 +0000 +++ b/Processes/Motion/motion.h Thu Apr 11 19:49:46 2013 +0000 @@ -7,13 +7,20 @@ #include "Kalman.h" #include "MotorControl.h" #include "supportfuncs.h" -#include "ai.h" namespace motion { void motionlayer(void const *dummy); +// can encapsulate mutex fully in motion with something like: bool set_new_wp_if_current_reached() +void setNewWaypoint(Waypoint *new_wp); +void setWaypointReached(); +void clearWaypointReached(); +bool checkWaypointStatus(); + +extern Mutex waypoint_flag_mutex; + } #endif //EUROBOT_PROCESSES_MOTION_MOTION_H_ \ No newline at end of file
--- a/Processes/Printing/Printing.h Thu Apr 11 18:49:08 2013 +0000 +++ b/Processes/Printing/Printing.h Thu Apr 11 19:49:46 2013 +0000 @@ -1,7 +1,7 @@ // Eurobot13 Printing.h -#define PRINTINGOFF +//#define PRINTINGOFF #include "mbed.h" #include "rtos.h"
--- a/System/system.cpp Thu Apr 11 18:49:08 2013 +0000 +++ b/System/system.cpp Thu Apr 11 19:49:46 2013 +0000 @@ -7,7 +7,7 @@ Ticker CPUIdleMeasureTicker; volatile unsigned int nopctr = 0; -const float s_per_nopcycle = 1.0f/24000000.0f; +const float s_per_nopcycle = 1.0f/16000000.0f; float CpuUsage = 0; @@ -17,9 +17,11 @@ } void PostAndResetCPUIdle(){ - CpuUsage = 1.0f - (s_per_nopcycle * nopctr); + static int oldnopctr = 0; + int deltanop = nopctr - oldnopctr; + oldnopctr = nopctr; + CpuUsage = 1.0f - (s_per_nopcycle * deltanop); Printing::updateval(10,CpuUsage); - nopctr = 0; } void measureCPUidle (void const*){