This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/AI/ai.cpp
- Committer:
- rsavitski
- Date:
- 2013-04-14
- Revision:
- 68:662164480f60
- Parent:
- 65:4709ff6c753c
- Child:
- 69:4b7bb92916da
File content as of revision 68:662164480f60:
#include "ai.h" #include "rtos.h" #include "globals.h" #include "motion.h" #include "Colour.h" #include "supportfuncs.h" #include "Arm.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.01*PI; motion::setNewWaypoint(Thread::gettid(),¤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+0.01; 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) { //temphack!!!!! Thread::wait(1000); arm::upper_arm.go_down(); 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); motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); } motion::waypoint_flag_mutex.unlock(); Thread::wait(50); } } } //namespace