automode.cpp

用于机器人自动低分辨路的地图测绘程序。用于机器人控制测绘。分为远端控制端和本地控制端。控制电机为标准舵机。

/*
    Robot Interface
    (C) 2006 Jason Hunt
    [e-mail]
    
    File: automode.cpp
*/

#include <windows.h>
#include <stdio.h>

#include "automode.h"
#include "display.h"
#include "robot.h"


HANDLE automode_thread_handle;               // Thread handle
unsigned long automode_thread_id;            // Thread ID
bool automode_thread_running;  

CRITICAL_SECTION automode_thread_running_lock;


/* Set inital values for auto mode */
void automode_start(void)
{
    InitializeCriticalSection(&automode_thread_running_lock);
    add_line("Automode start");    
    
    automode_thread_running = true;
    // Create camera thread
	automode_thread_handle  = CreateThread(NULL, 0, automode_thread, NULL, 0, &automode_thread_id);      
        
}
/* End of automode start */


/* Shutdown automode */
void automode_stop(void)
{
    EnterCriticalSection(&automode_thread_running_lock);
    automode_thread_running = false;                         // Tell thread to exit
    LeaveCriticalSection(&automode_thread_running_lock);

    WaitForSingleObject(automode_thread_handle, INFINITE); // Wait for thread to terminate
}
/* End of automode_stop */


/* Core thread for automode */
DWORD WINAPI automode_thread(LPVOID p)
{
    DWORD length;

    control_type c;
    telemetry_type t;
    
    static int cur_mode = AUTO_START_FORWARD;

    bool done = false;

    while (!done)
    {
        // Preform serial transfer, and send result to update    

        Sleep(10);
        

        if (robot.get_auto_mode())
        {

            
            if (cur_mode == AUTO_START_FORWARD)
            {
                add_line("Enter mode forward");
                robot.get_control(&c);                
                
                c.left_motor_target = 0x30;
                c.right_motor_target = 0x30;
   
                c.left_motor_dir = 0;
                c.right_motor_dir = 0;                                                                            
   
                robot.set_control(c);  
                
                cur_mode = AUTO_FORWARD;                                             
            }


            if (cur_mode == AUTO_FORWARD)
            {
                robot.get_telemetry(&t);
                
                char b[300];
                
                sprintf(b, "analog: %d",  t.analog_data[2]);
                add_line(b);               

                if (t.analog_data[2] > 150 || 
                    t.analog_data[0] > 200 || 
                    t.analog_data[1] > 200)
                {
                    add_line("Detect");
                    cur_mode = AUTO_AVOID_STOP;
                }        

            }
     
            if (cur_mode == AUTO_AVOID_STOP)
            {
//                add_line("Enter mode stop");
                robot.get_control(&c);                
                
                c.left_motor_target = 0x0;
                c.right_motor_target = 0x0;
                                                                             
                robot.set_control(c); 
                
                Sleep(1000);
                
                cur_mode = AUTO_BACKUP;                                                                             
            }        
            
            
            
            if (cur_mode == AUTO_STOP)
            {
//                add_line("Enter mode stop");
                robot.get_control(&c);                
                
                c.left_motor_target = 0x0;
                c.right_motor_target = 0x0;
                                                                             
                robot.set_control(c);  
                                                            
            }     
     
            if (cur_mode == AUTO_BACKUP)
            {
//                add_line("Enter mode stop");
                robot.get_control(&c);                
                
                c.left_motor_target = 0x30;
                c.right_motor_target = 0x30;
                                                                             
                c.left_motor_dir= 1;
                c.right_motor_dir= 1;

                robot.set_control(c); 
                
                Sleep(1000);
                
                robot.get_control(&c);                
                
                c.left_motor_target = 0;
                c.right_motor_target = 0;
                                                                             
                c.left_motor_dir= 0;
                c.right_motor_dir= 0;

                robot.set_control(c); 
                
                Sleep(500);

                cur_mode = AUTO_DETERMINE_DIR;                                                                             
            }        
                 

            if (cur_mode == AUTO_DETERMINE_DIR)
            {
                robot.get_telemetry(&t);

                // Left is greater, go right
                if (t.analog_data[0] > t.analog_data[1])
                    cur_mode = AUTO_TURN_RIGHT;
                else
                    cur_mode = AUTO_TURN_LEFT;
            }                


            if (cur_mode == AUTO_TURN_RIGHT)
            {
//                add_line("Enter mode stop");
                robot.get_control(&c);                
                
                c.left_motor_target = 0xa0;;
                c.right_motor_target = 0xa0;
                                                                             
                c.left_motor_dir= 0;
                c.right_motor_dir= 1;

                robot.set_control(c); 
                
                Sleep(800);
                            
                robot.get_control(&c);                
                
                c.left_motor_target = 0;;
                c.right_motor_target = 0;
                                                                             
                c.left_motor_dir= 0;
                c.right_motor_dir= 0;

                robot.set_control(c); 
                
                Sleep(1000);
                    
                cur_mode = AUTO_START_FORWARD;
            }

            if (cur_mode == AUTO_TURN_LEFT)
            {
//                add_line("Enter mode stop");
                robot.get_control(&c);                
                
                c.left_motor_target = 0xa0;;
                c.right_motor_target = 0xa0;
                                                                             
                c.left_motor_dir= 1;
                c.right_motor_dir= 0;

                robot.set_control(c); 
                
                Sleep(800);
                            
                robot.get_control(&c);                
                
                c.left_motor_target = 0;;
                c.right_motor_target = 0;
                                                                             
                c.left_motor_dir= 0;
                c.right_motor_dir= 0;

                robot.set_control(c); 
                
                Sleep(1000);
                    
                cur_mode = AUTO_START_FORWARD;
            }
                                        
        }
        else
            cur_mode = AUTO_START_FORWARD;

        
        EnterCriticalSection(&automode_thread_running_lock);
        done = !automode_thread_running;
        LeaveCriticalSection(&automode_thread_running_lock);
    }
        
    return(0);
}
/* End of automode thread*/