2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/AI/ai.cpp
- Revision:
- 78:3178a1e46146
- Parent:
- 74:9620d24a2f4e
- Parent:
- 77:8d83a0c00e66
- Child:
- 81:ef1ce4f5322b
--- a/Processes/AI/ai.cpp Mon Apr 15 15:29:40 2013 +0000 +++ b/Processes/AI/ai.cpp Mon Apr 15 17:07:40 2013 +0000 @@ -7,10 +7,25 @@ #include "Arm.h" #include "MotorControl.h" -//TODO: after 2013, kill entire AI layer as it is hacked together. Rest is fine-ish +//TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish + +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); namespace AI { +// starting position +#ifdef TEAM_RED +ColourEnum own_colour = RED; + +Waypoint home_wp = {2.7, 1.75, PI, 0.03, 0.05*PI, 32}; +#endif + +#ifdef TEAM_BLUE +ColourEnum own_colour = BLUE; + +Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32}; +#endif bool delayed_done = true; //TODO: kill @@ -20,16 +35,57 @@ Waypoint *wpptr; }; -void arm_upper(const void *dummy); //TODO: kill +void raise_top_arm(const void *dummy); +void raise_bottom_arm(const void *dummy); void delayed_setter(const void *tid_wpptr); //TODO: kill the hack void ailayer(void const *dummy) { - Waypoint current_waypoint; + RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce); + RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce); + + //NB: reassign ds.wpptr before each invocation + delayed_struct ds = {Thread::gettid(),NULL}; + RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds); + + MotorControl::motorsenabled = true; + motion::collavoiden = 1; + + motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home + Thread::signal_wait(0x1); //wait until wp reached + + MotorControl::motorsenabled = false; + + Thread::signal_wait(0x2); //wait for cord + + // CORD PULLED + + Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32}; - 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); + MotorControl::motorsenabled = true; + motion::setNewWaypoint(Thread::gettid(),&mid_wp); + Thread::signal_wait(0x1); //wait until wp reached + + Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32}; + motion::setNewWaypoint(Thread::gettid(),&approach_wp); + Thread::signal_wait(0x1); //wait until wp reached + + // TODO: ?(disable collison avoidance), approach first cake wp, enable collision + /* + ///////////////temp hack + Waypoint temp_wp = {1.222,1.440, 149.0/180*PI, 0.01, 0.02*PI, 512}; + motion::setNewWaypoint(Thread::gettid(),&temp_wp); + Thread::signal_wait(0x1); //wait until wp reached + arm::lower_arm.go_down(); + bottom_arm_up_timer.start(1200); + /////////////// temp hack over + */ + + while(1) + Thread::wait(1000); + /* + /////////////////////////////////////////////////////// // first waypoint for approach current_waypoint.x = 2.2; @@ -42,23 +98,23 @@ MotorControl::motorsenabled = 1; motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); - float r = 0.61+0.02+0.01; + float r = 0.26+0.35+0.02+0.01; bool firstavoidstop = 1; delayed_struct ds = {Thread::gettid(),¤t_waypoint}; RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds); - RtosTimer delayed_armer(arm_upper, osTimerOnce); + RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce); 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) && delayed_done) + if (motion::checkMotionStatus() && (c_upper.getColour()==own_colour || firstavoidstop) && delayed_done) { //temphack!!!!! //Thread::wait(1000); arm::upper_arm.go_down(); delayed_done = false; - delayed_armer.start(1200); + top_arm_up_timer.start(1200); delayed_wp_set.start(2400); //Thread::wait(2000); //arm::upper_arm.go_up(); @@ -87,18 +143,22 @@ motion::waypoint_flag_mutex.unlock(); Thread::wait(50); - } + }*/ } -void arm_upper(const void *dummy) +void raise_top_arm(const void *dummy) { arm::upper_arm.go_up(); } +void raise_bottom_arm(const void *dummy) +{ + arm::lower_arm.go_up(); +} + 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; }