robot.cpp

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

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


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

#include "robot.h"
#include "serial.h"
#include "display.h"
#include "utility.h"
#include "time.h"
#include "math.h"

bool remote_mode = false;


robot_class robot;  // Core instance of robot class


/* Robot constructor, set inital values */
robot_class::robot_class(void)
{
    // Setup Critical Sections
    InitializeCriticalSection(&control_data_lock);
    InitializeCriticalSection(&telemetry_data_lock);
    InitializeCriticalSection(&update_speed_lock);
    InitializeCriticalSection(&auto_mode_lock);
    
    
    update_speed = 0;
    updates = 0;

    auto_mode = false;

    reset_control();
}
/* End of robot_class constructor */

/* Stops motors and centers servos */
void robot_class::reset_control(void)
{
    control_type c;

    // Setup inital robot conditions

    c.left_motor_speed = 0;
    c.left_motor_target = 0;
    c.left_motor_dir = 0;

    c.right_motor_speed = 0;
    c.right_motor_target = 0;
    c.right_motor_dir = 0;

    c.servo_pos[0] = 0x80;
    c.servo_pos[1] = 0x80;

    set_control(c);
   
}
/* End of reset_control */


/* Robot destructor */
robot_class::~robot_class(void)
{
}
/* End of robot_class */


/* Start all robot sub systems */
void robot_class::start(void)
{
    start_serial();         // Start serial system
}
/* End of robot_class::start */


/* Stop all robot sub systems */
void robot_class::stop(void)
{
    stop_serial();          // Stop serial system
}
/* End of robot_class::stop */


/* Set robot auto mode */
void robot_class::set_auto_mode(bool mode)
{
    EnterCriticalSection(&auto_mode_lock);
    auto_mode = mode;    
    LeaveCriticalSection(&auto_mode_lock);
    
    if (!mode)
        reset_control();
    
}
/* End of set_auto_mode */


/* Set robot auto mode */
bool robot_class::get_auto_mode(void)
{
    bool mode;

    EnterCriticalSection(&auto_mode_lock);
    mode = auto_mode;
    LeaveCriticalSection(&auto_mode_lock);
    
    return(mode);
}
/* End of set_auto_mode */



/* Copy control data into the parameter */
// This consolidates all access to the control data to 
// Make it thread safe
//
void robot_class::get_control(control_type *control_data_copy)
{
    EnterCriticalSection(&control_data_lock);

    // Copy control data to copy
    memcpy(control_data_copy, &control_data, sizeof(control_data));

    LeaveCriticalSection(&control_data_lock);
}
/* End of get_control */


/* Put the control data into a buffer to be sent to the controller board */
void robot_class::get_control(unsigned char * buffer)
{
    EnterCriticalSection(&control_data_lock);

    // Clear buffer 
    memset(buffer, 0, SERIAL_SEND_SIZE);

    unsigned int i = 0;                                     // Index
    
    append_byte(buffer, i, 0xff);                           // Start byte
  
    unsigned char temp = 0;                                 // Temp control byte
    
    // Set direction bits      
    if (control_data.left_motor_dir) temp |= 0x01;          // Bit for left reverse
    if (control_data.right_motor_dir) temp |= 0x02;         // Bit for right reverse
      
    append_byte(buffer, i, temp);                           // Add the control byte
  
    // Speeds
    append_byte(buffer, i, (byte)control_data.left_motor_speed);  // Add left speed
    append_byte(buffer, i, (byte)control_data.right_motor_speed); // Add right speed

    // Servo positions
    append_byte(buffer, i, control_data.servo_pos[0]);      // Add pan
    append_byte(buffer, i, control_data.servo_pos[1]);      // Add tilt
                       
    buffer[SERIAL_SEND_SIZE-1] = 0xff;                      // End byte




   /* FILE *f = fopen("send.txt", "at");
    fprintf(f, "Send to robot %d %d %4f  %4f\n", control_data.left_motor_dir,
                                  control_data.right_motor_dir,
                                  control_data.left_motor_speed,
                                  control_data.right_motor_speed);

    fclose(f);*/





    LeaveCriticalSection(&control_data_lock);
}
/* End of get control */


/*  Copy passed control data to actual data */
void robot_class::set_control(control_type &new_control_data)
{
    EnterCriticalSection(&control_data_lock);

    // Copy new control data to actual control data
    memcpy(&control_data, &new_control_data, sizeof(control_type));

    LeaveCriticalSection(&control_data_lock);
}
/* End of set control */


/* Extract data from buffer into control data */
void robot_class::set_control(unsigned char *buffer)
{
    EnterCriticalSection(&control_data_lock);

    // Decode direction bits
    control_data.left_motor_dir = buffer[1] & 0x01;
    control_data.right_motor_dir = buffer[1] & 0x02;

    // Speeds
    control_data.left_motor_target = buffer[2];
    control_data.right_motor_target = buffer[3];

    if (control_data.left_motor_target == 0)
        control_data.left_motor_speed = 0;

    if (control_data.right_motor_target == 0)
        control_data.right_motor_speed = 0;



    // Servo Pos
    control_data.servo_pos[0] = buffer[4];
    control_data.servo_pos[1] = buffer[5];


   /*  FILE *f = fopen("send.txt", "at");
    fprintf(f, "sent from client %d %d %4f  %4f\n", control_data.left_motor_dir,
                                  control_data.right_motor_dir,
                                  control_data.left_motor_target,
                                  control_data.right_motor_target);

    fclose(f);*/

    
    LeaveCriticalSection(&control_data_lock);
}
/* End of set_control(buffer) */


/* Copy telemetry data into a struct */
void robot_class::get_telemetry(telemetry_type * telemetry_data_copy)   // telemetry into struct
{
    EnterCriticalSection(&telemetry_data_lock);

    // Copy telemetry data
    memcpy(telemetry_data_copy, &telemetry_data, sizeof(telemetry_type));

    LeaveCriticalSection(&telemetry_data_lock);    
}
/* End of get_telemetry */



/* Put telemetry data into buffer */
void robot_class::get_telemetry(unsigned char * buffer)   // tTlemetry into buffer
{
    EnterCriticalSection(&telemetry_data_lock);

    unsigned int i = 0;                                     // Index
    
    append_byte(buffer, i, 0xff);                           // Start byter
     
    // Add low words of analog
    append_byte(buffer, i, telemetry_data.analog_data[0] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[1] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[2] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[3] & 0xff);     

    append_byte(buffer, i, telemetry_data.analog_data[4] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[5] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[6] & 0xff);     
    append_byte(buffer, i, telemetry_data.analog_data[7] & 0xff);     

    unsigned char temp = 0;                                 // Temp control byte
  
    // High words of analog
    temp |= (telemetry_data.analog_data[0] >> 8) & 0x03;
    temp |= ((telemetry_data.analog_data[1] >> 8) & 0x03) << 2;
    temp |= ((telemetry_data.analog_data[2] >> 8) & 0x03) << 4;
    temp |= ((telemetry_data.analog_data[3] >> 8) & 0x03) << 6;
                        
    append_byte(buffer, i, temp);       

    temp = 0;                           
  
    temp |= (telemetry_data.analog_data[4] >> 8) & 0x03;
    temp |= ((telemetry_data.analog_data[5] >> 8) & 0x03) << 2;
    temp |= ((telemetry_data.analog_data[6] >> 8) & 0x03) << 4;
    temp |= ((telemetry_data.analog_data[7] >> 8) & 0x03) << 6;

    append_byte(buffer, i, temp); 
           
    append_byte(buffer, i, telemetry_data.encoder_counts[0] & 0xff);     
    append_byte(buffer, i, telemetry_data.encoder_counts[0] >> 8);     

    append_byte(buffer, i, telemetry_data.encoder_counts[1] & 0xff);     
    append_byte(buffer, i, telemetry_data.encoder_counts[1] >> 8);     

    append_byte(buffer, i, telemetry_data.encoder_counts[2] & 0xff);     
    append_byte(buffer, i, telemetry_data.encoder_counts[2] >> 8);     

    append_byte(buffer, i, telemetry_data.encoder_counts[3] & 0xff);     
    append_byte(buffer, i, telemetry_data.encoder_counts[3] >> 8);     
                        
    buffer[SERIAL_RECV_SIZE-1] = 0xff;                      // End byte