void BackwardOnLine(int power, int distance, string stopEvent)
{
if (abs(SensorValue[Gyroscope]) > 200)
SensorValue[Gyroscope] = 0;
SensorValue[ShaftLeft] = 0;
ClearTimer(T3);
int currentShaft = 0;
int velocity = 0;
while(abs(SensorValue[ShaftLeft]) < distance * 10)
{
float amountLeft = -fAmountRight() - 0.1 * fAmountLeft() - AmountTurn();
float amountRight = fAmountLeft() - 0.1 * fAmountRight() - AmountTurn();
amountLeft = amountLeft * power / 45;
amountRight = amountRight * power / 45;
motor[FrontLeft] = -power + amountLeft;
motor[FrontRight] = power + amountRight;
motor[BackLeft] = -power + amountLeft;
motor[BackRight] = power + amountRight;
if (time10(T4) > 10)
{
velocity = abs(SensorValue[ShaftLeft] - currentShaft);
currentShaft = SensorValue[ShaftLeft];
ClearTimer(T4);
}
if (velocity > 5) ClearTimer(T3);
if (time10(T3) > 50) break;
if (stopEventFunction(stopEvent)) break;
}
}
void Backward(int power, int distance, string stopEvent)
{
if (abs(SensorValue[Gyroscope]) > 200)
SensorValue[Gyroscope] = 0;
SensorValue[ShaftLeft] = 0;
ClearTimer(T3);
int currentShaft = 0;
int velocity = 0;
while(abs(SensorValue[ShaftLeft]) < distance * 10)
{
motor[FrontLeft] = -power - turnSens2 * AmountTurn();
motor[FrontRight] = power - turnSens2 * AmountTurn();
motor[BackLeft] = -power - turnSens2 * AmountTurn();
motor[BackRight] = power - turnSens2 * AmountTurn();
if (time10(T4) > 10)
{
velocity = abs(SensorValue[ShaftLeft] - currentShaft);
currentShaft = SensorValue[ShaftLeft];
ClearTimer(T4);
}
if (velocity > 5) ClearTimer(T3);
if (time10(T3) > 50) break;
if (stopEventFunction(stopEvent)) break;
}
}
task main()
{
nMotorEncoder[test] = 0;
while(true)
{
//1230
getJoystickSettings(joystick);
nxtDisplayCenteredTextLine(2, "enc: %d",nMotorEncoder[test]);
nxtDisplayCenteredTextLine(3, "AngVel: %f", LauncherAngularVelocity);
nxtDisplayCenteredTextLine(1, "time: %f", time10[T2]);
if(time1[T1]>200){
UpdateEncoders();
}
if(joy1Btn(2)&& time10(T2)>100 && LauncherAngularVelocity > 1){
motor[test] = 127;
motor[test2] = -127;
}else if(joy1Btn(1)&& time10(T2)>100 && LauncherAngularVelocity > 1){
motor[test] = -127;
motor[test2] = 127;
}else{
if(joystick.joy1_y2<20 && joystick.joy1_y2>-20){
motor[test] = 0;
ClearTimer(T2);
}else{
motor[test] = joystick.joy1_y2;
}
if(joystick.joy1_y2<20 && joystick.joy1_y2>-20){
motor[test2] = 0;
ClearTimer(T2);
}else{
motor[test2] = joystick.joy1_y2;
}
}
if(joy1Btn(3)){
motor[test] = 127;
motor[test2] = -127;
}else if(joy1Btn(4)){
motor[test] = -127;
motor[test2] = 127;
}
}
}
void turnPointLeft (float mRot, float mRotPerSec)
{
nxtDisplayCenteredTextLine(5, "TPL(%.2f,%.2f)",
mRot, mRotPerSec);
if (moveModeType != MMAllMoveTypes
&& moveModeType != MMTurnsOnly
&& moveModeType != MMPointTurnsOnly) {
return;
}
if (moveModeTiming == MMOneMoveAtATime) {
waitForTouch();
}
if (stepThroughMode == stepThroughModeOn) {
waitForTouch();
}
checkParameterRange(mRot, mRotPerSec);
int leftWheelInitial = nMotorEncoder[leftWheelMotor];
int rightWheelInitial = nMotorEncoder[rightWheelMotor];
int leftWheelTarget = nMotorEncoder[leftWheelMotor] - 360 * mRot;
int rightWheelTarget = nMotorEncoder[rightWheelMotor] + 360 * mRot;
ClearTimer(T1);
float motorPower = revolutionsPerSecondToMotorPower(mRotPerSec);
motor[leftWheelMotor] = -1 * motorPower;
motor[rightWheelMotor] = motorPower;
while ((nMotorEncoder[leftWheelMotor] > leftWheelTarget)
&& (nMotorEncoder[rightWheelMotor] < rightWheelTarget))
{
nxtDisplayCenteredTextLine(7, "%d", nMotorEncoder[leftWheelMotor]);
}
motor[leftWheelMotor] = 0;
motor[rightWheelMotor] = 0;
int leftWheelChange = nMotorEncoder[leftWheelMotor] - leftWheelInitial;
int rightWheelChange = nMotorEncoder[rightWheelMotor] - rightWheelInitial;
float revolutionsWheelsRotated =
((float) ( abs(leftWheelChange) > abs(rightWheelChange) ?
leftWheelChange : rightWheelChange ))
/ 360.0;
nxtDisplayCenteredTextLine(2, "TPL(%.2f,%.2f)",
mRot, mRotPerSec);
nxtDisplayCenteredTextLine(3, "%.2frev %.2fsec",
(float) revolutionsWheelsRotated, (float) time10(T1) / 100);
nxtDisplayCenteredTextLine(5, "");
nxtDisplayCenteredTextLine(7, "");
}
void VariableMove(int leftpower, int rightpower, int duration)
{
ClearTimer(T1);
while(time10(T1) < duration)
{
motor[FrontLeft] = leftpower;
motor[FrontRight] = -rightpower;
motor[BackLeft] = leftpower;
motor[BackRight] = -rightpower;
if (stopEventFunction("none"))
break;
}
}
task main() {
bool conveyoron=false;
initializeRobot();
waitForStart(); // wait for start of tele-op phase
StartTask(DriveUpdate,0);
servo[SERVOGATE] = SERVOGATECLOSE; B_gateOpen=false;
ClearTimer(T1); //we use timer T1 for program time
while(true){
getJoyValues();
if(B_gateOpen) servo[SERVOGATE]=SERVOGATEOPEN;
//////////////////////////////////
//arm control button processing
if(driveButton10) {
drivingreversed=!drivingreversed;
wait1Msec(200);
}
if(driveButton6){
if(servo[SERVOGATE]==SERVOGATECLOSE){
servo[SERVOGATE] = SERVOGATEOPEN;
B_gateOpen=true;
wait1Msec(200);
}
else {
servo[SERVOGATE] = SERVOGATECLOSE;
B_gateOpen=false;
wait1Msec(300);
}
}
if(driveButton7 || armButton7){
if(conveyoron){
motor[conveyor1]=0;
wait1Msec(300);
conveyoron=false;
}
else {
motor(conveyor1) = 100;
wait1Msec(300);
conveyoron=true;
}
}
if(driveButton5 || armButton5){
motor[conveyor1]=-100;
wait1Msec(300);
motor[conveyor1]=0;
conveyoron=false;
}
/*if(armButton1){
servo[servoFlip]=SERVOARMDOWN;
wait1Msec(500);
driveNudge(-50, -50, 500);
servo[servoFlip]=SERVOARMUP;
}
if(armButton4){
servo[servoFlip]=SERVOARMUP;
wait1Msec(50);
}
if(armButton2){
servo[servoFlip]=SERVOARMDOWN;
wait1Msec(50);
}
if(armTopHat>=0){
if(armTopHat==0){
servo[hopControl]=HOPCONTROLUP;
wait1Msec(50);
}
if(armTopHat==4){
servo[hopControl]=HOPCONTROLDOWN;
wait1Msec(50);
}
if(armTopHat==2){
for(int i=0; i<=2; i++){
servo[hopControl]=HOPCONTROLP1;
wait1Msec(300);
servo[hopControl]=HOPCONTROLP2;
wait1Msec(300);
}
servo[hopControl]=HOPCONTROLDOWN;
wait1Msec(50);
}
if(armTopHat==6){
servo[hopControl]=HOPCONTROLCLOSED;
wait1Msec(50);
}
}*/
//////////////////////////
//DRIVING
//////////////////////////
//Nudging, at most 1 every 1/10 second
if(time10(T4)<0) ClearTimer(T4); //keep timer positive
if(time10(T4)>10){
if(driveTopHat>=0){
if(driveTopHat==0) driveNudge( 50, 50,60); //forward
if(driveTopHat==1) driveNudge( 0, 50,60);
if(driveTopHat==2) driveNudge( -50,50,60); //right
if(driveTopHat==3) driveNudge( -50, 0,60);
if(driveTopHat==4) driveNudge(-50,-50,60); //back
if(driveTopHat==5) driveNudge( 0, -50,60);
if(driveTopHat==6) driveNudge( 50,-50,60); //left
if(driveTopHat==7) driveNudge( 50, 0,60);
ClearTimer(T4); //so delay at least 100 Msec before next nudge
}
}
/*if(abs(armjy1)<18) armjy1=0;
motor[flipper]=((100*armjy1)/700);*/
if(abs(drivejy1)<10) drivejy1=0;
if(abs(drivejy2)<10) drivejy2=0;
//standard driving
if(drivingreversed) standardDrive(-(100*drivejy2)/127,-(100*drivejy1)/127);
else standardDrive((100*drivejy1)/127,(100*drivejy2)/127);
}
////////// end of infinite loop
}
/**
* Arc turn has the center point to the left or right of the wheel in line
* with the axel. The idea is that the Rotations Per Second difference is
* the ratio between the outside and inside circle circumference.
*
* The only reason we really ask for both wheel numbers is to facilitate
* the teaching of algebra concepts. A more useful function would just
* use the robot dimensions and the number of degrees to turn.
*
* There isn't a left and right function because the only difference is which
* wheel has to travel the furthest distance.
*/
void turnArcBackwards (float mRotLeft, float mRotLeftPerSec, float mRotRight, float mRotRightPerSec) {
nxtDisplayCenteredTextLine(5, "TAL(%.2f,%.2f)",mRotLeft, mRotLeftPerSec);
nxtDisplayCenteredTextLine(6, "TAL(%.2f,%.2f)",mRotRight, mRotRightPerSec);
if (moveModeType != MMAllMoveTypes
&& moveModeType != MMTurnsOnly
&& moveModeType != MMArcTurnsOnly) {
return;
}
if (moveModeTiming == MMOneMoveAtATime) {
waitForTouch();
}
if (stepThroughMode == stepThroughModeOn) {
waitForTouch();
}
checkParameterRange(mRotLeft, mRotLeftPerSec);
checkParameterRange(mRotRight, mRotRightPerSec);
int leftWheelInitial = nMotorEncoder[leftWheelMotor];
int leftWheelTarget = nMotorEncoder[leftWheelMotor] - 360 * mRotLeft;
int rightWheelInitial = nMotorEncoder[rightWheelMotor];
int rightWheelTarget = nMotorEncoder[rightWheelMotor] - 360 * mRotRight;
ClearTimer(T1);
float leftMotorPower = revolutionsPerSecondToMotorPower(mRotLeftPerSec);
float rightMotorPower = revolutionsPerSecondToMotorPower(mRotRightPerSec);
motor[leftWheelMotor] = -1 * leftMotorPower;
motor[rightWheelMotor] = -1 * rightMotorPower;
while ( (nMotorEncoder[leftWheelMotor] > leftWheelTarget) &&
(nMotorEncoder[rightWheelMotor] > rightWheelTarget) )
{
nxtDisplayCenteredTextLine(7, "%d", nMotorEncoder[leftWheelMotor]);
}
motor[leftWheelMotor] = 0;
motor[rightWheelMotor] = 0;
int leftWheelChange = nMotorEncoder[leftWheelMotor] - leftWheelInitial;
float revolutionsWheelsRotatedLeft = ((float) leftWheelChange) / 360.0;
int rightWheelChange = nMotorEncoder[rightWheelMotor] - rightWheelInitial;
float revolutionsWheelsRotatedRight = ((float) rightWheelChange) / 360.0;
nxtDisplayCenteredTextLine(2, "TAL(%.2f,%.2f)",
mRotLeft, mRotLeftPerSec);
nxtDisplayCenteredTextLine(3, "%.2frev %.2fsec",
(float) revolutionsWheelsRotatedLeft, (float) time10(T1) / 100);
nxtDisplayCenteredTextLine(4, "TAL(%.2f,%.2f)",
mRotRight, mRotRightPerSec);
nxtDisplayCenteredTextLine(5, "%.2frev %.2fsec",
(float) revolutionsWheelsRotatedRight, (float) time10(T1) / 100);
nxtDisplayCenteredTextLine(6, "");
nxtDisplayCenteredTextLine(7, "");
}