2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/AI/ai.cpp
- Revision:
- 65:4709ff6c753c
- Parent:
- 57:d434ceab6892
- Child:
- 67:be3ea5450cc7
- Child:
- 68:662164480f60
--- a/Processes/AI/ai.cpp Sat Apr 13 22:42:01 2013 +0000 +++ b/Processes/AI/ai.cpp Sun Apr 14 17:57:12 2013 +0000 @@ -4,6 +4,7 @@ #include "motion.h" #include "Colour.h" #include "supportfuncs.h" +#include "Arm.h" namespace AI @@ -17,20 +18,26 @@ current_waypoint.y = 1.85; current_waypoint.theta = PI; current_waypoint.pos_threshold = 0.01; - current_waypoint.angle_threshold = 0.02*PI; + 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; + float r = 0.61+0.02+0.03; 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) + 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);