File content as of revision 87:272a7129b04b:

#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_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.83/*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
    
    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<17; ++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;
        }
        
        // 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) && MotorControl::motorsenabled)
            {
                arm::upper_arm.go_down();
                top_arm_up_timer.start(1100);
            }
        }
        else
        {
            ColourEnum colour_read = c_lower.getColour();
            if ((colour_read==own_colour || i==5 || i==7 || i==8 || i==10/*|| colour_read==WHITE*/) && MotorControl::motorsenabled)
            {
                arm::lower_arm.go_down();
                bottom_arm_up_timer.start(1100);
            }
        }
        Thread::wait(2000);
    }
    
    ////////////////////
    
    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