This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/AI/ai.cpp
- Committer:
- madcowswe
- Date:
- 2013-04-14
- Revision:
- 66:f1d75e51398d
- Parent:
- 57:d434ceab6892
- Child:
- 67:be3ea5450cc7
File content as of revision 66:f1d75e51398d:
#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(Thread::gettid(),¤t_waypoint); motion::collavoiden = 1; 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; bool firstavoidstop = 1; for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) { motion::waypoint_flag_mutex.lock(); if (motion::checkMotionStatus() && (c_lower.getColour()==RED || firstavoidstop)) { 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::setNewWaypoint(Thread::gettid(),¤t_waypoint); if (firstavoidstop){ motion::collavoiden = 0; firstavoidstop = 0; } else motion::collavoiden = 1; } motion::waypoint_flag_mutex.unlock(); Thread::wait(50); } } } //namespace