Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/AI/ai.cpp@77:8d83a0c00e66, 2013-04-15 (annotated)

Committer:
rsavitski
Date:
Mon Apr 15 15:30:12 2013 +0000
Revision:
77:8d83a0c00e66
Parent:
76:532d9bc1d2aa
Child:
78:3178a1e46146
merged

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 77:8d83a0c00e66 8 #include "MotorControl.h"
rsavitski 55:0c8897da6b3a 9
rsavitski 77:8d83a0c00e66 10 //TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish
rsavitski 77:8d83a0c00e66 11
rsavitski 77:8d83a0c00e66 12 Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
rsavitski 77:8d83a0c00e66 13 Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
rsavitski 30:791739422122 14
rsavitski 30:791739422122 15 namespace AI
rsavitski 30:791739422122 16 {
rsavitski 77:8d83a0c00e66 17 // starting position
rsavitski 77:8d83a0c00e66 18 #ifdef TEAM_RED
rsavitski 77:8d83a0c00e66 19 ColourEnum own_colour = RED;
rsavitski 77:8d83a0c00e66 20
rsavitski 77:8d83a0c00e66 21 Waypoint home_wp = {2.9, 1.75, PI, 0.03, 0.05*PI, 32};
rsavitski 77:8d83a0c00e66 22 #endif
rsavitski 77:8d83a0c00e66 23
rsavitski 77:8d83a0c00e66 24 #ifdef TEAM_BLUE
rsavitski 77:8d83a0c00e66 25 ColourEnum own_colour = BLUE;
rsavitski 77:8d83a0c00e66 26
rsavitski 77:8d83a0c00e66 27 Waypoint home_wp = {0.15, 1, 0, 0.03, 0.05*PI, 32};
rsavitski 77:8d83a0c00e66 28 #endif
rsavitski 30:791739422122 29
rsavitski 69:4b7bb92916da 30 bool delayed_done = true; //TODO: kill
rsavitski 69:4b7bb92916da 31
rsavitski 69:4b7bb92916da 32 struct delayed_struct //TODO: kill
rsavitski 69:4b7bb92916da 33 {
rsavitski 69:4b7bb92916da 34 osThreadId tid;
rsavitski 69:4b7bb92916da 35 Waypoint *wpptr;
rsavitski 69:4b7bb92916da 36 };
rsavitski 69:4b7bb92916da 37
rsavitski 76:532d9bc1d2aa 38 void raise_top_arm(const void *dummy);
rsavitski 76:532d9bc1d2aa 39 void raise_bottom_arm(const void *dummy);
rsavitski 70:0da6ca845762 40
rsavitski 69:4b7bb92916da 41 void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
rsavitski 69:4b7bb92916da 42
rsavitski 30:791739422122 43 void ailayer(void const *dummy)
rsavitski 30:791739422122 44 {
rsavitski 77:8d83a0c00e66 45 MotorControl::motorsenabled = true;
rsavitski 77:8d83a0c00e66 46 motion::collavoiden = 1;
rsavitski 77:8d83a0c00e66 47
rsavitski 77:8d83a0c00e66 48 motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
rsavitski 77:8d83a0c00e66 49
rsavitski 77:8d83a0c00e66 50 while(!motion::checkMotionStatus()); // wait until there
rsavitski 77:8d83a0c00e66 51 MotorControl::motorsenabled = false;
rsavitski 77:8d83a0c00e66 52
rsavitski 77:8d83a0c00e66 53 Thread::signal_wait(0x2);
rsavitski 77:8d83a0c00e66 54
rsavitski 77:8d83a0c00e66 55 // CORD PULLED
rsavitski 77:8d83a0c00e66 56
rsavitski 54:99d3158c9207 57 Waypoint current_waypoint;
rsavitski 69:4b7bb92916da 58
rsavitski 77:8d83a0c00e66 59 current_waypoint.x = 1.5;
rsavitski 77:8d83a0c00e66 60 current_waypoint.y = 1;
rsavitski 77:8d83a0c00e66 61 current_waypoint.theta = (-3.0f/4.0f)*PI;
rsavitski 77:8d83a0c00e66 62 current_waypoint.pos_threshold = 0.03;
rsavitski 77:8d83a0c00e66 63 current_waypoint.angle_threshold = 0.05*PI;
rsavitski 77:8d83a0c00e66 64 current_waypoint.angle_exponent = 512;
rsavitski 77:8d83a0c00e66 65
rsavitski 77:8d83a0c00e66 66 MotorControl::motorsenabled = true;
rsavitski 77:8d83a0c00e66 67 motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
rsavitski 77:8d83a0c00e66 68
rsavitski 77:8d83a0c00e66 69 while(1);
rsavitski 77:8d83a0c00e66 70
rsavitski 77:8d83a0c00e66 71 ///////////////////////////////////////////////////////
rsavitski 55:0c8897da6b3a 72
rsavitski 69:4b7bb92916da 73 // first waypoint for approach
rsavitski 54:99d3158c9207 74 current_waypoint.x = 2.2;
rsavitski 54:99d3158c9207 75 current_waypoint.y = 1.85;
madcowswe 67:be3ea5450cc7 76 current_waypoint.theta = (-3.0f/4.0f)*PI;
madcowswe 67:be3ea5450cc7 77 current_waypoint.pos_threshold = 0.03;
rsavitski 54:99d3158c9207 78 current_waypoint.angle_threshold = 0.02*PI;
rsavitski 69:4b7bb92916da 79
madcowswe 66:f1d75e51398d 80 motion::collavoiden = 1;
rsavitski 69:4b7bb92916da 81 motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
rsavitski 55:0c8897da6b3a 82
rsavitski 68:662164480f60 83 float r = 0.61+0.02+0.01;
rsavitski 53:b013df99b747 84
madcowswe 66:f1d75e51398d 85 bool firstavoidstop = 1;
rsavitski 69:4b7bb92916da 86 delayed_struct ds = {Thread::gettid(),&current_waypoint};
rsavitski 70:0da6ca845762 87 RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
rsavitski 76:532d9bc1d2aa 88 RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
rsavitski 69:4b7bb92916da 89
rsavitski 54:99d3158c9207 90 for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
rsavitski 30:791739422122 91 {
rsavitski 39:44d3dea4adcc 92 motion::waypoint_flag_mutex.lock();
rsavitski 77:8d83a0c00e66 93 if (motion::checkMotionStatus() && (c_upper.getColour()==own_colour || firstavoidstop) && delayed_done)
rsavitski 30:791739422122 94 {
rsavitski 65:4709ff6c753c 95 //temphack!!!!!
rsavitski 69:4b7bb92916da 96 //Thread::wait(1000);
rsavitski 65:4709ff6c753c 97 arm::upper_arm.go_down();
rsavitski 69:4b7bb92916da 98 delayed_done = false;
rsavitski 76:532d9bc1d2aa 99 top_arm_up_timer.start(1200);
rsavitski 70:0da6ca845762 100 delayed_wp_set.start(2400);
rsavitski 69:4b7bb92916da 101 //Thread::wait(2000);
rsavitski 69:4b7bb92916da 102 //arm::upper_arm.go_up();
rsavitski 65:4709ff6c753c 103
rsavitski 54:99d3158c9207 104 phi -= 22.5/180*PI;
rsavitski 54:99d3158c9207 105 current_waypoint.x = 1.5-r*cos(phi);
rsavitski 54:99d3158c9207 106 current_waypoint.y = 2-r*sin(phi);
rsavitski 54:99d3158c9207 107 current_waypoint.theta = constrainAngle(phi+PI/2);
rsavitski 56:ed585a82092b 108
rsavitski 56:ed585a82092b 109 //arm offset
rsavitski 56:ed585a82092b 110 current_waypoint.x += 0.0425*cos(current_waypoint.theta);
rsavitski 56:ed585a82092b 111 current_waypoint.y += 0.0425*sin(current_waypoint.theta);
madcowswe 67:be3ea5450cc7 112
madcowswe 67:be3ea5450cc7 113 current_waypoint.pos_threshold = 0.01;
madcowswe 67:be3ea5450cc7 114 current_waypoint.angle_threshold = 0.01*PI;
rsavitski 57:d434ceab6892 115
rsavitski 69:4b7bb92916da 116 //motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
rsavitski 69:4b7bb92916da 117 if (firstavoidstop)
rsavitski 69:4b7bb92916da 118 {
madcowswe 66:f1d75e51398d 119 motion::collavoiden = 0;
madcowswe 66:f1d75e51398d 120 firstavoidstop = 0;
madcowswe 66:f1d75e51398d 121 }
madcowswe 66:f1d75e51398d 122 else
madcowswe 66:f1d75e51398d 123 motion::collavoiden = 1;
rsavitski 30:791739422122 124 }
rsavitski 54:99d3158c9207 125 motion::waypoint_flag_mutex.unlock();
rsavitski 54:99d3158c9207 126
rsavitski 54:99d3158c9207 127 Thread::wait(50);
rsavitski 30:791739422122 128 }
rsavitski 30:791739422122 129 }
rsavitski 30:791739422122 130
rsavitski 76:532d9bc1d2aa 131 void raise_top_arm(const void *dummy)
rsavitski 70:0da6ca845762 132 {
rsavitski 70:0da6ca845762 133 arm::upper_arm.go_up();
rsavitski 70:0da6ca845762 134 }
rsavitski 70:0da6ca845762 135
rsavitski 76:532d9bc1d2aa 136 void raise_bottom_arm(const void *dummy)
rsavitski 76:532d9bc1d2aa 137 {
rsavitski 76:532d9bc1d2aa 138 arm::lower_arm.go_up();
rsavitski 76:532d9bc1d2aa 139 }
rsavitski 76:532d9bc1d2aa 140
rsavitski 69:4b7bb92916da 141 void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
rsavitski 69:4b7bb92916da 142 {
rsavitski 69:4b7bb92916da 143 delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
rsavitski 69:4b7bb92916da 144 motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
rsavitski 69:4b7bb92916da 145 delayed_done = true;
rsavitski 69:4b7bb92916da 146 }
rsavitski 69:4b7bb92916da 147
rsavitski 30:791739422122 148 } //namespace