This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/AI/ai.cpp
- Committer:
- rsavitski
- Date:
- 2013-04-13
- Revision:
- 56:ed585a82092b
- Parent:
- 55:0c8897da6b3a
- Child:
- 57:d434ceab6892
File content as of revision 56:ed585a82092b:
#include "ai.h" #include "rtos.h" #include "globals.h" #include "motion.h" #include "Colour.h" #include "supportfuncs.h" namespace AI { void ailayer(void const *dummy) { Waypoint current_waypoint; 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; motion::setNewWaypoint(¤t_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); float r = 0.61+0.02; for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) { motion::waypoint_flag_mutex.lock(); if (motion::checkWaypointStatus() && c_upper.getColour()==RED) { 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); motion::clearWaypointReached(); motion::setNewWaypoint(¤t_waypoint); } motion::waypoint_flag_mutex.unlock(); Thread::wait(50); } } } //namespace