EncodersClass::EncodersClass() {
clearEncoders();
for (uint8_t i = 0; i < GUI_NUM_ENCODERS; i++) {
sr_old2s[i] = 0;
}
sr_old = 0;
}
int forwardDistance(float distance){
clearEncoders(); //clear the encoders
firstForward = (((tileWidth*distance)/ wheelDistance) *rotationTicks); //calculates distance
while((nMotorEncoder[rightMotor] <= firstForward) &&(nMotorEncoder[leftMotor] <= firstForward) ){
forward(); //robot moves forward
}
return 0;
}
task main()
{
int timeToWait = requestTimeToWait();
initializeRobot();
waitForStart();
disableDiagnosticsDisplay();
//Initialize the gyro and turning
GyroInit(g_Gyro, gyro, 0);
PidTurnInit(g_PidTurn, leftDrive, rightDrive, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
countdown(timeToWait);
//Start actual movement code
moveForwardInches(60,43);//initial forwards
turn(g_PidTurn,45,20);
clearEncoders();
wait1Msec(50);
const int totalTics = 6806;//total tics from before IR to end- CHANGED FOR LESSENED AMOUNT OF FORWARDS
const int ticsToCenter = 3663;//tics from start to central beam
const int ticsToSubtract = 1665;//failsafe, may still need testing
//finding IR
while(HTIRS2readACDir(IR) != 4 && (abs(nMotorEncoder[leftDrive]) < totalTics - ticsToSubtract)){ //finds the beacon zone 4 (rough)
//nxtDisplayCenteredTextLine(5,"Direction:%d",HTIRS2readACDir(IR));
startBackward(27);
}
stopDrive();
wait1Msec(300);
while(HTIRS2readACDir(IR) != 5 && (abs(nMotorEncoder[leftDrive]) < totalTics - ticsToSubtract)){ //slow down to look for basket (fine)
startForward(15);
}
int currentPosition = abs(nMotorEncoder[leftDrive]);
if (currentPosition > ticsToCenter)//check where we are
moveForwardInchesNoReset(20, 6);//move forwards 5 inches (buckets 1 and 2)
else
moveForwardInchesNoReset(20, 3);//forwards 3 inches (buckets 3 and 4)
stopDrive();//stops robot
servo[dumper] = 30;//dumps the block
motor[lift]= 50;//starts the lift up
wait1Msec(700);
motor[lift]= 0;//stops lift
servo[dumper] = servoRestPosition;//resets servo
int ticsToMove= totalTics - abs(nMotorEncoder[leftDrive]);//tics left after IR
displayCenteredTextLine(0,"TTM: %d", ticsToMove);
moveBackwardTics(90, ticsToMove); //move to end after IR
turn(g_PidTurn, -87,40); //turn to go towards ramp
moveForwardInches(90, 44, false, LEFTENCODER); //forwards to ramp
turn(g_PidTurn, 95, 40); //turn to face ramp
moveForwardInches(90, 45, false, LEFTENCODER);//onto ramp
motor[lift]= -50;//starts the lift down
wait1Msec(500);
motor[lift]= 0;//stops lift
while(true){}
}
int turnRight () //use this function to turn right
{
clearEncoders();
while(nMotorEncoder[rightMotor] >= -(turnTicks) && nMotorEncoder[leftMotor] <= turnTicks){
motor[leftMotor] = turnSpeed; //left motor goes backwards
motor[rightMotor] = -(turnSpeed); //right motor goes forward
}
return 0;
}
int turnLeft() //use this function to turn left
{
clearEncoders();
while((nMotorEncoder[rightMotor] <= turnTicks) && (nMotorEncoder[leftMotor] >= -(turnTicks))){
motor[leftMotor] = -turnSpeed;
motor[rightMotor] = turnSpeed;
}
return 0;
}
void pre_auton()
{
// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
bStopTasksBetweenModes = true;
clearEncoders();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
//----test drives (lol)----//
void drive(char synchRatio, int travelDistance, char speed)
{
//note a 90 degree turn is ~190 encoders clicks
clearEncoders();
nSyncedMotors = synchBC;
nSyncedTurnRatio = synchRatio;
nMotorPIDSpeedCtrl[motorB] = mtrSpeedReg;
//nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg;
nMotorEncoderTarget[motorB] = travelDistance;
motor[motorB] = speed;
while(nMotorRunState[motorB] != runStateIdle) {}
changeTheta = getRotation();
currentDirection += changeTheta;
if(currentDirection > 360)
{
currentDirection -= 360;
}
}
//----main----//
task main ()
{
nxtDisplayCenteredTextLine(3, "Roaming");
nxtDisplayCenteredTextLine(5, "This is a test");
initialisePose(); //set up
iterate(stepSize); //run excitation etc
currentDirection = 0; //set initial
currentTheta = 0;
setTemp(); //get local view
checkLocalCell(); //create first association
displayMax();
nxtDisplayTextLine(2, "Num Act.: %3d",numActive);
nxtDisplayTextLine(3, "Direction: %3d", currentDirection);
nxtDisplayTextLine(4, "changeTheta:%3d", changeTheta);
while(nextEmptyCell<numLocalCells)
{
alive(); //stop NXT from sleeping
//eraseDisplay();
//nxtDisplayCenteredTextLine(2, "Num Active: - %4d",numActive);
char lastCellNum = nextEmptyCell;
drive(100,180,50);
pose3D(changeTheta,0.5);
setTemp();
checkLocalCell();
doTurn();
displayMax();
nxtDisplayTextLine(2, "Num Act.: %3d",numActive);
nxtDisplayTextLine(3, "Direction: %3d", currentDirection);
nxtDisplayTextLine(4, "changeTheta:%3d", changeTheta);
//store data
clearEncoders(); //clear encoder count
changeTheta=0;
// wait1Msec(200);
}
}
//----main----//
task main ()
{
Delete(sFileName1, nIoResult);
Delete(sFileName2, nIoResult);
nxtDisplayCenteredTextLine(3, "Roaming");
nxtDisplayCenteredTextLine(5, "This is a test");
initialisePose(); //set up
iterate(stepSize); //run excitation etc
currentDirection = 0; //set initial
currentTheta = 0;
setTemp(); //get local view
checkLocalCell(); //create first association
datalogging();
sumPoseStruct();
while(1)
{
alive(); //stop NXT from sleeping
float centreSonarValue = SensorValue(centreSonar);
if(centreSonarValue<19)
{
doTurn();
pose3D(changeTheta,0);
}
else {
drive(100,180,50);
pose3D(changeTheta,0.5);
}
sumPoseStruct();
setTemp();
checkLocalCell();
//store data
datalogging();
clearEncoders(); //clear encoder count
changeTheta=0;
}
SaveNxtDatalog();
PlaySound(soundException);
while(bSoundActive){}
}
//----main----//
task main ()
{
nxtDisplayCenteredTextLine(3, "Pose Test");
wait1Msec(500);
initialisePose(); //set up
iterate(stepSize); //run excitation etc
currentDirection = 0; //set initial
currentTheta = 0;
changeTheta = 0;
//display data
displayMax();
nxtDisplayTextLine(2, "Num Act.: %3d",numActive);
nxtDisplayTextLine(4, "Direction: %1d", currentDirection);
nxtDisplayTextLine(5, "currentTheta:%3d", currentTheta);
nxtDisplayTextLine(6, "changeTheta:%3d", changeTheta);
datalogging();
while(totalClicks<1800)
{
alive(); //stop NXT from sleeping
totalClicks += clicks;
//drive(-100,190,50);
// drive(50,180,50);
pose3D(changeTheta,0.5);
displayMax();
nxtDisplayTextLine(2, "Num Act.: %3d",numActive);
nxtDisplayTextLine(4, "Direction: %1d", currentDirection);
nxtDisplayTextLine(5, "currentTheta:%3d", currentTheta);
nxtDisplayTextLine(6, "changeTheta:%3d", changeTheta);
clearEncoders(); //clear encoder count
changeTheta=0;
datalogging();
}
PlaySound(soundFastUpwardTones);
while(bSoundActive) {}
SaveNxtDatalog();
}
task autonomous()
{
switch(selectedAuton)
{
case Autonomous_LeftScoreMatchLoads:
{
clearEncoders();
motor[intakeTread] = -127;
motor[intakeWheel] = -127;
wait1Msec(700);
stopIntake();
positionArm(Arm_ScoreHeight);
stopArm();
wait1Msec(1000);
int leftDest = SensorValue[encoder_Left] + 2200;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(60);
}
stopDrive();
motor[intakeTread] = 127;
motor[intakeWheel] = 127;
wait10Msec(200);
}
break;
case Autonomous_RightPickupScoreYellow:
{
clearEncoders();
int leftDest = SensorValue[encoder_Left] + 1850;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(50);
}
stopDrive();
motor[intakeTread] = -127;
motor[intakeWheel] = -127;
wait1Msec(1000);
stopIntake();
positionArm(Arm_ScoreHeight);
int rightDest = SensorValue[encoder_Right] - 730;
while(SensorValue[encoder_Right] > rightDest)
{
motor[driveLeftBack] = 38;
motor[driveRightBack] = 0;
motor[driveLeftFront] = 38;
motor[driveRightFront] = 0;
}
leftDest = SensorValue[encoder_Left] + 650;
while(SensorValue[encoder_Left] < leftDest)
{
motor[driveLeftBack] = 38;
motor[driveRightBack] = 40;
motor[driveLeftFront] = 38;
motor[driveRightFront] = 40;
}
stopDrive();
motor[intakeTread] = 127;
motor[intakeWheel] = 127;
wait1Msec(1500);
stopIntake();
}
break;
case Autonomous_ProgrammingSkills:
{
clearEncoders();
int leftDest = SensorValue[encoder_Left] + 1850;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(50);
}
stopDrive();
motor[intakeTread] = -127;
motor[intakeWheel] = -127;
wait1Msec(1000);
stopIntake();
positionArm(Arm_ScoreHeight);
int rightDest = SensorValue[encoder_Right] - 730;
while(SensorValue[encoder_Right] > rightDest)
{
motor[driveLeftBack] = 38;
motor[driveRightBack] = 0;
motor[driveLeftFront] = 38;
motor[driveRightFront] = 0;
}
leftDest = SensorValue[encoder_Left] + 650;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(40);
}
stopDrive();
motor[intakeTread] = 127;
motor[intakeWheel] = 127;
wait1Msec(500);
stopIntake();
leftDest = SensorValue[encoder_Left] - 390;
while(SensorValue[encoder_Left] > leftDest)
{
driveAtSpeed(-30);
}
stopDrive();
positionArm(990);
stopArm();
rightDest = SensorValue[encoder_Right] + 660;
while(SensorValue[encoder_Right] < rightDest)
{
motor[driveLeftBack] = -30;
motor[driveLeftFront] = -30;
}
stopDrive();
leftDest = SensorValue[encoder_Left] - 2050;
while(SensorValue[encoder_Left] > leftDest)
{
driveAtSpeed(-50);
}
stopDrive();
}
break;
case Autonomous_None:
default:
{
}
break;
}
}
