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@81:ef1ce4f5322b, 2013-04-15 (annotated)

Committer:
rsavitski
Date:
Mon Apr 15 19:35:29 2013 +0000
Revision:
81:ef1ce4f5322b
Parent:
78:3178a1e46146
Child:
83:223186cd7531
blue + white tested and works with procedural wps

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 78:3178a1e46146 21 Waypoint home_wp = {2.7, 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 78:3178a1e46146 27 Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32};
rsavitski 77:8d83a0c00e66 28 #endif
rsavitski 30:791739422122 29
rsavitski 81:ef1ce4f5322b 30 enum layer { top_layer, bot_layer };
rsavitski 81:ef1ce4f5322b 31
rsavitski 69:4b7bb92916da 32 bool delayed_done = true; //TODO: kill
rsavitski 69:4b7bb92916da 33
rsavitski 69:4b7bb92916da 34 struct delayed_struct //TODO: kill
rsavitski 69:4b7bb92916da 35 {
rsavitski 69:4b7bb92916da 36 osThreadId tid;
rsavitski 69:4b7bb92916da 37 Waypoint *wpptr;
rsavitski 69:4b7bb92916da 38 };
rsavitski 69:4b7bb92916da 39
rsavitski 76:532d9bc1d2aa 40 void raise_top_arm(const void *dummy);
rsavitski 76:532d9bc1d2aa 41 void raise_bottom_arm(const void *dummy);
rsavitski 70:0da6ca845762 42
rsavitski 69:4b7bb92916da 43 void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
rsavitski 69:4b7bb92916da 44
rsavitski 30:791739422122 45 void ailayer(void const *dummy)
rsavitski 30:791739422122 46 {
rsavitski 78:3178a1e46146 47 RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
rsavitski 78:3178a1e46146 48 RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
rsavitski 78:3178a1e46146 49
rsavitski 78:3178a1e46146 50 //NB: reassign ds.wpptr before each invocation
rsavitski 78:3178a1e46146 51 delayed_struct ds = {Thread::gettid(),NULL};
rsavitski 78:3178a1e46146 52 RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
rsavitski 78:3178a1e46146 53
rsavitski 77:8d83a0c00e66 54 MotorControl::motorsenabled = true;
rsavitski 77:8d83a0c00e66 55 motion::collavoiden = 1;
rsavitski 77:8d83a0c00e66 56
rsavitski 81:ef1ce4f5322b 57 motion::waypoint_flag_mutex.lock();
rsavitski 77:8d83a0c00e66 58 motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
rsavitski 81:ef1ce4f5322b 59 motion::waypoint_flag_mutex.unlock();
rsavitski 78:3178a1e46146 60 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 78:3178a1e46146 61
rsavitski 77:8d83a0c00e66 62 MotorControl::motorsenabled = false;
rsavitski 81:ef1ce4f5322b 63 arm::upper_arm.go_up();
rsavitski 81:ef1ce4f5322b 64 arm::lower_arm.go_up();
rsavitski 77:8d83a0c00e66 65
rsavitski 78:3178a1e46146 66 Thread::signal_wait(0x2); //wait for cord
rsavitski 77:8d83a0c00e66 67
rsavitski 77:8d83a0c00e66 68 // CORD PULLED
rsavitski 81:ef1ce4f5322b 69 MotorControl::motorsenabled = true;
rsavitski 77:8d83a0c00e66 70
rsavitski 81:ef1ce4f5322b 71 #ifdef TEAM_BLUE
rsavitski 81:ef1ce4f5322b 72 Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
rsavitski 81:ef1ce4f5322b 73 motion::waypoint_flag_mutex.lock();
rsavitski 78:3178a1e46146 74 motion::setNewWaypoint(Thread::gettid(),&mid_wp);
rsavitski 81:ef1ce4f5322b 75 motion::waypoint_flag_mutex.unlock();
rsavitski 78:3178a1e46146 76 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 81:ef1ce4f5322b 77 #endif
rsavitski 78:3178a1e46146 78
rsavitski 78:3178a1e46146 79 Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
rsavitski 81:ef1ce4f5322b 80 motion::waypoint_flag_mutex.lock();
rsavitski 78:3178a1e46146 81 motion::setNewWaypoint(Thread::gettid(),&approach_wp);
rsavitski 81:ef1ce4f5322b 82 motion::waypoint_flag_mutex.unlock();
rsavitski 78:3178a1e46146 83 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 77:8d83a0c00e66 84
rsavitski 81:ef1ce4f5322b 85 Waypoint mutable_cake_wp = {0, 0, 0, 0.01, 0.01*PI, 512};
rsavitski 81:ef1ce4f5322b 86
rsavitski 81:ef1ce4f5322b 87 float r = 0.26+0.35+0.01+0.01; //second 0.01 for being less collisiony with sensors
rsavitski 81:ef1ce4f5322b 88
rsavitski 81:ef1ce4f5322b 89 layer layer_to_push = top_layer;
rsavitski 81:ef1ce4f5322b 90
rsavitski 81:ef1ce4f5322b 91 float top_target_phi = (180-(11.25+22.5))/180*PI;
rsavitski 81:ef1ce4f5322b 92 float bot_target_phi = (180-(7.5+15))/180*PI;
rsavitski 81:ef1ce4f5322b 93
rsavitski 81:ef1ce4f5322b 94 float phi = 0; // angle of vector (robot->center of cake)
rsavitski 81:ef1ce4f5322b 95
rsavitski 81:ef1ce4f5322b 96 for(int i=0; i<17; ++i)
rsavitski 81:ef1ce4f5322b 97 {
rsavitski 81:ef1ce4f5322b 98 if (top_target_phi > bot_target_phi)
rsavitski 81:ef1ce4f5322b 99 {
rsavitski 81:ef1ce4f5322b 100 layer_to_push = top_layer;
rsavitski 81:ef1ce4f5322b 101 phi = top_target_phi;
rsavitski 81:ef1ce4f5322b 102 top_target_phi -= 22.5/180*PI;
rsavitski 81:ef1ce4f5322b 103 }
rsavitski 81:ef1ce4f5322b 104 else
rsavitski 81:ef1ce4f5322b 105 {
rsavitski 81:ef1ce4f5322b 106 layer_to_push = bot_layer;
rsavitski 81:ef1ce4f5322b 107 phi = bot_target_phi;
rsavitski 81:ef1ce4f5322b 108 bot_target_phi -= 15.0/180*PI;
rsavitski 81:ef1ce4f5322b 109 }
rsavitski 81:ef1ce4f5322b 110
rsavitski 81:ef1ce4f5322b 111 // set new wp
rsavitski 81:ef1ce4f5322b 112 mutable_cake_wp.x = 1.5-r*cos(phi);
rsavitski 81:ef1ce4f5322b 113 mutable_cake_wp.y = 2-r*sin(phi);
rsavitski 81:ef1ce4f5322b 114 mutable_cake_wp.theta = constrainAngle(phi+PI/2);
rsavitski 81:ef1ce4f5322b 115
rsavitski 81:ef1ce4f5322b 116 //arm offset
rsavitski 81:ef1ce4f5322b 117 mutable_cake_wp.x += 0.0425*cos(mutable_cake_wp.theta);
rsavitski 81:ef1ce4f5322b 118 mutable_cake_wp.y += 0.0425*sin(mutable_cake_wp.theta);
rsavitski 81:ef1ce4f5322b 119
rsavitski 81:ef1ce4f5322b 120
rsavitski 81:ef1ce4f5322b 121 motion::waypoint_flag_mutex.lock();
rsavitski 81:ef1ce4f5322b 122 motion::setNewWaypoint(Thread::gettid(),&mutable_cake_wp);
rsavitski 81:ef1ce4f5322b 123 motion::waypoint_flag_mutex.unlock();
rsavitski 81:ef1ce4f5322b 124 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 81:ef1ce4f5322b 125
rsavitski 81:ef1ce4f5322b 126 if(layer_to_push == top_layer)
rsavitski 81:ef1ce4f5322b 127 {
rsavitski 81:ef1ce4f5322b 128 ColourEnum colour_read = c_upper.getColour();
rsavitski 81:ef1ce4f5322b 129 if (colour_read==own_colour)
rsavitski 81:ef1ce4f5322b 130 {
rsavitski 81:ef1ce4f5322b 131 arm::upper_arm.go_down();
rsavitski 81:ef1ce4f5322b 132 top_arm_up_timer.start(1200);
rsavitski 81:ef1ce4f5322b 133 }
rsavitski 81:ef1ce4f5322b 134 }
rsavitski 81:ef1ce4f5322b 135 else
rsavitski 81:ef1ce4f5322b 136 {
rsavitski 81:ef1ce4f5322b 137 ColourEnum colour_read = c_lower.getColour();
rsavitski 81:ef1ce4f5322b 138 if (colour_read==own_colour || i==5 || i==7 || i==8 || i==10/*|| colour_read==WHITE*/)
rsavitski 81:ef1ce4f5322b 139 {
rsavitski 81:ef1ce4f5322b 140 arm::lower_arm.go_down();
rsavitski 81:ef1ce4f5322b 141 bottom_arm_up_timer.start(1200);
rsavitski 81:ef1ce4f5322b 142 }
rsavitski 81:ef1ce4f5322b 143 }
rsavitski 81:ef1ce4f5322b 144 Thread::wait(2200);
rsavitski 81:ef1ce4f5322b 145 }
rsavitski 81:ef1ce4f5322b 146
rsavitski 81:ef1ce4f5322b 147 ////////////////////
rsavitski 77:8d83a0c00e66 148
rsavitski 78:3178a1e46146 149 while(1)
rsavitski 78:3178a1e46146 150 Thread::wait(1000);
rsavitski 30:791739422122 151 }
rsavitski 30:791739422122 152
rsavitski 76:532d9bc1d2aa 153 void raise_top_arm(const void *dummy)
rsavitski 70:0da6ca845762 154 {
rsavitski 70:0da6ca845762 155 arm::upper_arm.go_up();
rsavitski 70:0da6ca845762 156 }
rsavitski 70:0da6ca845762 157
rsavitski 76:532d9bc1d2aa 158 void raise_bottom_arm(const void *dummy)
rsavitski 76:532d9bc1d2aa 159 {
rsavitski 76:532d9bc1d2aa 160 arm::lower_arm.go_up();
rsavitski 76:532d9bc1d2aa 161 }
rsavitski 76:532d9bc1d2aa 162
rsavitski 69:4b7bb92916da 163 void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
rsavitski 69:4b7bb92916da 164 {
rsavitski 69:4b7bb92916da 165 delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
rsavitski 69:4b7bb92916da 166 motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
rsavitski 69:4b7bb92916da 167 delayed_done = true;
rsavitski 69:4b7bb92916da 168 }
rsavitski 69:4b7bb92916da 169
rsavitski 30:791739422122 170 } //namespace