forked from logzhan/RobotHardware-UESTC
269 lines
5.8 KiB
C++
269 lines
5.8 KiB
C++
#include "joystick.h"
|
|
|
|
#include "board.h"
|
|
#include "print.h"
|
|
|
|
#define pressures false
|
|
#define rumble false
|
|
|
|
Joystick::Joystick()
|
|
{
|
|
}
|
|
|
|
bool Joystick::init()
|
|
{
|
|
pb_printf("init joystick");
|
|
//PS2_SetInit();
|
|
#if 0
|
|
unsigned long start = Board::get()->get_tick_count();
|
|
while(Board::get()->get_tick_count()-start>500){
|
|
;
|
|
}
|
|
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
|
|
if (error == 0)
|
|
{
|
|
pb_printf("Teleop start");
|
|
return true;
|
|
}
|
|
|
|
if (error == 1)
|
|
pb_printf("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
|
|
else if (error == 2)
|
|
pb_printf("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
|
|
else if (error == 3)
|
|
pb_printf("Controller refusing to enter Pressures mode, may not support it. ");
|
|
else
|
|
pb_printf("Teleop err");
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
#define JOYSTICK_TEST
|
|
void Joystick::test()
|
|
{
|
|
#ifdef JOYSTICK_TEST
|
|
|
|
unsigned char key=PS2_DataKey();
|
|
|
|
//有按键按下
|
|
if(key!=0) {
|
|
switch (key) {
|
|
case PSB_SELECT:
|
|
pb_printf("SELECT");
|
|
break;
|
|
case PSB_L3:
|
|
pb_printf("L3");
|
|
break;
|
|
case PSB_R3:
|
|
pb_printf("R3");
|
|
break;
|
|
case PSB_START:
|
|
pb_printf("START");
|
|
break;
|
|
case PSB_PAD_UP:
|
|
pb_printf("UP");
|
|
break;
|
|
case PSB_PAD_RIGHT:
|
|
pb_printf("RIGHT");
|
|
break;
|
|
case PSB_PAD_DOWN:
|
|
pb_printf("DOWN");
|
|
break;
|
|
case PSB_PAD_LEFT:
|
|
pb_printf("LEFT");
|
|
break;
|
|
case PSB_L2:
|
|
pb_printf("L2");
|
|
break;
|
|
case PSB_R2:
|
|
pb_printf("R2");
|
|
break;
|
|
case PSB_L1:
|
|
pb_printf("L1");
|
|
break;
|
|
case PSB_R1:
|
|
pb_printf("R1");
|
|
break;
|
|
case PSB_TRIANGLE:
|
|
pb_printf("TRIANGLE");
|
|
break;
|
|
case PSB_CIRCLE:
|
|
pb_printf("CIRCLE");
|
|
break;
|
|
case PSB_CROSS:
|
|
pb_printf("CROSS");
|
|
break;
|
|
case PSB_SQUARE:
|
|
pb_printf("SQUARE");
|
|
break;
|
|
default:
|
|
pb_printf("UNKNOWN");
|
|
break;
|
|
}
|
|
}
|
|
|
|
pb_printf("(need to switch mode) %5d %5d %5d %5d",PS2_AnologData(PSS_LX),PS2_AnologData(PSS_LY), PS2_AnologData(PSS_RX),PS2_AnologData(PSS_RY) );
|
|
#endif
|
|
}
|
|
|
|
bool Joystick::check(short& liner_x, short liner_y, short& angular_z){
|
|
#if JOYSTICK_FOR_HOLONOMIC
|
|
return holonomic_check(liner_x, liner_y, angular_z);
|
|
#else
|
|
return nonholonomic_check(liner_x, liner_y, angular_z);
|
|
#endif
|
|
}
|
|
|
|
bool Joystick::holonomic_check(short& liner_x, short liner_y, short& angular_z){
|
|
bool rtn = false;
|
|
|
|
unsigned char key=PS2_DataKey();
|
|
|
|
liner_y = liner_y;
|
|
//up down left right for liner x, y
|
|
if (key == PSB_PAD_UP) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("UP");
|
|
#endif
|
|
liner_x = MAX_LINER_X;
|
|
rtn = true;
|
|
}
|
|
|
|
if(key == PSB_PAD_DOWN) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("DOWN");
|
|
#endif
|
|
liner_x = -MAX_LINER_X;
|
|
rtn = true;
|
|
}
|
|
|
|
if(key == PSB_PAD_RIGHT) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("RIGHT");
|
|
#endif
|
|
liner_y = MAX_LINER_Y;
|
|
rtn = true;
|
|
}
|
|
|
|
if(key == PSB_PAD_LEFT) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("LEFT");
|
|
#endif
|
|
liner_y = -MAX_LINER_Y;
|
|
rtn = true;
|
|
}
|
|
|
|
//triangle square circle cross for angular z
|
|
if (key == PSB_SQUARE) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("SQUARE");
|
|
#endif
|
|
angular_z = MAX_ANGULAR_Z;
|
|
rtn = true;
|
|
}
|
|
|
|
if(key == PSB_CIRCLE) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("CIRCLE");
|
|
#endif
|
|
angular_z = -MAX_ANGULAR_Z;
|
|
rtn = true;
|
|
}
|
|
|
|
if (key == PSB_L1) { //print stick values if either is TRUE
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("Stick Values:");
|
|
pb_printf("%d", PS2_AnologData(PSS_LY));
|
|
pb_printf(",");
|
|
pb_printf("%d", PS2_AnologData(PSS_LX));
|
|
pb_printf(",");
|
|
pb_printf("%d", PS2_AnologData(PSS_RY));
|
|
pb_printf(",");
|
|
pb_printf("%d", PS2_AnologData(PSS_RX));
|
|
#endif
|
|
|
|
if (PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255 &&
|
|
PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("switch mode for use rocker");
|
|
#endif
|
|
} else {
|
|
liner_x = ((255.0/2)-PS2_AnologData(PSS_LY))/(255.0/2)*MAX_LINER_X;
|
|
liner_y = ((255.0/2)-PS2_AnologData(PSS_LX))/(255.0/2)*MAX_LINER_Y;
|
|
angular_z = ((255.0/2)-PS2_AnologData(PSS_RX))/(255.0/2)*MAX_ANGULAR_Z;
|
|
rtn = true;
|
|
}
|
|
}
|
|
|
|
return rtn;
|
|
}
|
|
|
|
bool Joystick::nonholonomic_check(short& liner_x, short liner_y, short& angular_z){
|
|
|
|
liner_y = 0;
|
|
|
|
bool rtn = false;
|
|
|
|
unsigned char key=PS2_DataKey();
|
|
|
|
liner_y = liner_y;
|
|
//will be TRUE if button was JUST pressed
|
|
if (key == PSB_PAD_UP) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("Up held this hard: ");
|
|
#endif
|
|
liner_x = MAX_LINER_X;
|
|
rtn = true;
|
|
}
|
|
|
|
if(key == PSB_PAD_RIGHT) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("Right held this hard: ");
|
|
#endif
|
|
angular_z = MAX_ANGULAR_Z;
|
|
rtn = true;
|
|
}
|
|
|
|
if(key == PSB_PAD_LEFT) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("LEFT held this hard: ");
|
|
#endif
|
|
angular_z = -MAX_ANGULAR_Z;
|
|
rtn = true;
|
|
}
|
|
|
|
if(key == PSB_PAD_DOWN) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("DOWN held this hard: ");
|
|
#endif
|
|
liner_x = -MAX_LINER_X;
|
|
rtn = true;
|
|
}
|
|
|
|
if (key == PSB_L1) { //print stick values if either is TRUE
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("Stick Values:");
|
|
pb_printf("%d", PS2_AnologData(PSS_LY));
|
|
pb_printf(",");
|
|
pb_printf("%d", PS2_AnologData(PSS_LX));
|
|
pb_printf(",");
|
|
pb_printf("%d", PS2_AnologData(PSS_RY));
|
|
pb_printf(",");
|
|
pb_printf("%d", PS2_AnologData(PSS_RX));
|
|
#endif
|
|
|
|
if (PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255 &&
|
|
PS2_AnologData(PSS_LX) == 255 && PS2_AnologData(PSS_LX) == 255) {
|
|
#if JOYSTICK_DEBUG_ENABLE
|
|
pb_printf("switch mode for use rocker");
|
|
#endif
|
|
} else {
|
|
liner_x = ((255.0/2)-PS2_AnologData(PSS_LY))/(255.0/2)*MAX_LINER_X;
|
|
angular_z = ((255.0/2)-PS2_AnologData(PSS_LX))/(255.0/2)*MAX_ANGULAR_Z;
|
|
rtn = true;
|
|
}
|
|
}
|
|
|
|
return rtn;
|
|
}
|