task main()
{
float leftFront, leftBack, rightFront, rightBack; // motors
float y, x, c;
tPSP controller;
/***** BEGIN Mecanum Field Oriented Drive Test *****/
init();
wait10Msec(100);
StartTask(readSensors, 8);
StartTask(displaySmartDiags, 255);
//if (bCompetitionMode) {waitForStart();}
while (true)
{
nxtDisplayCenteredTextLine(3, "%i", gyro_getheading());
/***** Proportional Motor Control *****/
PSPV4readButtons(PSPNXV4, controller);
//scale to -1 to 1
y = (deadband(k_deadband,-controller.joystickLeft_y))/100; //strafe
x = (deadband(k_deadband,controller.joystickLeft_x))/100; //forward/rev
c = (deadband(k_deadband,controller.joystickRight_x))/100; //spin
nxtDisplayTextLine(4, "%i", controller.joypadRight);
nxtDisplayTextLine(4, "%i", controller.joypadLeft);
if ((y == 0) && (x == 0))
{
x += controller.joypadRight; //strafe
x -= controller.joypadLeft; //strafe
y += controller.joypadUp; //forward/rev
y -= controller.joypadDown; //forward/rev
}
nxtDisplayTextLine(5, "%i", y);
mecanum_arcadeFOD(y, x, c, gyro_getheading(),
leftFront, rightFront, leftBack, rightBack);
motor[Lf] = leftFront*100;
motor[Rf] = rightFront*100;
motor[Lb] = leftBack*100;
motor[Rb] = rightBack*100;
if(controller.circleBtn == 1) { gyro_reset(); }
while(nNxtButtonPressed == kEnterButton) { gyro_reset(); }
wait1Msec(5);
}
}
task main ()
{
// This is the struct that holds all the info on all buttons and joypads/sticks
tPSP controller;
int angle;
int speed;
int rotation;
while (true)
{
// Read the state of the buttons
PSPV4readButtons(PSPNXV4, controller);
// Two controls are used:
// The left joystick controls speed and direction
// The right joystick controls rotational speed
// Calculate the angle we need to travel at using simple geometry
angle = radiansToDegrees(atan2(-controller.joystickLeft_x, -controller.joystickLeft_y));
// This is the length of the vector, which is based on the amount we've moved the
// joystick in the X and Y directions
speed = sqrt((controller.joystickLeft_x * controller.joystickLeft_x) +
(controller.joystickLeft_y*controller.joystickLeft_y));
// Rotation speed is controlled by the right joystick
rotation = -controller.joystickRight_x;
nxtDisplayCenteredTextLine(1, "%d:%d", controller.joystickLeft_x, controller.joystickLeft_y);
nxtDisplayCenteredTextLine(3, "%d:%d", controller.joystickRight_x, controller.joystickRight_y);
nxtDisplayCenteredTextLine(5, "%d", angle);
nxtDisplayCenteredTextLine(6, "%d", speed);
nxtDisplayCenteredTextLine(7, "%d", rotation);
MoveRobot(angle, speed, rotation);
wait1Msec(100);
}
PlaySound(soundBeepBeep);
MoveRobot(0, 0, 0);
while (bSoundActive) EndTimeSlice();
wait1Msec(100);
}
