This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Revision 54:99d3158c9207, committed 2013-04-12
- Comitter:
- rsavitski
- Date:
- Fri Apr 12 20:58:59 2013 +0000
- Parent:
- 53:b013df99b747
- Child:
- 55:0c8897da6b3a
- Commit message:
- crappy movement around cake
Changed in this revision
--- a/Processes/AI/ai.cpp Fri Apr 12 18:22:53 2013 +0000 +++ b/Processes/AI/ai.cpp Fri Apr 12 20:58:59 2013 +0000 @@ -5,70 +5,35 @@ void ailayer(void const *dummy) { - Waypoint *current_waypoint = new Waypoint[5]; -/* - current_waypoint[0].x = 1; - current_waypoint[0].y = 1; - current_waypoint[0].theta = 0.0; - current_waypoint[0].pos_threshold = 0.05; - current_waypoint[0].angle_threshold = 0.05*PI; + Waypoint current_waypoint; - current_waypoint[1].x = 2.2; - current_waypoint[1].y = 1.5; - current_waypoint[1].theta = PI/2; - current_waypoint[1].pos_threshold = 0.05; - current_waypoint[1].angle_threshold = 0.05*PI; - - current_waypoint[2].x = -999; -*/ - - current_waypoint[0].x = 1.8; - current_waypoint[0].y = 1.39; - current_waypoint[0].theta = PI; - current_waypoint[0].pos_threshold = 0.03; - current_waypoint[0].angle_threshold = 0.05*PI; + current_waypoint.x = 2.2; + current_waypoint.y = 1.85; + current_waypoint.theta = PI; + current_waypoint.pos_threshold = 0.01; + current_waypoint.angle_threshold = 0.02*PI; - current_waypoint[1].x = 1.5; - current_waypoint[1].y = 1.39; - current_waypoint[1].theta = PI; - current_waypoint[1].pos_threshold = 0.03; - current_waypoint[1].angle_threshold = 0.05*PI; + motion::setNewWaypoint(¤t_waypoint); + - current_waypoint[2].x = -999; -/* - //TODO: temp current waypoint hack - current_waypoint = new Waypoint; - current_waypoint->x = 0.5; - current_waypoint->y = 0.7; - current_waypoint->theta = 0.0; - current_waypoint->pos_threshold = 0.05; - current_waypoint->angle_threshold = 0.05*PI; - Waypoint* secondwp = new Waypoint; - secondwp->x = 1.20; - secondwp->y = 0.18; - secondwp->theta = PI; - secondwp->pos_threshold = 0.01; - secondwp->angle_threshold = 0.00001; -*/ + float r = 0.61; - motion::setNewWaypoint(current_waypoint); - while(1) + for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) { - Thread::wait(50); - motion::waypoint_flag_mutex.lock(); if (motion::checkWaypointStatus()) { - - if ((current_waypoint+1)->x != -999) - { - motion::clearWaypointReached(); - current_waypoint++; - motion::setNewWaypoint(current_waypoint); - } + 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); + motion::clearWaypointReached(); + motion::setNewWaypoint(¤t_waypoint); } - motion::waypoint_flag_mutex.unlock(); + motion::waypoint_flag_mutex.unlock(); + + Thread::wait(50); } }
--- a/Processes/AI/ai.h Fri Apr 12 18:22:53 2013 +0000 +++ b/Processes/AI/ai.h Fri Apr 12 20:58:59 2013 +0000 @@ -4,6 +4,7 @@ #include "rtos.h" #include "globals.h" #include "motion.h" +#include "supportfuncs.h" namespace AI {
--- a/Processes/Motion/motion.cpp Fri Apr 12 18:22:53 2013 +0000 +++ b/Processes/Motion/motion.cpp Fri Apr 12 20:58:59 2013 +0000 @@ -86,11 +86,11 @@ // forward velocity controller - const float p_gain_fv = 0.5; //TODO: tune + const float p_gain_fv = 0.25; //TODO: tune float max_fv = 0.2; // meters per sec //TODO: tune float max_fv_reverse = 0.05; //TODO: tune - const float angle_envelope_exponent = 32.0; //8.0; //TODO: tune + const float angle_envelope_exponent = 512;//32.0; //8.0; //TODO: tune // control, distance_err in meters float forward_v = p_gain_fv * distance_err;