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