RobotHardware-UESTC/Hardware/银星机器人底盘/PiRobot-YH_Firmware v1.0/STM32/Lib/Joystick/joystick.cpp

#include "joystick.h"

#include "board.h"
#include "print.h"

#define pressures   false
#define rumble      false

Joystick::Joystick()
{
}

bool Joystick::init()
{
	log("init joystick");
	//PS2_SetInit();
#if 0
	unsigned long start = Board::get()->getTickCount();
	while(Board::get()->getTickCount()-start>500){
		;
	}
    error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
	if (error == 0)
	{
		log("Teleop start");
        return true;
	}

	if (error == 1)
		log("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
	else if (error == 2)
		log("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
	else if (error == 3)
		log("Controller refusing to enter Pressures mode, may not support it. ");
	else
		log("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:
				log("SELECT");
				break;
			case PSB_L3:
				log("L3");
				break;
			case PSB_R3:
				log("R3");
				break;
			case PSB_START:
				log("START");
				break;
			case PSB_PAD_UP:
				log("UP");
				break;
			case PSB_PAD_RIGHT:
				log("RIGHT");
				break;
			case PSB_PAD_DOWN:
				log("DOWN");
				break;
			case PSB_PAD_LEFT:
				log("LEFT");
				break;
			case PSB_L2:
				log("L2");
				break;
			case PSB_R2:
				log("R2");
				break;
			case PSB_L1:
				log("L1");
				break;
			case PSB_R1:
				log("R1");
				break;
			case PSB_TRIANGLE:
				log("TRIANGLE");
				break;
			case PSB_CIRCLE:
				log("CIRCLE");
				break;
			case PSB_CROSS:
				log("CROSS");
				break;
			case PSB_SQUARE:
				log("SQUARE");
				break;
			default:
				log("UNKNOWN");
				break;
		}
	}

	log("(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
      	log("UP");
	#endif
		liner_x = MAX_LINER_X;
		rtn = true;
	}

    if(key == PSB_PAD_DOWN) {
	#if JOYSTICK_DEBUG_ENABLE
      	log("DOWN");
	#endif
		liner_x = -MAX_LINER_X;
		rtn = true;
    }

    if(key == PSB_PAD_RIGHT) {
	#if JOYSTICK_DEBUG_ENABLE
		log("RIGHT");
	#endif
		liner_y = MAX_LINER_Y;
		rtn = true;
    }

    if(key == PSB_PAD_LEFT) {
	#if JOYSTICK_DEBUG_ENABLE
      	log("LEFT");
	#endif
		liner_y = -MAX_LINER_Y;
		rtn = true;
    }
	
	//triangle square circle cross  for angular z
	if (key == PSB_SQUARE) {
	#if JOYSTICK_DEBUG_ENABLE
      	log("SQUARE");
	#endif
		angular_z = MAX_ANGULAR_Z;
		rtn = true;
	}

    if(key == PSB_CIRCLE) {
	#if JOYSTICK_DEBUG_ENABLE
      	log("CIRCLE");
	#endif
		angular_z = -MAX_ANGULAR_Z;
		rtn = true;
    }

	if (key == PSB_L1) { //print stick values if either is TRUE
	#if JOYSTICK_DEBUG_ENABLE
		log("Stick Values:");
		log("%d", PS2_AnologData(PSS_LY));
		log(",");
		log("%d", PS2_AnologData(PSS_LX));
		log(",");
		log("%d", PS2_AnologData(PSS_RY));
		log(",");
		log("%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
		log("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
      	log("Up held this hard: ");
	#endif
		liner_x = MAX_LINER_X;
		rtn = true;
	}

    if(key == PSB_PAD_RIGHT) {
	#if JOYSTICK_DEBUG_ENABLE
		log("Right held this hard: ");
	#endif
		angular_z = MAX_ANGULAR_Z;
		rtn = true;
    }

    if(key == PSB_PAD_LEFT) {
	#if JOYSTICK_DEBUG_ENABLE
      	log("LEFT held this hard: ");
	#endif
		angular_z = -MAX_ANGULAR_Z;
		rtn = true;
    }

    if(key == PSB_PAD_DOWN) {
	#if JOYSTICK_DEBUG_ENABLE
      	log("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
		log("Stick Values:");
		log("%d", PS2_AnologData(PSS_LY));
		log(",");
		log("%d", PS2_AnologData(PSS_LX));
		log(",");
		log("%d", PS2_AnologData(PSS_RY));
		log(",");
		log("%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
		log("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;
}