This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Diff: Processes/AI/ai.cpp
- Revision:
- 69:4b7bb92916da
- Parent:
- 67:be3ea5450cc7
- Parent:
- 68:662164480f60
- Child:
- 70:0da6ca845762
--- a/Processes/AI/ai.cpp Sun Apr 14 18:47:17 2013 +0000 +++ b/Processes/AI/ai.cpp Sun Apr 14 20:23:07 2013 +0000 @@ -6,39 +6,55 @@ #include "supportfuncs.h" #include "Arm.h" +//TODO: after 2013, kill entire AI layer as it is hacked together. Rest is fine-ish namespace AI { +bool delayed_done = true; //TODO: kill + +struct delayed_struct //TODO: kill +{ + osThreadId tid; + Waypoint *wpptr; +}; + +void delayed_setter(const void *tid_wpptr); //TODO: kill the hack + void ailayer(void const *dummy) { Waypoint current_waypoint; + + 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); + // first waypoint for approach current_waypoint.x = 2.2; current_waypoint.y = 1.85; current_waypoint.theta = (-3.0f/4.0f)*PI; current_waypoint.pos_threshold = 0.03; 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); + motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); float r = 0.61+0.02+0.01; bool firstavoidstop = 1; + delayed_struct ds = {Thread::gettid(),¤t_waypoint}; + for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) { motion::waypoint_flag_mutex.lock(); - if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop)) + if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop) && delayed_done) { //temphack!!!!! - Thread::wait(1000); + //Thread::wait(1000); arm::upper_arm.go_down(); - Thread::wait(2000); - arm::upper_arm.go_up(); + delayed_done = false; + RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds); + //Thread::wait(2000); + //arm::upper_arm.go_up(); phi -= 22.5/180*PI; current_waypoint.x = 1.5-r*cos(phi); @@ -52,8 +68,9 @@ current_waypoint.pos_threshold = 0.01; current_waypoint.angle_threshold = 0.01*PI; - motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); - if (firstavoidstop){ + //motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); + if (firstavoidstop) + { motion::collavoiden = 0; firstavoidstop = 0; } @@ -66,4 +83,12 @@ } } +void delayed_setter(const void *tid_wpptr) //TODO: kill the hack +{ + delayed_struct *dsptr = (delayed_struct *)tid_wpptr; + arm::upper_arm.go_up(); + motion::setNewWaypoint(dsptr->tid,dsptr->wpptr); + delayed_done = true; +} + } //namespace \ No newline at end of file