This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Revision 66:f1d75e51398d, committed 2013-04-14
- Comitter:
- madcowswe
- Date:
- Sun Apr 14 17:22:20 2013 +0000
- Parent:
- 64:c979fb1cd3b5
- Child:
- 67:be3ea5450cc7
- Commit message:
- Obstacle avoidance working smoothly, fairly hacked motion-ai communication of override
Changed in this revision
--- a/Processes/AI/ai.cpp Sun Apr 14 14:59:57 2013 +0000 +++ b/Processes/AI/ai.cpp Sun Apr 14 17:22:20 2013 +0000 @@ -20,16 +20,18 @@ current_waypoint.angle_threshold = 0.02*PI; motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); + motion::collavoiden = 1; Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER); Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER); float r = 0.61+0.02; + bool firstavoidstop = 1; for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) { motion::waypoint_flag_mutex.lock(); - if (motion::checkMotionStatus() && c_lower.getColour()==RED) + if (motion::checkMotionStatus() && (c_lower.getColour()==RED || firstavoidstop)) { phi -= 22.5/180*PI; current_waypoint.x = 1.5-r*cos(phi); @@ -41,6 +43,12 @@ current_waypoint.y += 0.0425*sin(current_waypoint.theta); motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); + if (firstavoidstop){ + motion::collavoiden = 0; + firstavoidstop = 0; + } + else + motion::collavoiden = 1; } motion::waypoint_flag_mutex.unlock();
--- a/Processes/Motion/motion.cpp Sun Apr 14 14:59:57 2013 +0000 +++ b/Processes/Motion/motion.cpp Sun Apr 14 17:22:20 2013 +0000 @@ -10,6 +10,7 @@ #include "Kalman.h" #include "MotorControl.h" #include "supportfuncs.h" +#include "AvoidDstSensor.h" namespace motion { @@ -23,6 +24,9 @@ namespace motion { +volatile int collavoiden = 0; +AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR); + // motion commands supported enum motion_type_t { motion_waypoint }; @@ -152,6 +156,13 @@ //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v); + if(collavoiden){ + float d = ADS.Distanceincm(); + if(d > 10){ + forward_v *= max(min((d-15)*(1.0f/20.0f),1.0f),-0.1f); + } + } + // pass values to the motor control MotorControl::set_fwdcmd(forward_v); MotorControl::set_omegacmd(angular_v);
--- a/Processes/Motion/motion.h Sun Apr 14 14:59:57 2013 +0000 +++ b/Processes/Motion/motion.h Sun Apr 14 17:22:20 2013 +0000 @@ -7,6 +7,8 @@ namespace motion { +extern volatile int collavoiden; + void motionlayer(void const *dummy); void waypoint_motion_handler();
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/AvoidDstSensor/AvoidDstSensor.h Sun Apr 14 17:22:20 2013 +0000 @@ -0,0 +1,17 @@ +#include "mbed.h" + +class AvoidDstSensor{ + private: + AnalogIn ain; + + public: + AvoidDstSensor(PinName analoginpin) : ain(analoginpin){} + + float Raw(){return ain;} + + float Distanceincm(){ + float d = ((1.0f/ain)-0.5f)*(1.0f/0.11f); + d = (d < 10 || d > 50)? -1:d; + return d; + } +}; \ No newline at end of file