task main()
{
while(true)
{
Motors_SetSpeed(S1, 1, 1, -100);
Motors_SetSpeed(S1, 1, 2, 100);
}
}
void Move(int RightPower, int LeftPower)
{
Motors_SetSpeed(S1, 2, 1, RightPower);
Motors_SetSpeed(S1, 2, 2, -LeftPower);
}
task main()
{
initializeRobot();
waitForStart();
int curGyro;
int oldGyro;
oldGyro = getGyroData(S2);
movData data;
//int offset = getOffset();
bool fieldoriented = true;
//bNxtLCDStatusDisplay = false;
while( true )
{
getJoystickSettings(joystick);
if (joystick. joy2_TopHat == 0 || joystick. joy2_TopHat == 1 || joystick. joy2_TopHat == 7) { //lift up
Motors_SetSpeed(S1, 1, 1, -100);
Motors_SetSpeed(S1, 1, 2, 100);
} else if (joystick. joy2_TopHat == 3 || joystick. joy2_TopHat == 4 || joystick. joy2_TopHat == 5){ //lift down
Motors_SetSpeed(S1, 1, 1, 60);
Motors_SetSpeed(S1, 1, 2, -60);
}else {
Motors_SetSpeed(S1, 1, 1, 0);
Motors_SetSpeed(S1, 1, 2, 0);
}
if (joy2Btn(4) == 1) //ball intake
Motors_SetSpeed(S1, 4, 1, 60);
else if (joy2Btn(2) == 1)
Motors_SetSpeed(S1, 4, 1, -60);
else
Motors_SetSpeed(S1, 4, 1, 0);
if (joy2Btn(3) == 1) //kickstand knocker-downer
servo[stand] = 240;
if (joy2Btn(1) == 1)
servo[stand] = 150;
if (joy2Btn(5) == 1) //dropper
servo[ball] = 18;
if (joy2Btn(6) == 1)
servo[ball] = 160;
//if (joy2Btn(8) == 1)
// fieldoriented = false;
// Drive Base
data.xComp = joystick.joy1_x1;
data.yComp = joystick.joy1_y1-1;
data.rot = joystick.joy1_x2;
//data.xComp = 127;
//data.yComp = 0;
//data.rot = 38;
data.xComp = (data.xComp != -128 ? data.xComp : data.xComp+1);
data.yComp = (data.yComp != -128 ? data.yComp : data.yComp+1);
data.rot = (data.rot != -128 ? data.rot : data.rot+1);
//curGyro = getGyroData(S2);
//this block of if statements is the controller dead-zone
if (data.rot < 10 && data.rot > -10)
data.rot = 0;
if (data.xComp < 10 && data.xComp > -10)
data.xComp = 0;
if (data.yComp < 10 && data.yComp > -10)
data.yComp = 0;
nxtDisplayClearTextLine(2);
nxtDisplayClearTextLine(4);
nxtDisplayBigTextLine(2, "%d", curGyro);
wait1Msec(1);
nxtDisplayBigTextLine(4, "%d", oldGyro);
//if (fieldoriented)
// oldGyro = useGyro(data, oldGyro, curGyro, offset);
if (data.rot < 2 && data.rot > -2)
data.rot = 0;
if (data.xComp < 2 && data.xComp > -2)
data.xComp = 0;
if (data.yComp < 2 && data.yComp > -2)
data.yComp = 0;
//byte speed = getSqrt(data.xComp, data.yComp) + abs(data.rot);
int speed = (int)(sqrt(data.xComp*data.xComp + data.yComp*data.yComp) + abs(data.rot)); //finds speed (dist formula)
if (speed > 127) speed = 127; //Regulates speed
drive(data, (byte)speed);
}
drive(data, 0);
}
void Move(int SpeedLeft, int SpeedRight)
{
Motors_SetSpeed(S1, 1, 1, SpeedRight);
Motors_SetSpeed(S1, 1, 2, -SpeedLeft);
}
