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 ();
}
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 ();
}
