Ryan Savitski
2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/AI/ai.cpp
- Revision:
- 91:fdadfd883825
- Parent:
- 90:e4164bb8c60e
diff -r e4164bb8c60e -r fdadfd883825 Processes/AI/ai.cpp
--- a/Processes/AI/ai.cpp Wed Apr 17 23:16:25 2013 +0000
+++ b/Processes/AI/ai.cpp Tue Oct 15 12:17:11 2013 +0000
@@ -1,296 +1,294 @@
-#include "ai.h"
-#include "rtos.h"
-#include "globals.h"
-#include "motion.h"
-#include "Colour.h"
-#include "supportfuncs.h"
-#include "Arm.h"
-#include "MotorControl.h"
-
-//TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish
-
-//#define HARDCODED_WAYPOINTS_HACK
-
-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);
-
-namespace AI
-{
-// starting position
-#ifdef TEAM_RED
-ColourEnum own_colour = RED;
-
-Waypoint home_wp = {2.7, 1.75, PI, 0.03, 0.05*PI, 32};
-#endif
-
-#ifdef TEAM_BLUE
-ColourEnum own_colour = BLUE;
-
-Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32};
-#endif
-
-enum layer { top_layer, bot_layer };
-
-bool delayed_done = true; //TODO: kill
-
-struct delayed_struct //TODO: kill
-{
- osThreadId tid;
- Waypoint *wpptr;
-};
-
-void raise_top_arm(const void *dummy);
-void raise_bottom_arm(const void *dummy);
-
-void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
-
-#ifndef HARDCODED_WAYPOINTS_HACK
-void ailayer(void const *dummy)
-{
- RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
- RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
-
- //NB: reassign ds.wpptr before each invocation
- //delayed_struct ds = {Thread::gettid(),NULL};
- //RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
-
- MotorControl::motorsenabled = true;
- motion::collavoiden = 1;
-
- motion::waypoint_flag_mutex.lock();
- motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
- motion::waypoint_flag_mutex.unlock();
- Thread::signal_wait(0x1); //wait until wp reached
-
- MotorControl::motorsenabled = false;
- arm::upper_arm.go_up();
- arm::lower_arm.go_down();
-
- Thread::signal_wait(0x2); //wait for cord
-
- // CORD PULLED
- MotorControl::motorsenabled = true;
- arm::lower_arm.go_up();
-
- #ifdef TEAM_BLUE
-
- Waypoint mid_wp5 = {1.2, 1, 0, 0.10, 0.15*PI, 32};
- motion::waypoint_flag_mutex.lock();
- motion::setNewWaypoint(Thread::gettid(),&mid_wp5);
- motion::waypoint_flag_mutex.unlock();
- Thread::signal_wait(0x1); //wait until wp reached
- Thread::wait(3000);
-
- Waypoint mid_wp = {1.9, 1.1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
- motion::waypoint_flag_mutex.lock();
- motion::setNewWaypoint(Thread::gettid(),&mid_wp);
- motion::waypoint_flag_mutex.unlock();
- Thread::signal_wait(0x1); //wait until wp reached
-
- Waypoint approach_wp = {2.24, 1.84/*1.85*/, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
- motion::waypoint_flag_mutex.lock();
- motion::setNewWaypoint(Thread::gettid(),&approach_wp);
- motion::waypoint_flag_mutex.unlock();
- Thread::signal_wait(0x1); //wait until wp reached
- #endif
- /*
- Waypoint approach_wp2 = {2.2-0.05, 1.83, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
- motion::waypoint_flag_mutex.lock();
- motion::setNewWaypoint(Thread::gettid(),&approach_wp2);
- motion::waypoint_flag_mutex.unlock();
- Thread::signal_wait(0x1); //wait until wp reached*/
-
- Waypoint mutable_cake_wp = {0, 0, 0, 0.01, 0.01*PI, 512};
-
- float r = 0.26+0.35-0.0075;//-0.01; //second 0.01 for being less collisiony with sensors
-
- layer layer_to_push = top_layer;
-
- float top_target_phi = (180-(11.25/*+22.5*/))/180*PI;
- float bot_target_phi = (180-(7.5+15))/180*PI;
-
- float phi = 0; // angle of vector (robot->center of cake)
-
- for(int i=0; i<18; ++i)
- {
- if (top_target_phi > bot_target_phi)
- {
- layer_to_push = top_layer;
- phi = top_target_phi;
- top_target_phi -= 22.5/180*PI;
- }
- else
- {
- layer_to_push = bot_layer;
- phi = bot_target_phi;
- bot_target_phi -= 15.0/180*PI;
- }
-
- // hack
- if (own_colour==BLUE && i==0)
- continue;
-
- // set new wp
- mutable_cake_wp.x = 1.5-r*cos(phi);
- mutable_cake_wp.y = 2-r*sin(phi);
- mutable_cake_wp.theta = constrainAngle(phi+PI/2);
-
- //arm offset
- mutable_cake_wp.x += 0.0425*cos(mutable_cake_wp.theta);
- mutable_cake_wp.y += 0.0425*sin(mutable_cake_wp.theta);
-
-
- motion::waypoint_flag_mutex.lock();
- motion::setNewWaypoint(Thread::gettid(),&mutable_cake_wp);
- motion::waypoint_flag_mutex.unlock();
- Thread::signal_wait(0x1); //wait until wp reached
-
- if(layer_to_push == top_layer)
- {
- ColourEnum colour_read = c_upper.getColour();
- if ((colour_read==own_colour || i==0) && MotorControl::motorsenabled)
- {
- arm::upper_arm.go_down();
- top_arm_up_timer.start(1200);
- }
- }
- else
- {
- ColourEnum colour_read = c_lower.getColour();
- if ((colour_read==own_colour || i==5+1 || i==7+1 || i==8+1 || i==10+1/*|| colour_read==WHITE*/) && MotorControl::motorsenabled)
- {
- arm::lower_arm.go_down();
- bottom_arm_up_timer.start(1200);
- }
- }
- Thread::wait(2100);
- }
-
- ////////////////////
-
- while(1)
- Thread::wait(1000);
-}
-#else
-enum action_t {top_push_colour, bot_push_colour, bot_push_white};
-
-void ailayer(void const *dummy)
-{
- RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
- RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
-
- ////////////////////////////////////
- // put waypoints here
- ////////////////////////////////////
- const int wp_num = 3;
-
- float x_arr[wp_num] = {1.2,1.5,1.7}; //<--------------------------
- float y_arr[wp_num] = {1,1.2,1.4}; //<--------------------------
- float theta_arr[wp_num] = {0,PI, PI/2}; //<----------------------
- action_t action[wp_num] = {top_push_colour, bot_push_colour, bot_push_white}; //<----------------------------
-
- Waypoint wp_arr[wp_num];
-
- for(int i=0; i<wp_num; ++i)
- {
- wp_arr[i].x =x_arr[i];
- wp_arr[i].y =y_arr[i];
- wp_arr[i].theta =theta_arr[i];
-
- wp_arr[i].pos_threshold = 0.01;
- wp_arr[i].angle_threshold = 0.01*PI;
- wp_arr[i].angle_exponent = 512;
- }
-
- ////////////////////////////////////
-
- MotorControl::motorsenabled = true;
- motion::collavoiden = 1;
-
- motion::waypoint_flag_mutex.lock();
- motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
- motion::waypoint_flag_mutex.unlock();
- Thread::signal_wait(0x1); //wait until wp reached
-
- MotorControl::motorsenabled = false;
- arm::upper_arm.go_up();
- arm::lower_arm.go_up();
-
- Thread::signal_wait(0x2); //wait for cord
-
- // CORD PULLED
- MotorControl::motorsenabled = true;
-
- #ifdef TEAM_BLUE
- Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
- motion::waypoint_flag_mutex.lock();
- motion::setNewWaypoint(Thread::gettid(),&mid_wp);
- motion::waypoint_flag_mutex.unlock();
- Thread::signal_wait(0x1); //wait until wp reached
- #endif
-
- Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
- motion::waypoint_flag_mutex.lock();
- motion::setNewWaypoint(Thread::gettid(),&approach_wp);
- motion::waypoint_flag_mutex.unlock();
- Thread::signal_wait(0x1); //wait until wp reached
-
-
- for(int i=0; i<wp_num; ++i)
- {
- motion::waypoint_flag_mutex.lock();
- motion::setNewWaypoint(Thread::gettid(),&(wp_arr[i])); //go to home
- motion::waypoint_flag_mutex.unlock();
- Thread::signal_wait(0x1); //wait until wp reached
-
- switch(action[i])
- {
- case top_push_colour:
- if ((c_upper.getColour()==own_colour) && MotorControl::motorsenabled)
- {
- arm::upper_arm.go_down();
- top_arm_up_timer.start(1200);
- }
- break;
- case bot_push_colour:
- if ((c_lower.getColour()==own_colour) && MotorControl::motorsenabled)
- {
- arm::lower_arm.go_down();
- bottom_arm_up_timer.start(1200);
- }
- break;
- case bot_push_white:
- if (MotorControl::motorsenabled)
- {
- arm::lower_arm.go_down();
- bottom_arm_up_timer.start(1200);
- }
- break;
- }
-
- Thread::wait(2200);
- }
-
- while(1)
- Thread::wait(1000);
-}
-#endif
-
-void raise_top_arm(const void *dummy)
-{
- arm::upper_arm.go_up();
-}
-
-void raise_bottom_arm(const void *dummy)
-{
- arm::lower_arm.go_up();
-}
-
-void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
-{
- delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
- motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
- delayed_done = true;
-}
-
-} //namespace
\ No newline at end of file
+//#include "ai.h"
+//#include "rtos.h"
+//#include "globals.h"
+//#include "motion.h"
+//#include "supportfuncs.h"
+//#include "MotorControl.h"
+//
+////TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish
+//
+////#define HARDCODED_WAYPOINTS_HACK
+//
+//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);
+//
+//namespace AI
+//{
+//// starting position
+//#ifdef TEAM_RED
+//ColourEnum own_colour = RED;
+//
+//Waypoint home_wp = {2.7, 1.75, PI, 0.03, 0.05*PI, 32};
+//#endif
+//
+//#ifdef TEAM_BLUE
+//ColourEnum own_colour = BLUE;
+//
+//Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32};
+//#endif
+//
+//enum layer { top_layer, bot_layer };
+//
+//bool delayed_done = true; //TODO: kill
+//
+//struct delayed_struct //TODO: kill
+//{
+// osThreadId tid;
+// Waypoint *wpptr;
+//};
+//
+//void raise_top_arm(const void *dummy);
+//void raise_bottom_arm(const void *dummy);
+//
+//void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
+//
+//#ifndef HARDCODED_WAYPOINTS_HACK
+//void ailayer(void const *dummy)
+//{
+// RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
+// RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
+//
+// //NB: reassign ds.wpptr before each invocation
+// //delayed_struct ds = {Thread::gettid(),NULL};
+// //RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
+//
+// MotorControl::motorsenabled = true;
+// motion::collavoiden = 1;
+//
+// motion::waypoint_flag_mutex.lock();
+// motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
+// motion::waypoint_flag_mutex.unlock();
+// Thread::signal_wait(0x1); //wait until wp reached
+//
+// MotorControl::motorsenabled = false;
+// arm::upper_arm.go_up();
+// arm::lower_arm.go_down();
+//
+// Thread::signal_wait(0x2); //wait for cord
+//
+// // CORD PULLED
+// MotorControl::motorsenabled = true;
+// arm::lower_arm.go_up();
+//
+// #ifdef TEAM_BLUE
+//
+// Waypoint mid_wp5 = {1.2, 1, 0, 0.10, 0.15*PI, 32};
+// motion::waypoint_flag_mutex.lock();
+// motion::setNewWaypoint(Thread::gettid(),&mid_wp5);
+// motion::waypoint_flag_mutex.unlock();
+// Thread::signal_wait(0x1); //wait until wp reached
+// Thread::wait(3000);
+//
+// Waypoint mid_wp = {1.9, 1.1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
+// motion::waypoint_flag_mutex.lock();
+// motion::setNewWaypoint(Thread::gettid(),&mid_wp);
+// motion::waypoint_flag_mutex.unlock();
+// Thread::signal_wait(0x1); //wait until wp reached
+//
+// Waypoint approach_wp = {2.24, 1.84/*1.85*/, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
+// motion::waypoint_flag_mutex.lock();
+// motion::setNewWaypoint(Thread::gettid(),&approach_wp);
+// motion::waypoint_flag_mutex.unlock();
+// Thread::signal_wait(0x1); //wait until wp reached
+// #endif
+// /*
+// Waypoint approach_wp2 = {2.2-0.05, 1.83, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
+// motion::waypoint_flag_mutex.lock();
+// motion::setNewWaypoint(Thread::gettid(),&approach_wp2);
+// motion::waypoint_flag_mutex.unlock();
+// Thread::signal_wait(0x1); //wait until wp reached*/
+//
+// Waypoint mutable_cake_wp = {0, 0, 0, 0.01, 0.01*PI, 512};
+//
+// float r = 0.26+0.35-0.0075;//-0.01; //second 0.01 for being less collisiony with sensors
+//
+// layer layer_to_push = top_layer;
+//
+// float top_target_phi = (180-(11.25/*+22.5*/))/180*PI;
+// float bot_target_phi = (180-(7.5+15))/180*PI;
+//
+// float phi = 0; // angle of vector (robot->center of cake)
+//
+// for(int i=0; i<18; ++i)
+// {
+// if (top_target_phi > bot_target_phi)
+// {
+// layer_to_push = top_layer;
+// phi = top_target_phi;
+// top_target_phi -= 22.5/180*PI;
+// }
+// else
+// {
+// layer_to_push = bot_layer;
+// phi = bot_target_phi;
+// bot_target_phi -= 15.0/180*PI;
+// }
+//
+// // hack
+// if (own_colour==BLUE && i==0)
+// continue;
+//
+// // set new wp
+// mutable_cake_wp.x = 1.5-r*cos(phi);
+// mutable_cake_wp.y = 2-r*sin(phi);
+// mutable_cake_wp.theta = constrainAngle(phi+PI/2);
+//
+// //arm offset
+// mutable_cake_wp.x += 0.0425*cos(mutable_cake_wp.theta);
+// mutable_cake_wp.y += 0.0425*sin(mutable_cake_wp.theta);
+//
+//
+// motion::waypoint_flag_mutex.lock();
+// motion::setNewWaypoint(Thread::gettid(),&mutable_cake_wp);
+// motion::waypoint_flag_mutex.unlock();
+// Thread::signal_wait(0x1); //wait until wp reached
+//
+// if(layer_to_push == top_layer)
+// {
+// ColourEnum colour_read = c_upper.getColour();
+// if ((colour_read==own_colour || i==0) && MotorControl::motorsenabled)
+// {
+// arm::upper_arm.go_down();
+// top_arm_up_timer.start(1200);
+// }
+// }
+// else
+// {
+// ColourEnum colour_read = c_lower.getColour();
+// if ((colour_read==own_colour || i==5+1 || i==7+1 || i==8+1 || i==10+1/*|| colour_read==WHITE*/) && MotorControl::motorsenabled)
+// {
+// arm::lower_arm.go_down();
+// bottom_arm_up_timer.start(1200);
+// }
+// }
+// Thread::wait(2100);
+// }
+//
+// ////////////////////
+//
+// while(1)
+// Thread::wait(1000);
+//}
+//#else
+//enum action_t {top_push_colour, bot_push_colour, bot_push_white};
+//
+//void ailayer(void const *dummy)
+//{
+// RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
+// RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
+//
+// ////////////////////////////////////
+// // put waypoints here
+// ////////////////////////////////////
+// const int wp_num = 3;
+//
+// float x_arr[wp_num] = {1.2,1.5,1.7}; //<--------------------------
+// float y_arr[wp_num] = {1,1.2,1.4}; //<--------------------------
+// float theta_arr[wp_num] = {0,PI, PI/2}; //<----------------------
+// action_t action[wp_num] = {top_push_colour, bot_push_colour, bot_push_white}; //<----------------------------
+//
+// Waypoint wp_arr[wp_num];
+//
+// for(int i=0; i<wp_num; ++i)
+// {
+// wp_arr[i].x =x_arr[i];
+// wp_arr[i].y =y_arr[i];
+// wp_arr[i].theta =theta_arr[i];
+//
+// wp_arr[i].pos_threshold = 0.01;
+// wp_arr[i].angle_threshold = 0.01*PI;
+// wp_arr[i].angle_exponent = 512;
+// }
+//
+// ////////////////////////////////////
+//
+// MotorControl::motorsenabled = true;
+// motion::collavoiden = 1;
+//
+// motion::waypoint_flag_mutex.lock();
+// motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
+// motion::waypoint_flag_mutex.unlock();
+// Thread::signal_wait(0x1); //wait until wp reached
+//
+// MotorControl::motorsenabled = false;
+// arm::upper_arm.go_up();
+// arm::lower_arm.go_up();
+//
+// Thread::signal_wait(0x2); //wait for cord
+//
+// // CORD PULLED
+// MotorControl::motorsenabled = true;
+//
+// #ifdef TEAM_BLUE
+// Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
+// motion::waypoint_flag_mutex.lock();
+// motion::setNewWaypoint(Thread::gettid(),&mid_wp);
+// motion::waypoint_flag_mutex.unlock();
+// Thread::signal_wait(0x1); //wait until wp reached
+// #endif
+//
+// Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
+// motion::waypoint_flag_mutex.lock();
+// motion::setNewWaypoint(Thread::gettid(),&approach_wp);
+// motion::waypoint_flag_mutex.unlock();
+// Thread::signal_wait(0x1); //wait until wp reached
+//
+//
+// for(int i=0; i<wp_num; ++i)
+// {
+// motion::waypoint_flag_mutex.lock();
+// motion::setNewWaypoint(Thread::gettid(),&(wp_arr[i])); //go to home
+// motion::waypoint_flag_mutex.unlock();
+// Thread::signal_wait(0x1); //wait until wp reached
+//
+// switch(action[i])
+// {
+// case top_push_colour:
+// if ((c_upper.getColour()==own_colour) && MotorControl::motorsenabled)
+// {
+// arm::upper_arm.go_down();
+// top_arm_up_timer.start(1200);
+// }
+// break;
+// case bot_push_colour:
+// if ((c_lower.getColour()==own_colour) && MotorControl::motorsenabled)
+// {
+// arm::lower_arm.go_down();
+// bottom_arm_up_timer.start(1200);
+// }
+// break;
+// case bot_push_white:
+// if (MotorControl::motorsenabled)
+// {
+// arm::lower_arm.go_down();
+// bottom_arm_up_timer.start(1200);
+// }
+// break;
+// }
+//
+// Thread::wait(2200);
+// }
+//
+// while(1)
+// Thread::wait(1000);
+//}
+//#endif
+//
+//void raise_top_arm(const void *dummy)
+//{
+// arm::upper_arm.go_up();
+//}
+//
+//void raise_bottom_arm(const void *dummy)
+//{
+// arm::lower_arm.go_up();
+//}
+//
+//void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
+//{
+// delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
+// motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
+// delayed_done = true;
+//}
+//
+//} //namespace
\ No newline at end of file