This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Revision 77:8d83a0c00e66, committed 2013-04-15
- Comitter:
- rsavitski
- Date:
- Mon Apr 15 15:30:12 2013 +0000
- Parent:
- 73:265d3cc6b0b1
- Parent:
- 76:532d9bc1d2aa
- Child:
- 78:3178a1e46146
- Commit message:
- merged
Changed in this revision
--- a/Processes/AI/ai.cpp Mon Apr 15 13:44:49 2013 +0000 +++ b/Processes/AI/ai.cpp Mon Apr 15 15:30:12 2013 +0000 @@ -5,11 +5,27 @@ #include "Colour.h" #include "supportfuncs.h" #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.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 @@ -19,16 +35,40 @@ 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) { + 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; - 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); + 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; @@ -45,18 +85,18 @@ 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(); @@ -88,15 +128,19 @@ } } -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; }
--- a/Processes/Motion/motion.cpp Mon Apr 15 13:44:49 2013 +0000 +++ b/Processes/Motion/motion.cpp Mon Apr 15 15:30:12 2013 +0000 @@ -44,7 +44,7 @@ }; // local copy of the current active motion -motion_cmd current_motion; +motion_cmd current_motion = {motion_waypoint, 0, false, NULL}; Waypoint target_waypoint = {0,0,0,0,0}; //local wp copy, TODO: fix and make a shared local memory pool for any movement cmd to be copied to @@ -132,10 +132,10 @@ // forward velocity controller - const float p_gain_fv = 1;//0.7; //TODO: tune + const float p_gain_fv = 0.9;//0.7; //TODO: tune float max_fv = 0.3;//0.2; // meters per sec //TODO: tune - float max_fv_reverse = 0.05; //TODO: tune + float max_fv_reverse = 0.03; //TODO: tune const float angle_envelope_exponent = 32;//512; //8.0; //TODO: tune // control, distance_err in meters @@ -146,7 +146,7 @@ max_fv = max_fv_reverse; // control the forward velocity envelope based on angular error - max_fv = max_fv * pow(cos(angle_err_saved/2), angle_envelope_exponent); + max_fv = max_fv * pow(cos(angle_err_saved/2), /*angle_envelope_exponent*/target_waypoint.angle_exponent); //temp hack // constrain range if (forward_v > max_fv) @@ -186,7 +186,6 @@ current_motion.setter_tid = setter_tid_in; current_motion.motion_type = motion_waypoint; - //current_motion.wp_ptr = new_wp; //TODO: need to make local copy or edits to mem ptr contents screw motion over target_waypoint = *new_wp; }
--- a/Processes/Motion/motion.h Mon Apr 15 13:44:49 2013 +0000 +++ b/Processes/Motion/motion.h Mon Apr 15 15:30:12 2013 +0000 @@ -12,6 +12,7 @@ void motionlayer(void const *dummy); void waypoint_motion_handler(); +// supports both polling on sticky bit via checkMotionStatus(), waiting/blocking on signal 0x1 in setter thread (signalled upon reaching the wp) void setNewWaypoint(osThreadId setter_tid_in, Waypoint *new_wp); bool checkMotionStatus();
--- a/globals.h Mon Apr 15 13:44:49 2013 +0000 +++ b/globals.h Mon Apr 15 15:30:12 2013 +0000 @@ -83,7 +83,7 @@ const PinName P_COLOR_SENSOR_RED_LOWER = p11; const PinName P_COLOR_SENSOR_BLUE_LOWER = p10; -const PinName P_START_CORD = p27; +const PinName P_START_CORD = p17; @@ -112,6 +112,7 @@ float theta; float pos_threshold; float angle_threshold; + float angle_exponent; //temp hack } Waypoint; typedef struct State