Oskar Weigl / Mbed 2 deprecated ICRSEurobot13

This is some awesome robot code

Dependencies:   mbed-rtos mbed QEI

Fork of ICRSEurobot13 by Thomas Branch

Processes/AI/ai.cpp@67:be3ea5450cc7, 2013-04-14 (annotated)

Committer:
madcowswe
Date:
Sun Apr 14 18:47:17 2013 +0000
Revision:
67:be3ea5450cc7
Parent:
66:f1d75e51398d
Parent:
65:4709ff6c753c
Child:
69:4b7bb92916da
Tuned merge. Testing needed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rsavitski 30:791739422122 1 #include "ai.h"
rsavitski 55:0c8897da6b3a 2 #include "rtos.h"
rsavitski 55:0c8897da6b3a 3 #include "globals.h"
rsavitski 55:0c8897da6b3a 4 #include "motion.h"
rsavitski 55:0c8897da6b3a 5 #include "Colour.h"
rsavitski 55:0c8897da6b3a 6 #include "supportfuncs.h"
rsavitski 65:4709ff6c753c 7 #include "Arm.h"
rsavitski 55:0c8897da6b3a 8
rsavitski 30:791739422122 9
rsavitski 30:791739422122 10 namespace AI
rsavitski 30:791739422122 11 {
rsavitski 30:791739422122 12
rsavitski 30:791739422122 13 void ailayer(void const *dummy)
rsavitski 30:791739422122 14 {
rsavitski 54:99d3158c9207 15 Waypoint current_waypoint;
rsavitski 38:c9058a401410 16
rsavitski 54:99d3158c9207 17 current_waypoint.x = 2.2;
rsavitski 54:99d3158c9207 18 current_waypoint.y = 1.85;
madcowswe 67:be3ea5450cc7 19 current_waypoint.theta = (-3.0f/4.0f)*PI;
madcowswe 67:be3ea5450cc7 20 current_waypoint.pos_threshold = 0.03;
rsavitski 54:99d3158c9207 21 current_waypoint.angle_threshold = 0.02*PI;
rsavitski 39:44d3dea4adcc 22
rsavitski 57:d434ceab6892 23 motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
madcowswe 66:f1d75e51398d 24 motion::collavoiden = 1;
rsavitski 39:44d3dea4adcc 25
madcowswe 50:937e860f4621 26 Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
madcowswe 50:937e860f4621 27 Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
rsavitski 55:0c8897da6b3a 28
madcowswe 67:be3ea5450cc7 29 float r = 0.61+0.02+0.01;
rsavitski 53:b013df99b747 30
madcowswe 66:f1d75e51398d 31 bool firstavoidstop = 1;
rsavitski 54:99d3158c9207 32 for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
rsavitski 30:791739422122 33 {
rsavitski 39:44d3dea4adcc 34 motion::waypoint_flag_mutex.lock();
madcowswe 67:be3ea5450cc7 35 if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop))
rsavitski 30:791739422122 36 {
rsavitski 65:4709ff6c753c 37 //temphack!!!!!
rsavitski 65:4709ff6c753c 38 Thread::wait(1000);
rsavitski 65:4709ff6c753c 39 arm::upper_arm.go_down();
rsavitski 65:4709ff6c753c 40 Thread::wait(2000);
rsavitski 65:4709ff6c753c 41 arm::upper_arm.go_up();
rsavitski 65:4709ff6c753c 42
rsavitski 54:99d3158c9207 43 phi -= 22.5/180*PI;
rsavitski 54:99d3158c9207 44 current_waypoint.x = 1.5-r*cos(phi);
rsavitski 54:99d3158c9207 45 current_waypoint.y = 2-r*sin(phi);
rsavitski 54:99d3158c9207 46 current_waypoint.theta = constrainAngle(phi+PI/2);
rsavitski 56:ed585a82092b 47
rsavitski 56:ed585a82092b 48 //arm offset
rsavitski 56:ed585a82092b 49 current_waypoint.x += 0.0425*cos(current_waypoint.theta);
rsavitski 56:ed585a82092b 50 current_waypoint.y += 0.0425*sin(current_waypoint.theta);
madcowswe 67:be3ea5450cc7 51
madcowswe 67:be3ea5450cc7 52 current_waypoint.pos_threshold = 0.01;
madcowswe 67:be3ea5450cc7 53 current_waypoint.angle_threshold = 0.01*PI;
rsavitski 57:d434ceab6892 54
rsavitski 57:d434ceab6892 55 motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
madcowswe 66:f1d75e51398d 56 if (firstavoidstop){
madcowswe 66:f1d75e51398d 57 motion::collavoiden = 0;
madcowswe 66:f1d75e51398d 58 firstavoidstop = 0;
madcowswe 66:f1d75e51398d 59 }
madcowswe 66:f1d75e51398d 60 else
madcowswe 66:f1d75e51398d 61 motion::collavoiden = 1;
rsavitski 30:791739422122 62 }
rsavitski 54:99d3158c9207 63 motion::waypoint_flag_mutex.unlock();
rsavitski 54:99d3158c9207 64
rsavitski 54:99d3158c9207 65 Thread::wait(50);
rsavitski 30:791739422122 66 }
rsavitski 30:791739422122 67 }
rsavitski 30:791739422122 68
rsavitski 30:791739422122 69 } //namespace