2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Revision 91:fdadfd883825, committed 2013-10-15
- Comitter:
- rsavitski
- Date:
- Tue Oct 15 12:17:11 2013 +0000
- Parent:
- 90:e4164bb8c60e
- Child:
- 92:4a1225fbb146
- Commit message:
- 2014 ICRS Eurobot codebase
Changed in this revision
--- a/Actuators/Arms/Arm.cpp Wed Apr 17 23:16:25 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,9 +0,0 @@ -#include "Arm.h" - -namespace arm -{ - -Arm lower_arm(p25, 0.05, 0.9, 1.9);//2.0 -Arm upper_arm(p26, 0.05, 0.6, 2.35);//2.5 - -} //namespace \ No newline at end of file
--- a/Actuators/Arms/Arm.h Wed Apr 17 23:16:25 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,44 +0,0 @@ -#ifndef EUROBOT_ACTUATORS_ARMS_ARM_H_ -#define EUROBOT_ACTUATORS_ARMS_ARM_H_ - -#include "mbed.h" - -namespace arm -{ - -class Arm; - -extern Arm lower_arm; -extern Arm upper_arm; - - -class Arm -{ -public: - Arm (PwmOut pwm_in, float period_in, float min_pos_in, float max_pos_in) - : pwm_(pwm_in), period_(period_in), min_pos_(min_pos_in), max_pos_(max_pos_in) - { - pwm_.period(period_); - } - - void go_up() - { - pwm_.pulsewidth_us(max_pos_*1000); - } - - void go_down() - { - pwm_.pulsewidth_us(min_pos_*1000); - } - -private: - PwmOut pwm_; - float period_; - float min_pos_; - float max_pos_; - -}; - -} //namespace - -#endif \ No newline at end of file
--- 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
--- a/Processes/AI/ai.h Wed Apr 17 23:16:25 2013 +0000 +++ b/Processes/AI/ai.h Tue Oct 15 12:17:11 2013 +0000 @@ -1,11 +1,11 @@ -#ifndef EUROBOT_PROCESSES_AI_AI_H_ -#define EUROBOT_PROCESSES_AI_AI_H_ - -namespace AI -{ - -void ailayer(void const *dummy); - -} - -#endif \ No newline at end of file +//#ifndef EUROBOT_PROCESSES_AI_AI_H_ +//#define EUROBOT_PROCESSES_AI_AI_H_ +// +//namespace AI +//{ +// +//void ailayer(void const *dummy); +// +//} +// +//#endif \ No newline at end of file
--- a/Processes/Motion/motion.cpp Wed Apr 17 23:16:25 2013 +0000 +++ b/Processes/Motion/motion.cpp Tue Oct 15 12:17:11 2013 +0000 @@ -11,7 +11,6 @@ #include "Kalman.h" #include "MotorControl.h" #include "supportfuncs.h" -#include "AvoidDstSensor.h" namespace motion { @@ -25,8 +24,11 @@ namespace motion { -volatile int collavoiden = 0; // TODO: kill oskar's code -AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR); //TODO: kill oskar's hack +//2014: rework mutable waypoints and make them nicer + + +//volatile int collavoiden = 0; // TODO: kill oskar's code +//AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR); //TODO: kill oskar's hack //2014: redo collision avoidance // motion commands supported enum motion_type_t { motion_waypoint }; @@ -158,8 +160,9 @@ //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v); + //2014 //TODO: remove oskar's avoidance hack - if(collavoiden) + /*if(collavoiden) { float d = ADS.Distanceincm(); if(d > 10) @@ -167,6 +170,7 @@ forward_v *= max(min((d-15)*(1.0f/25.0f),1.0f),-0.1f); } } + */ // end of Oskar hack // pass values to the motor control
--- a/Sensors/AvoidDstSensor/AvoidDstSensor.h Wed Apr 17 23:16:25 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,17 +0,0 @@ -#include "mbed.h" - -class AvoidDstSensor{ - private: - AnalogIn ain; - - public: - AvoidDstSensor(PinName analoginpin) : ain(analoginpin){} - - float Raw(){return ain;} - - float Distanceincm(){ - float d = ((1.0f/ain)-0.5f)*(1.0f/0.11f); - d = (d < 10 || d > 50)? -1:d; - return d; - } -}; \ No newline at end of file
--- a/Sensors/CakeSensor/CakeSensor.h Wed Apr 17 23:16:25 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,33 +0,0 @@ - -// Eurobot13 CakeSensor.h - -#include "mbed.h" - -class CakeSensor{ - private: - AnalogIn ain; - - public: - CakeSensor(PinName analoginpin) : ain(analoginpin){} - - float Distance(){return ain;} - - float Distanceincm(){ - //float d = 5.5/(Distance()-0.13); - float d = 7.53/(Distance()-0.022); - d = (d < 6 || d > 30)? -1:d; - - return d; - } -}; - /* - data = {{1/6,0.95},{1/9, 0.86}, {1/12, 0.65}, {1/15, 0.52}, {1/18, 0.44}, {1/21, 0.38}, {1/24, 0.33}, {1/27, 0.30}, {1/30, 0.28}} - Regress[data, {1, x}, x] - float d = 5.5/(Distance()-0.13); - - - data2 = {{1/9, 0.86}, {1/12, 0.65}, {1/15, 0.52}, {1/18, 0.44}, {1/21, 0.38}, {1/24, 0.33}, {1/27, 0.30}, {1/30, 0.28}} - Regress[data2, {1, x}, x] - float d = 7.53/(Distance()-0.022); - - */ \ No newline at end of file
--- a/Sensors/Colour/Colour.cpp Wed Apr 17 23:16:25 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,110 +0,0 @@ - -// Eurobot13 Colour.cpp - -#include "Colour.h" -#include "mbed.h" - - -Colour::Colour(PinName _blue_led, - PinName _red_led, - PinName _pt, - ArmEnum _arm) - : blue_led(_blue_led), - red_led(_red_led), - pt(_pt), - arm(_arm) -{ - - - - - - - if (arm == UPPER) { - red_correction_factor = UPPERARM_CORRECTION; - } else if (arm == LOWER) { - red_correction_factor = LOWERARM_CORRECTION; - } else { - red_correction_factor = 0.00f; - } - - togglecolour = 0; - blue = 0; - red = 0; - noise = 0; - buff_pointer = 0; - - - for (int i = 0; i < BUFF_SIZE; i++) { - blue_buff[i] = 0; - red_buff[i] = 0; - noise_buff[i] = 0; - } - - ticker.attach(this, &Colour::Blink, 0.01); - -} - -void Colour::Blink (void) -{ - - - if (togglecolour == 0) { - - float noise_temp = pt.read(); - noise += (noise_temp - noise_buff[buff_pointer])/BUFF_SIZE; - noise_buff[buff_pointer] = noise_temp; - - buff_pointer = (buff_pointer + 1) % BUFF_SIZE; - - - SNR = 20.0f*log10(hypot(blue,red)/noise); - - float blue_base = (blue - noise); - float red_base = (red - noise)*red_correction_factor; - colour = atan2(red_base,blue_base); - - //toggles leds for the next state - blue_led = 1; - red_led = 0; - } else if (togglecolour == 1) { - float blue_temp = pt.read(); - blue += (blue_temp - blue_buff[buff_pointer])/BUFF_SIZE; - blue_buff[buff_pointer] = blue_temp; - //toggles leds for the next state - blue_led = 0; - red_led = 1; - } else if (togglecolour == 2) { - float red_temp = pt.read(); - red += (red_temp - red_buff[buff_pointer])/BUFF_SIZE; - red_buff[buff_pointer] = red_temp; - //toggles leds for the next state - blue_led = 0; - red_led = 0; - } - - - - - togglecolour = (togglecolour + 1) % 3; - - -} - -ColourEnum Colour::getColour() -{ - if (SNR > SNR_THRESHOLD_DB) { - if ((colour >= -30*PI/180) && (colour < 30*PI/180)) { - return BLUE; - } else if ((colour >= 30*PI/180) && (colour < 60*PI/180)) { - return WHITE; - } else if ((colour >= 60*PI/180) && (colour < 120*PI/180)) { - return RED; - } else { - return BLACK; - } - } else { - return BLACK; - } - -}
--- a/Sensors/Colour/Colour.h Wed Apr 17 23:16:25 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,56 +0,0 @@ - -// Eurobot13 Colour.h -#ifndef COLOUR_H -#define COLOUR_H - -#include "mbed.h" -#include "globals.h" -#include "math.h" - -#define BUFF_SIZE 10 -#define SNR_THRESHOLD_DB 4 - -#define UPPERARM_CORRECTION 2.310f -#define LOWERARM_CORRECTION 1.000f - - -enum ColourEnum {BLUE=0, RED, WHITE, BLACK}; -enum ArmEnum {UPPER=0, LOWER}; - -class Colour{ -public: - - Colour( - PinName blue_led, - PinName red_led, - PinName pt, - ArmEnum arm); - - ColourEnum getColour(); - - -private: - Ticker ticker; - DigitalOut blue_led; - DigitalOut red_led; - AnalogIn pt; - ArmEnum arm; - - float red_correction_factor; - float colour; - float SNR; - void Blink(); - - int togglecolour; - float blue; - float blue_buff[BUFF_SIZE]; - float red; - float red_buff[BUFF_SIZE]; - float noise; - float noise_buff[BUFF_SIZE]; - - int buff_pointer; - -}; - -#endif \ No newline at end of file
--- a/globals.h Wed Apr 17 23:16:25 2013 +0000 +++ b/globals.h Tue Oct 15 12:17:11 2013 +0000 @@ -1,4 +1,3 @@ - #ifndef GLOBALS_H #define GLOBALS_H
--- a/main.cpp Wed Apr 17 23:16:25 2013 +0000 +++ b/main.cpp Tue Oct 15 12:17:11 2013 +0000 @@ -2,11 +2,8 @@ #include "Kalman.h" #include "mbed.h" #include "rtos.h" -#include "Arm.h" #include "MainMotor.h" #include "Encoder.h" -#include "Colour.h" -#include "CakeSensor.h" #include "Printing.h" #include "coprocserial.h" #include <algorithm> @@ -81,7 +78,7 @@ Ticker motorcontroltestticker; motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05); // ai layer thread - Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048); + //Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048); //2014: add new ai layer // motion layer periodic callback RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic); @@ -102,7 +99,7 @@ Timeout timeout_90s; timeout_90s.attach(timeisup_isr, 90); - aithread.signal_set(0x2); //Tell AI thread that its time to go + //aithread.signal_set(0x2); //Tell AI thread that its time to go //2014 measureCPUidle(); //repurpose thread for idle measurement @@ -194,7 +191,7 @@ } } */ - +/* void cakesensortest() { wait(1); @@ -235,7 +232,7 @@ } } - +*/ /* void pt_test()
--- a/tvmet/tvmet.h Wed Apr 17 23:16:25 2013 +0000 +++ b/tvmet/tvmet.h Tue Oct 15 12:17:11 2013 +0000 @@ -26,6 +26,15 @@ #include <tvmet/config.h> +// BEGIN typeid hack +#include <string> +class typeid_hack{ +public: + string name() { return "dummy_typeid"; } +}; +#define typeid(arg...) typeid_hack() +// END typeid hack + /*********************************************************************** * Compiler specifics