input.cpp

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

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


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

#include "input.h"
#include "robot.h"
#include "time.h"
#include "display.h"
#include "joystick.h"
#include "utility.h"

bool keys[256] = {false};               // Keyboard table


/* Reset robot values when focus is lost */
void input_lose_focus( void )
{
    robot.reset_control();
}
/* End of input lose focus */


/* Process currently pressed keys */
void input_check(void)
{
    // Storte last time value
    static double last_time = get_time();

    // Calculate time difference between calls       
    double cur_time = get_time();    
    double time_diff = cur_time - last_time;
    
    last_time = cur_time;
    
    double rate = 2;
       
    control_type c;
    
    robot.get_control(&c);

    telemetry_type t;
    
robot.get_telemetry(&t);

    double left_motor_error = (c.left_motor_target  - c.left_motor_speed);

    double right_motor_error = (c.right_motor_target  - c.right_motor_speed);


//    double left_motor_error = (c.left_motor_target  - t.encoders[0]);

//    double right_motor_error = (c.right_motor_target  - t.encoders[0]); 
    
    // Calculate speed corrections       
    double left_correction =  left_motor_error * rate * time_diff;
    double right_correction = right_motor_error * rate * time_diff;
        
    c.left_motor_speed  += left_correction;
    c.right_motor_speed += right_correction;
        
    if (c.left_motor_speed > c.left_motor_target)
        c.left_motor_speed  = c.left_motor_target;

    if (c.right_motor_speed > c.right_motor_target)
        c.right_motor_speed  = c.right_motor_target;



 /* char b[300];
    
    sprintf(b, "l:  %f  r:  %f     ld: %d  rd:  %d", 
    
      c.left_motor_speed,
    c.right_motor_speed,

    c.left_motor_dir, 
    c.right_motor_dir);  
    
 debug(b);
 */









    robot.set_control(c);
}
/* End of check keys */


/* Update robot movement based on current states */
void input_update(void)
{
    if (remote_mode) return;
    if (robot.get_auto_mode()) return;

/*
static double s = 0;

    if (keys['1']) s += 0.01;
    if (keys['2']) s -= 0.01;
    
  robot.set_motion(0, s);
  return;*/

    if (keys['W'] || joystick.up)
    {
        if (keys['A']|| joystick.left)  robot.set_motion(-0.7, 0.3);  else
        if (keys['D']|| joystick.right) robot.set_motion(0.7, 0.3);  else
            robot.set_motion(0, 0.1);  
    } else
    if (keys['S'] || joystick.down)
    {
        if (keys['A']|| joystick.left)  robot.set_motion(0.7, -0.3); else 
        if (keys['D']|| joystick.right) robot.set_motion(-0.7, -0.3); else
            robot.set_motion(0, -0.1); 
    } else    
    if (keys['A'] || joystick.left)  robot.set_motion(-0.5, 0); else 
    if (keys['D'] || joystick.right) robot.set_motion( 0.5, 0); else
    {
        robot.set_motion(0, 0);
    } 
    
    input_check();
}
/* End of update input */


/* Process key down message */
void input_key_down(WPARAM w, LPARAM l)
{
    if (l & REPEAT_CODE) return;  

    keys[w & 0xff] = true;          // Set key
    
    // Toggle auto mode
    if (keys['M'])
        robot.set_auto_mode(!robot.get_auto_mode());    
    
        if (keys['R']) reset_vals();
    
    input_update();
}
/* End of key_down */


/* Process keyup message */
void input_key_up(WPARAM w)
{
    keys[w & 0xff] = false;         // Free key
    
    input_update();
}
/* End of keyup */