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@78:3178a1e46146, 2013-04-15 (annotated)

Committer:
rsavitski
Date:
Mon Apr 15 17:07:40 2013 +0000
Revision:
78:3178a1e46146
Parent:
74:9620d24a2f4e
Parent:
77:8d83a0c00e66
Child:
81:ef1ce4f5322b
merged x2

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 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 78:3178a1e46146 45 RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
rsavitski 78:3178a1e46146 46 RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
rsavitski 78:3178a1e46146 47
rsavitski 78:3178a1e46146 48 //NB: reassign ds.wpptr before each invocation
rsavitski 78:3178a1e46146 49 delayed_struct ds = {Thread::gettid(),NULL};
rsavitski 78:3178a1e46146 50 RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
rsavitski 78:3178a1e46146 51
rsavitski 77:8d83a0c00e66 52 MotorControl::motorsenabled = true;
rsavitski 77:8d83a0c00e66 53 motion::collavoiden = 1;
rsavitski 77:8d83a0c00e66 54
rsavitski 77:8d83a0c00e66 55 motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
rsavitski 78:3178a1e46146 56 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 78:3178a1e46146 57
rsavitski 77:8d83a0c00e66 58 MotorControl::motorsenabled = false;
rsavitski 77:8d83a0c00e66 59
rsavitski 78:3178a1e46146 60 Thread::signal_wait(0x2); //wait for cord
rsavitski 77:8d83a0c00e66 61
rsavitski 77:8d83a0c00e66 62 // CORD PULLED
rsavitski 77:8d83a0c00e66 63
rsavitski 78:3178a1e46146 64 Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
rsavitski 77:8d83a0c00e66 65
rsavitski 77:8d83a0c00e66 66 MotorControl::motorsenabled = true;
rsavitski 78:3178a1e46146 67 motion::setNewWaypoint(Thread::gettid(),&mid_wp);
rsavitski 78:3178a1e46146 68 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 78:3178a1e46146 69
rsavitski 78:3178a1e46146 70 Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
rsavitski 78:3178a1e46146 71 motion::setNewWaypoint(Thread::gettid(),&approach_wp);
rsavitski 78:3178a1e46146 72 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 77:8d83a0c00e66 73
rsavitski 78:3178a1e46146 74 // TODO: ?(disable collison avoidance), approach first cake wp, enable collision
rsavitski 78:3178a1e46146 75 /*
rsavitski 78:3178a1e46146 76 ///////////////temp hack
rsavitski 78:3178a1e46146 77 Waypoint temp_wp = {1.222,1.440, 149.0/180*PI, 0.01, 0.02*PI, 512};
rsavitski 78:3178a1e46146 78 motion::setNewWaypoint(Thread::gettid(),&temp_wp);
rsavitski 78:3178a1e46146 79 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 78:3178a1e46146 80 arm::lower_arm.go_down();
rsavitski 78:3178a1e46146 81 bottom_arm_up_timer.start(1200);
rsavitski 78:3178a1e46146 82 /////////////// temp hack over
rsavitski 78:3178a1e46146 83 */
rsavitski 77:8d83a0c00e66 84
rsavitski 78:3178a1e46146 85 while(1)
rsavitski 78:3178a1e46146 86 Thread::wait(1000);
rsavitski 78:3178a1e46146 87 /*
rsavitski 77:8d83a0c00e66 88 ///////////////////////////////////////////////////////
rsavitski 55:0c8897da6b3a 89
rsavitski 69:4b7bb92916da 90 // first waypoint for approach
rsavitski 54:99d3158c9207 91 current_waypoint.x = 2.2;
rsavitski 54:99d3158c9207 92 current_waypoint.y = 1.85;
madcowswe 67:be3ea5450cc7 93 current_waypoint.theta = (-3.0f/4.0f)*PI;
madcowswe 67:be3ea5450cc7 94 current_waypoint.pos_threshold = 0.03;
rsavitski 54:99d3158c9207 95 current_waypoint.angle_threshold = 0.02*PI;
rsavitski 69:4b7bb92916da 96
madcowswe 66:f1d75e51398d 97 motion::collavoiden = 1;
madcowswe 74:9620d24a2f4e 98 MotorControl::motorsenabled = 1;
rsavitski 69:4b7bb92916da 99 motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
rsavitski 55:0c8897da6b3a 100
rsavitski 78:3178a1e46146 101 float r = 0.26+0.35+0.02+0.01;
rsavitski 53:b013df99b747 102
madcowswe 66:f1d75e51398d 103 bool firstavoidstop = 1;
rsavitski 69:4b7bb92916da 104 delayed_struct ds = {Thread::gettid(),&current_waypoint};
rsavitski 70:0da6ca845762 105 RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
rsavitski 76:532d9bc1d2aa 106 RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
rsavitski 69:4b7bb92916da 107
rsavitski 54:99d3158c9207 108 for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
rsavitski 30:791739422122 109 {
rsavitski 39:44d3dea4adcc 110 motion::waypoint_flag_mutex.lock();
rsavitski 77:8d83a0c00e66 111 if (motion::checkMotionStatus() && (c_upper.getColour()==own_colour || firstavoidstop) && delayed_done)
rsavitski 30:791739422122 112 {
rsavitski 65:4709ff6c753c 113 //temphack!!!!!
rsavitski 69:4b7bb92916da 114 //Thread::wait(1000);
rsavitski 65:4709ff6c753c 115 arm::upper_arm.go_down();
rsavitski 69:4b7bb92916da 116 delayed_done = false;
rsavitski 76:532d9bc1d2aa 117 top_arm_up_timer.start(1200);
rsavitski 70:0da6ca845762 118 delayed_wp_set.start(2400);
rsavitski 69:4b7bb92916da 119 //Thread::wait(2000);
rsavitski 69:4b7bb92916da 120 //arm::upper_arm.go_up();
rsavitski 65:4709ff6c753c 121
rsavitski 54:99d3158c9207 122 phi -= 22.5/180*PI;
rsavitski 54:99d3158c9207 123 current_waypoint.x = 1.5-r*cos(phi);
rsavitski 54:99d3158c9207 124 current_waypoint.y = 2-r*sin(phi);
rsavitski 54:99d3158c9207 125 current_waypoint.theta = constrainAngle(phi+PI/2);
rsavitski 56:ed585a82092b 126
rsavitski 56:ed585a82092b 127 //arm offset
rsavitski 56:ed585a82092b 128 current_waypoint.x += 0.0425*cos(current_waypoint.theta);
rsavitski 56:ed585a82092b 129 current_waypoint.y += 0.0425*sin(current_waypoint.theta);
madcowswe 67:be3ea5450cc7 130
madcowswe 67:be3ea5450cc7 131 current_waypoint.pos_threshold = 0.01;
madcowswe 67:be3ea5450cc7 132 current_waypoint.angle_threshold = 0.01*PI;
rsavitski 57:d434ceab6892 133
rsavitski 69:4b7bb92916da 134 //motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
rsavitski 69:4b7bb92916da 135 if (firstavoidstop)
rsavitski 69:4b7bb92916da 136 {
madcowswe 66:f1d75e51398d 137 motion::collavoiden = 0;
madcowswe 66:f1d75e51398d 138 firstavoidstop = 0;
madcowswe 66:f1d75e51398d 139 }
madcowswe 66:f1d75e51398d 140 else
madcowswe 66:f1d75e51398d 141 motion::collavoiden = 1;
rsavitski 30:791739422122 142 }
rsavitski 54:99d3158c9207 143 motion::waypoint_flag_mutex.unlock();
rsavitski 54:99d3158c9207 144
rsavitski 54:99d3158c9207 145 Thread::wait(50);
rsavitski 78:3178a1e46146 146 }*/
rsavitski 30:791739422122 147 }
rsavitski 30:791739422122 148
rsavitski 76:532d9bc1d2aa 149 void raise_top_arm(const void *dummy)
rsavitski 70:0da6ca845762 150 {
rsavitski 70:0da6ca845762 151 arm::upper_arm.go_up();
rsavitski 70:0da6ca845762 152 }
rsavitski 70:0da6ca845762 153
rsavitski 76:532d9bc1d2aa 154 void raise_bottom_arm(const void *dummy)
rsavitski 76:532d9bc1d2aa 155 {
rsavitski 76:532d9bc1d2aa 156 arm::lower_arm.go_up();
rsavitski 76:532d9bc1d2aa 157 }
rsavitski 76:532d9bc1d2aa 158
rsavitski 69:4b7bb92916da 159 void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
rsavitski 69:4b7bb92916da 160 {
rsavitski 69:4b7bb92916da 161 delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
rsavitski 69:4b7bb92916da 162 motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
rsavitski 69:4b7bb92916da 163 delayed_done = true;
rsavitski 69:4b7bb92916da 164 }
rsavitski 69:4b7bb92916da 165
rsavitski 30:791739422122 166 } //namespace