2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/AI/ai.cpp
- Committer:
- rsavitski
- Date:
- 2013-04-15
- Revision:
- 77:8d83a0c00e66
- Parent:
- 76:532d9bc1d2aa
- Child:
- 78:3178a1e46146
File content as of revision 77:8d83a0c00e66:
#include "ai.h" #include "rtos.h" #include "globals.h" #include "motion.h" #include "Colour.h" #include "supportfuncs.h" #include "Arm.h" #include "MotorControl.h" //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.9, 1.75, PI, 0.03, 0.05*PI, 32}; #endif #ifdef TEAM_BLUE ColourEnum own_colour = BLUE; Waypoint home_wp = {0.15, 1, 0, 0.03, 0.05*PI, 32}; #endif bool delayed_done = true; //TODO: kill struct delayed_struct //TODO: kill { osThreadId tid; Waypoint *wpptr; }; 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) { MotorControl::motorsenabled = true; motion::collavoiden = 1; motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home while(!motion::checkMotionStatus()); // wait until there MotorControl::motorsenabled = false; Thread::signal_wait(0x2); // CORD PULLED Waypoint current_waypoint; current_waypoint.x = 1.5; current_waypoint.y = 1; current_waypoint.theta = (-3.0f/4.0f)*PI; current_waypoint.pos_threshold = 0.03; current_waypoint.angle_threshold = 0.05*PI; current_waypoint.angle_exponent = 512; MotorControl::motorsenabled = true; motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); while(1); /////////////////////////////////////////////////////// // 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::collavoiden = 1; motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); float r = 0.61+0.02+0.01; bool firstavoidstop = 1; delayed_struct ds = {Thread::gettid(),¤t_waypoint}; RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds); 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()==own_colour || firstavoidstop) && delayed_done) { //temphack!!!!! //Thread::wait(1000); arm::upper_arm.go_down(); delayed_done = false; top_arm_up_timer.start(1200); delayed_wp_set.start(2400); //Thread::wait(2000); //arm::upper_arm.go_up(); phi -= 22.5/180*PI; current_waypoint.x = 1.5-r*cos(phi); current_waypoint.y = 2-r*sin(phi); current_waypoint.theta = constrainAngle(phi+PI/2); //arm offset current_waypoint.x += 0.0425*cos(current_waypoint.theta); current_waypoint.y += 0.0425*sin(current_waypoint.theta); current_waypoint.pos_threshold = 0.01; current_waypoint.angle_threshold = 0.01*PI; //motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); if (firstavoidstop) { motion::collavoiden = 0; firstavoidstop = 0; } else motion::collavoiden = 1; } motion::waypoint_flag_mutex.unlock(); Thread::wait(50); } } 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; motion::setNewWaypoint(dsptr->tid,dsptr->wpptr); delayed_done = true; } } //namespace