void TestPSPAnalog () {
  psp currState;
	PSP_ReadButtonState(SensorPort, Addr, currState);
	if ((int)currState.l_j_y == 99 && (int)currState.r_j_y == 99) {
	  	PlaySound(soundDownwardTones);
      wait1Msec(500);
      StopAllTasks();
    }
  }
task
main ()
{
  int powerLeft, powerRight;
  int d_left_X, d_left_Y;
  int d_right_X, d_right_Y;
  int hopper_up, hopper_down, hopper_score;
  int fingers_up, fingers_down, fingers_up_encoder, fingers_down_encoder;
  int trigger_pressed;
  int ok_to_drive = false;
  int joystick_click;

	psp currState;

  nNxtExitClicks = 3;                // Triple clicking EXIT button will terminate program
  nI2CBytesReady[SensorPort] = 0;
  SensorType[SensorPort] = sensorI2CMuxController;
  wait10Msec (100);

  while (true) {
  PSP_ReadButtonState(SensorPort, Addr, currState);

      // joysticks
      d_left_X = (int)currState.l_j_x;
      d_left_Y = (int)currState.l_j_y;
      d_right_X = (int)currState.r_j_x;
      d_right_Y = (int)currState.r_j_y;

      // left button pad
      hopper_down = !(bool)currState.d;
      hopper_up = !(bool)currState.c || !(bool)currState.a;
      hopper_score = !(bool)currState.b;

      // right button pad
      fingers_up_encoder = !(bool)currState.triang;
      fingers_down_encoder = !(bool)currState.cross;
      fingers_up = !(bool)currState.square;
      fingers_down = !(bool)currState.circle;

      // any of the trigger buttons
      trigger_pressed = (!(bool)currState.l1) || (!(bool)currState.l2)
                     || (!(bool)currState.r1) || (!(bool)currState.r2);

      joystick_click = !(bool)currState.l_j_b || !(bool)currState.r_j_b;

	  	nxtDisplayTextLine(1,"Left: %d", d_left_Y);
  		nxtDisplayTextLine(2,"Right: %d", d_right_Y);
  		nxtDisplayTextLine(3,"Hopper: %d", hopper_score);
  		nxtDisplayTextLine(4,"Fingers: %d", fingers_down);
  		nxtDisplayTextLine(5,"Triggers: %d", trigger_pressed);
  		nxtDisplayTextLine(5,"Joy Click: %d", joystick_click);

}
  wait10Msec (100);

  StopAllTasks ();
}
Ejemplo n.º 3
0
task main()
{
psp currState;
nI2CBytesReady[PSPNX] = 0;
PlaySound(soundUpwardTones);

	while(true)
	{
		/*user code below*/
		PSP_ReadButtonState(PSPNX, Addr, currState);
		bool joystick_joy1_btn_1			= !(char)currState.square;//
		bool joystick_joy1_btn_2 			= !(char)currState.cross;	//
		bool joystick_joy1_btn_3 			= !(char)currState.circle;//
		bool joystick_joy1_btn_4 			= !(char)currState.triang;//
		bool joystick_joy1_Dpad_Up 		= !(char)currState.d;			//unused
		bool joystick_joy1_Dpad_Right = !(char)currState.c;			//unused
		bool joystick_joy1_Dpad_Down 	= !(char)currState.b;			//unused
		bool joystick_joy1_Dpad_Left 	= !(char)currState.a;			//unused
		bool joystick_joy1_L_Bumper		= !(char)currState.l1;		//
		bool joystick_joy1_R_Bumper		= !(char)currState.r1;		//
		bool joystick_joy1_L_Trigger	= !(char)currState.l2;		//
		bool joystick_joy1_R_Trigger	= !(char)currState.r2;		//
		bool joystick_joy1_btn_13			=	!(char)currState.l_j_b;	//unused
		bool joystick_joy1_btn_14			= !(char)currState.r_j_b;	//unused`
		int joystick_joy1_x1 					= (int)currState.l_j_x;		//unused
		int joystick_joy1_y1 					= (int)currState.l_j_y;		//
		int joystick_joy1_x2					= (int)currState.r_j_x;		//unused
		int joystick_joy1_y2					= (int)currState.r_j_y;		//

		//first joypad
		//right drive

		const static int threshold =12; // Int 'threshold' will allow us to ignore low  readings that keep our robot in perpetual motion.

		if(abs(joystick_joy1_y1) > threshold)   // If the right analog stick's Y-axis readings are either above or below the threshold:
		{
		motor[RightMotor] = joystick_joy1_y1;         // Motor D is assigned a power level equal to the right analog stick's Y-axis reading.
		writeDebugStreamLine("joystick_joy1_y1: %i",joystick_joy1_y1);
		}else                                    // Else if the readings are within the threshold:
		{
			motor[RightMotor] = 0;                        // Motor D is stopped with a power level of 0.
		}
		//left drive
		if(abs(joystick_joy1_y2) > threshold)   // If the left analog stick's Y-axis readings are either above or below the threshold:
		{
		motor[LeftMotor] = joystick_joy1_y2;         // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
		writeDebugStreamLine("joystick_joy1_y2: %i",joystick_joy1_y2);
		}
		else                                    // Else if the readings are within the threshold:
		{
			motor[LeftMotor] = 0;                        // Motor E is stopped with a power level of 0.
		}
	}
}
task
main ()
{
  int powerLeft = 0;
  int powerRight = 0;
  int d_left_X, d_left_Y;
  int d_right_X, d_right_Y;
  bool arm_up, arm_down, arm_left, arm_right;
  int left_joystick_click, right_joystick_click;
  bool claw_close_1, claw_close_2;
  bool hopper_forward, hopper_backward;
  int claw_position, hopper_position;
  int tank_drive_ok = false;
  int steer_drive_ok = false;
  int turbo_speed = false;
  psp currState;

  // Note: program cannot be terminated if we hijack the 'exit' button. So there has to be an escape sequence
  //       that will return buttons to system control! We'll use a triple click
  //
  nNxtExitClicks = 3;                // Triple clicking EXIT button will terminate program
  nI2CBytesReady[SensorPort] = 0;
  SensorType[SensorPort] = sensorI2CMuxController;
  wait10Msec (100);
  TestPSPAnalog();

  while ( true )
    {
      wait1Msec (100);

      PSP_ReadButtonState(SensorPort, Addr, currState);

      // joysticks
      d_left_X = (int)currState.l_j_x;
      d_left_Y = (int)currState.l_j_y;
      d_right_X = (int)currState.r_j_x;
      d_right_Y = (int)currState.r_j_y;

      // trigger buttons control claw, if connected
      claw_close_1 = (!(bool)currState.l1) || (!(bool)currState.l2);
      claw_close_2 = (!(bool)currState.r1) || (!(bool)currState.r2);

      // trigger buttons control ball hopper, if connected
      hopper_forward = (!(bool)currState.r1);
      hopper_backward = (!(bool)currState.r2);

      // for controlling tank or steer
      left_joystick_click = !(bool)currState.l_j_b ;
      right_joystick_click = !(bool)currState.r_j_b;

      // by default, do not enable driving, to avoid driving off a table
      if (left_joystick_click) { tank_drive_ok = true; steer_drive_ok = false; }
      if (right_joystick_click) { tank_drive_ok = false; steer_drive_ok = true; }

      // drive slowly unless turbo
      turbo_speed = false ; // trigger_pressed;

      if (!tank_drive_ok) { // if not tank drive, then left joystick controls arm and turntable
	      // left y controls arm
	      arm_up = (d_left_Y > DeadZone);
	      arm_down = (d_left_Y < -DeadZone);
	      // left x controls turntable
	      arm_left = (d_left_X > DeadZone);
	      arm_right = (d_left_X < -DeadZone);
	    }

      // tank drive
      if (tank_drive_ok) {
	      powerLeft = abs(d_left_Y) > DeadZone ? abs(d_left_Y)*d_left_Y/100 : 0;
	      powerRight = abs(d_right_Y) > DeadZone ? abs(d_right_Y)*d_right_Y/100 : 0;
	      if (!turbo_speed) {	powerLeft /= 2; powerRight /= 2; }
	  	  nxtDisplayTextLine(1,"Left: %d", powerLeft);
  		  nxtDisplayTextLine(2,"Right: %d", powerRight);

  		// steering
  	  } else if (steer_drive_ok) {
  	    // distribute power proportionally based on x value
  	    int differential = abs(d_right_X) > DeadZone ? d_right_X : 0;
	      int totalPower = abs(d_right_Y) > DeadZone ? abs(d_right_Y)*d_right_Y/100 : 0;
	      if (!turbo_speed) {	totalPower /= 2; }
	      float proportion = abs(differential) / 100.0;  // extreme steer -> 1
	      powerLeft = differential > 0 ? totalPower * proportion : totalPower * (1.0 - proportion);
	      powerRight = differential < 0 ? totalPower * proportion : totalPower * (1.0 - proportion);
	  	  nxtDisplayTextLine(1,"Left: %d", powerLeft);
  		  nxtDisplayTextLine(2,"Right: %d", powerRight);

  		  // driving not enabled yet
	    } else {
		    nxtDisplayTextLine(1,"Click a joystick");
		    nxtDisplayTextLine(2,"to drive");
		  }

		  // motor[left] = powerLeft;
		  // motor[right] = powerRight;

		  // arm
		  if (arm_up) {
		  	servo[arm] = SERVO_FORWARD;
		  	nxtDisplayTextLine(3,"Arm up");
			} else if (arm_down) {
		  	servo[arm] = SERVO_BACKWARD;
		  	nxtDisplayTextLine(3,"Arm down");
		  } else {
		  	servo[arm] = SERVO_STOP;
		  	nxtDisplayTextLine(3,"Arm stop");
		  }

		  // turntable
		  if (arm_left) {
		  	servo[turntable] = SERVO_FORWARD;
		  	nxtDisplayTextLine(4,"Arm left");
			} else if (arm_right) {
		  	servo[turntable] = SERVO_BACKWARD;
		  	nxtDisplayTextLine(4,"Arm right");
		  } else {
		  	servo[turntable] = SERVO_STOP;
		  	nxtDisplayTextLine(4,"Arm not turning");
		  }

		  // claw, if connected
		  if (claw_close_1) {
		  	claw_position = CLAW_CLOSED_1;
			} else if (claw_close_2) {
		  	claw_position = CLAW_CLOSED_2;
		  } else {
		  	claw_position = CLAW_OPEN;
		  }
		  servo[claw] = claw_position;
		  nxtDisplayTextLine(4,"Claw position %d", claw_position);

		  // debris ball hopper, if connected
		  if (hopper_forward) {
		  	hopper_position = SERVO_FORWARD;
		  } else if (hopper_backward) {
		  	hopper_position = SERVO_BACKWARD;
		  } else {
		  	hopper_position = SERVO_STOP;
		  }
		  servo[debris] = hopper_position;
		  nxtDisplayTextLine(4,"Hopper position %d", hopper_position);

		  // drive
		  motor[left] = powerLeft;
		  motor[right] = powerRight;

    }

  StopAllTasks ();
}