File content as of revision 78:3178a1e46146:

#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

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

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

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::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
    Thread::signal_wait(0x1); //wait until wp reached
   
    MotorControl::motorsenabled = false;
    
    Thread::signal_wait(0x2); //wait for cord
    
    // CORD PULLED
    
    Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
    
    MotorControl::motorsenabled = true;
    motion::setNewWaypoint(Thread::gettid(),&mid_wp);
    Thread::signal_wait(0x1); //wait until wp reached
    
    Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
    motion::setNewWaypoint(Thread::gettid(),&approach_wp); 
    Thread::signal_wait(0x1); //wait until wp reached
    
    // TODO: ?(disable collison avoidance), approach first cake wp, enable collision
    /*
    ///////////////temp hack
    Waypoint temp_wp = {1.222,1.440, 149.0/180*PI, 0.01, 0.02*PI, 512};
    motion::setNewWaypoint(Thread::gettid(),&temp_wp); 
    Thread::signal_wait(0x1); //wait until wp reached
    arm::lower_arm.go_down();
    bottom_arm_up_timer.start(1200);
    /////////////// temp hack over
    */
    
    while(1)
        Thread::wait(1000);
    /*
    ///////////////////////////////////////////////////////

    // first waypoint for approach
    current_waypoint.x = 2.2;
    current_waypoint.y = 1.85;
    current_waypoint.theta = (-3.0f/4.0f)*PI;
    current_waypoint.pos_threshold = 0.03;
    current_waypoint.angle_threshold = 0.02*PI;
    
    motion::collavoiden = 1;
    MotorControl::motorsenabled = 1;
    motion::setNewWaypoint(Thread::gettid(),&current_waypoint);

    float r = 0.26+0.35+0.02+0.01;

    bool firstavoidstop = 1;
    delayed_struct ds = {Thread::gettid(),&current_waypoint};
    RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
    RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
    
    for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
    {
        motion::waypoint_flag_mutex.lock();
        if (motion::checkMotionStatus() && (c_upper.getColour()==own_colour || firstavoidstop) && delayed_done)
        {
            //temphack!!!!!
            //Thread::wait(1000);
            arm::upper_arm.go_down();
            delayed_done = false;
            top_arm_up_timer.start(1200);
            delayed_wp_set.start(2400);
            //Thread::wait(2000);
            //arm::upper_arm.go_up();
            
            phi -= 22.5/180*PI;
            current_waypoint.x = 1.5-r*cos(phi);
            current_waypoint.y = 2-r*sin(phi);
            current_waypoint.theta = constrainAngle(phi+PI/2);
            
            //arm offset
            current_waypoint.x += 0.0425*cos(current_waypoint.theta);
            current_waypoint.y += 0.0425*sin(current_waypoint.theta);
            
            current_waypoint.pos_threshold = 0.01;
            current_waypoint.angle_threshold = 0.01*PI;

            //motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
            if (firstavoidstop)
            {
                motion::collavoiden = 0;
                firstavoidstop = 0;
            }
            else
                motion::collavoiden = 1;
        }
        motion::waypoint_flag_mutex.unlock();            
        
        Thread::wait(50);
    }*/
}

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