void turn(int direction,int degrees,int powerLevel){//Dir:1 = right, 0 = left::Degrees
	//Accurately turn with GYRO
	float rotSpeed = 0;
	float heading = 0;
	int slow = powerLevel/2;
	// Calibrate the gyro, make sure you hold the sensor still
	HTGYROstartCal(HTGYRO);
	while(true){
		while (time1[T1] < 20)
			wait1Msec(1);

		// Reset the timer
		time1[T1]=0;

		// Read the current rotation speed
		rotSpeed = HTGYROreadRot(HTGYRO);

		// Calculate the new heading by adding the amount of degrees
		// we've turned in the last 20ms
		// If our current rate of rotation is 100 degrees/second,
		// then we will have turned 100 * (20/1000) = 2 degrees since
		// the last time we measured.
		heading += rotSpeed * 0.02;
		if(heading>degrees*3.0/4.0)
			powerLevel=slow;
		if(direction==1)
			right(powerLevel);
		else if(direction==0)
			left(powerLevel);

		if(abs(heading)>degrees)
			break;
	}
}
task main(){

HTGYROstartCal(Gyro);

motor[leftWheel] = 100;
motor[rightWheel] = 100;

wait10Msec(20);

motor[leftWheel] = 0;
motor[rightWheel] = 0;

wait10Msec(100);

while(turnRight(40)){
	motor[leftWheel] = -100;
	motor[rightWheel] = 100;
}

motor[leftWheel] = 0;
motor[rightWheel] = 0;

wait10Msec(100);

while(SensorValue[ultraSonic] > 30){
	motor[leftWheel] = 100;
	motor[rightWheel] = 100;
}

	motor[leftWheel] = 0;
	motor[rightWheel] = 0;

}
void initializeRobot()
{
  // Place code here to initialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  nMotorPIDSpeedCtrl[wristMotor] = mtrSpeedReg;
  nMotorEncoder[wristMotor] = 0; //Resets Encoder Values

  //servoChangeRate[wrist] = 2;    // Slows the Change Rate of wrist to 2 positions per update.  Default = 10
  servo[wrist] = wristPosition;
  servo[elbow] = elbowPosition;
  servo[trapperL] = trapperPosition;
  servo[trapperR] = 256 - trapperPosition;
  servo[tipperL] = tipperPosition;
  servo[tipperR] = 256 - tipperPosition;

  Gyro_offset = HTGYROstartCal(HTGYRO);   //start calibration, but do not trust results
  for (int i = 0 ; i < 10; i++)           //instead read 10 values and generate average offset
  {
    sum = sum + HTGYROreadRot(HTGYRO);
    wait1Msec(50);
  }
  Gyro_offset = sum / 10.0;

return;
}
Beispiel #4
0
void initializeRobot() {
	bDisplayDiagnostics=false;
	setGyroPos(false);
	if (HTSMUXreadPowerStatus(MUX)) {			// Multiplexer is off or dead?
		nxtDisplayBigStringAt(0,45,"   MUX   ");
		nxtDisplayBigStringAt(0,25,"Dead Batt");
		PlayImmediateTone(440, 50);
        wait1Msec(1500);
        eraseDisplay();
        fallbackMode=true;
        selection[SONAR_MENU]=3;
	}
	if (SensorRaw[gyro]>=640 || SensorRaw[gyro]<=590) {		// Gyro moving or disconnected
		nxtDisplayBigStringAt(21,45,"Gyro");
		nxtDisplayBigStringAt(15,25,"Error");
		nxtDisplayCenteredTextLine(7, "%i", SensorRaw[gyro]);
		PlayImmediateTone(440, 50);
		wait1Msec(1500);
		setGyroEnabled(false);
	} else {
		wait1Msec(100);
		nxtDisplayBigStringAt(15,45,"Wait...");
		HTGYROstartCal(gyro);
		PlaySound(soundBeepBeep);
		setGyroEnabled(true);
	}
	eraseDisplay();
	setClamp(true);
	initMenus();
	return;
}
task main () {

  nxtDisplayCenteredTextLine(0, "HiTechnic");
  nxtDisplayCenteredBigTextLine(1, "GYRO");
  nxtDisplayCenteredTextLine(3, "SMUX Test");
  nxtDisplayCenteredTextLine(5, "Connect SMUX to");
  nxtDisplayCenteredTextLine(6, "S1 and sensor to");
  nxtDisplayCenteredTextLine(7, "SMUX Port 1");
  wait1Msec(2000);

  nxtDisplayCenteredTextLine(5, "Press enter");
  nxtDisplayCenteredTextLine(6, "to set relative");
  nxtDisplayCenteredTextLine(7, "heading");
  wait1Msec(2000);

  // Before using the SMUX, you need to initialise the driver
  HTSMUXinit();

  // Tell the SMUX to scan its ports for connected sensors
  HTSMUXscanPorts(HTSMUX);

  // The sensor is connected to the first port
  // of the SMUX which is connected to the NXT port S1.
  // To access that sensor, we must use msensor_S1_1.  If the sensor
  // were connected to 3rd port of the SMUX connected to the NXT port S4,
  // we would use msensor_S4_3

  wait1Msec(2000);
  eraseDisplay();
  time1[T1] = 0;
  while(true) {
    if (time1[T1] > 1000) {
      eraseDisplay();
      nxtDisplayTextLine(1, "Resetting");
      nxtDisplayTextLine(1, "heading");
      wait1Msec(500);

      // Start the calibration and display the offset
      nxtDisplayTextLine(2, "Offset: %4d", HTGYROstartCal(msensor_S1_1));
      PlaySound(soundBlip);
      while(bSoundActive);
      time1[T1] = 0;
    }

    while(nNxtButtonPressed != kEnterButton) {
      eraseDisplay();

      nxtDisplayTextLine(1, "Reading");
      // Read the current calibration offset and display it
      nxtDisplayTextLine(2, "Offset: %4d", HTGYROreadCal(msensor_S1_1));

      nxtDisplayClearTextLine(4);
      // Read the current rotational speed and display it
      nxtDisplayTextLine(4, "Gyro:   %4d", HTGYROreadRot(msensor_S1_1));
      nxtDisplayTextLine(6, "Press enter");
      nxtDisplayTextLine(7, "to recalibrate");
      wait1Msec(100);
    }
  }
}
task main ()
{
  float rotSpeed = 0;
  float heading = 0;

  // Calibrate the gyro, make sure you hold the sensor still
  HTGYROstartCal(HTGYRO);

  // Reset the timer.
  time1[T1] = 0;

  while (true)
  {
    // Wait until 20ms has passed
    while (time1[T1] < 20)
      wait1Msec(1);

    // Reset the timer
    time1[T1]=0;

    // Read the current rotation speed
    rotSpeed = HTGYROreadRot(HTGYRO);

    // Calculate the new heading by adding the amount of degrees
    // we've turned in the last 20ms
    // If our current rate of rotation is 100 degrees/second,
    // then we will have turned 100 * (20/1000) = 2 degrees since
    // the last time we measured.
    heading += rotSpeed * 0.02;

    // Display our current heading on the screen
    nxtDisplayCenteredBigTextLine(3, "%2.0f", heading);
  }
}
void gyroTurn(float power, float fDegrees, eDirection direct)
{//Robot turns at a specific power, a certain angle, either right or left
	HTGYROstartCal(gyro); //Activate gyroscope
	wait1Msec(100); //Let it adjust

	float	fCurrent = 0.0; //Set heading to 0 degrees
	float fRotSpeed = 0.0; //Current rotational speed is 0

	if (direct == dLeft) //If told to turn left
	{
		leftTurn(power); //Set motors to turn left
	}
	else //If tol to turn right
	{
		rightTurn(power); //Set motors to turn right
	}

	do //Loop through following until conditions are met
	{
		wait1Msec(MEASUREMENT_MS); //Give a moment to recalibrate
		fRotSpeed = HTGYROreadRot(gyro); //Rotation speed is now the gyro's input

		fCurrent += fRotSpeed * (MEASUREMENT_MS / 1000.0); //Current heading is what is was before
		//plus the value of the rate of turn times the amount of time that rate value was taken
	} while (abs(fCurrent) < fDegrees); //Repeat until the heading is the amount told to turn
	stopMotors(); //You got there, now stop
}
Beispiel #8
0
void initSweg() {
	//Stop moving.
	motor[left] = 0;
	motor[right] = 0;

	//Stand up.
	servo[rearFlipper] = REAR_LIFT_DEPRESSED;
	servo[frontFlipper] = FRONT_LIFT_DEPRESSED;

	//Wait for it to stand all the way up.
	wait1Msec(2700);

	//Recalibrate the gyro.
	StopTask(gyros);
	PlaySound(soundBeepBeep);//"Starting calibration!"
	HTGYROstartCal(forwardsTilt);
	wait1Msec(1000);//Give it time to calibrate
	PlaySound(soundBeepBeep);//"Done calibrating!"

	//K, finished with gyro calibration. Start measuring angle now.
	StartTask(gyros);

	//Not sure what these two lines are for.
	nMotorEncoder[left] = 0;
	nMotorEncoder[right] = 0;
}
void driveSonar(int direction,float distance,int powerLevel){//Dir:distance(centimeters):1 = reverse, 0 = forward:
	//Drive in a straight line GYRO
	float rotSpeed = 0;
	float heading = 0;
	int dir = direction==0?1:-1;
	// Calibrate the gyro, make sure you hold the sensor still
	HTGYROstartCal(HTGYRO);
	time1[T2] = 0;
	while(SensorValue[sonarSensor] > distance){
		while (time1[T1] < 20)
			wait1Msec(1);
		// Reset the timer
		time1[T1]=0;

		// Read the current rotation speed
		rotSpeed = HTGYROreadRot(HTGYRO);

		// Calculate the new heading by adding the amount of degrees
		// we've turned in the last 20ms
		// If our current rate of rotation is 100 degrees/second,
		// then we will have turned 100 * (20/1000) = 2 degrees since
		// the last time we measured.
		heading += rotSpeed * 0.02;

		if(direction==0){
			motor[driveLeft] = (powerLevel+powerLevel*.15*heading)*dir;
			motor[driveRight] = -(powerLevel-powerLevel*.15*heading)*dir;
		}else{
			motor[driveLeft] = (powerLevel-powerLevel*.15*heading)*dir;
			motor[driveRight] = -(powerLevel+powerLevel*.15*heading)*dir;
		}
	}
}
Beispiel #10
0
void Gyro_Start()
{
	float rotSpeed = 0;
  float heading = 0;
  HTGYROstartCal(Gyro);
  time1[T1] = 0;
  StartTask(UpdateGyro);
}
//Initialization: don't touch
void initializeRobot() {
	HTGYROstartCal(gyro); //Gyro calibration
	distanceCalibration(); //Calculating correction constants
	initializeAssignments(); //Filling assignment array
	HTACreadAllAxes(HTAC, xcal, ycal, zcal); //Calibrating accelerometer

	return;
}
void init(){
	servo[fieldGrabberLeft] = _open;
	servo[fieldGrabberRight] = 255-_open;
	servo[scoopBridge] = 155;
	nMotorEncoder[intake] = 0;
	sticksUp();
	retainAutoBall();
	HTGYROstartCal(HTGYRO);
}
task update_gyro ()
{
	HTGYROstartCal(Gyro);
	while(true)
	{
		float angle=HTGYROreadRot(Gyro)/100.0;
		happy_angle=happy_angle+angle;
		wait1Msec (10);
	}
}
void initialize()
{
  HTGYROstartCal(S3);
  servo[servo1] = 0;
  //servo[servo2] = 10;
  servo[servo3] = 250;
  servo[servo4] = 139;
  servo[servo5] = 75;
  servo[servo2] = 0;
}
Beispiel #15
0
void turnTask(float turnDegrees, int motorSpeed)
{
  float rotSpeed = 0;
  float heading = 0;

  // Calibrate the gyro, make sure you hold the sensor still
  HTGYROstartCal(HTGYRO);

  // Get current program time in milliseconds.
  int lastPgmTime = nPgmTime;
  int currentPgmTime = nPgmTime;

  // Begin moving robot
  if (turnDegrees > 0)
  {
    motor[leftMotor] = motorSpeed;
    motor[rightMotor] = motorSpeed * -1;
  } else
  {
    motor[rightMotor] = motorSpeed;
    motor[leftMotor] = motorSpeed * -1;
  }

  while (abs(heading) < abs(turnDegrees))
  {

    // Read the current rotation speed
    if (lastPgmTime + 20 < nPgmTime)
    {

      currentPgmTime = nPgmTime;

      rotSpeed = HTGYROreadRot(HTGYRO);

	    // Calculate the new heading by adding the amount of degrees
	    // we've turned in the last 20ms
	    // If our current rate of rotation is 100 degrees/second,
	    // then we will have turned 100 * (20/1000) = 2 degrees since
	    // the last time we measured.
	    heading += rotSpeed * (((float)currentPgmTime - (float)lastPgmTime) / 1000);

	    lastPgmTime = currentPgmTime;

	    // Display our current heading on the screen
	    nxtDisplayCenteredBigTextLine(3, "%3.2f", heading);
   }

  }

  // stop motors
  motor[leftMotor] = 0;
  motor[rightMotor] = 0;


}
void initializeRobot()
{
	HTGYROstartCal(HTGYRO);
	nMotorEncoder[motorC] = 0;

	servo[wrist] = 245;
	servo[shoulder] = 255;
	servo[right_servo] = 0;
	servo[left_servo] = 180;
	return;
}
task update_gyroSensor()
{
	HTGYROstartCal(gyroSensor);
	while(true)
	{
		float angle=HTGYROreadRot(gyroSensor)/100.0;
		happy_angle=happy_angle+angle;
		nxtDisplayTextLine(7, "%d", happy_angle);
		wait1Msec (10);
	}
}
void initializeRobot()
{
  Gyro_offset = HTGYROstartCal(HTGYRO);   //start calibration, but do not trust results
  for (int i = 0 ; i < 20; i++)           //instead read 20 values and generate average offset
  {
    sum = sum + HTGYROreadRot(HTGYRO);
    wait1Msec(10);
  }
  Gyro_offset = sum / 20.0;

  return;
}
task main()
{

//Wait for the start command//
waitForStart();
	
//Start getHeading.h//
	HTGYROstartCal(GYRO);
	StartTask(getHeading);
	
//Move forward until it sees the beacon or until 5 sec is up//
	motor[motorD] = -50;
	motor[motorE] = -50;
	time1[T3] = 0;

	while (atBeacon() == false && time1[T3] < 5000) {
		wait1Msec(2);
	}

//Turn 90 to the right//
	TurnAngle(90, false);

	///////////////////////
	
//Move forward for 1 sec//
	motor[motorD] = 80;
	motor[motorE] = 80;

	wait1Msec(1000);

//Stop motors//
	motor[motorD] = 0;
	motor[motorE] = 0;

//Move the block belt for 2.5 sec//
	motor[motorC] = -100;

	wait1Msec(2500);

	motor[motorC] = 0;


//Move backward for 1 sec//
	motor[motorD] = -50;
	motor[motorE] = -50;

	wait1Msec(1000);

	motor[motorD] = 0;
	motor[motorE] = 0;

}
void initializeRobot()
{
	disableDiagnosticsDisplay();
	RequestedScreen=S_MISSION;			// display mission selection screen at startup
	StartTask(buttons);							// start user interface = buttons
	StartTask(display);							// start user interface - display
	StartTask(sensors);							// sensors task runs continuously
	StartTask(datalog);							// data log for IR sensors runs continuously
	moveLightServo(UP);
	wait1Msec(100);									// give some stabilizing time before doing GYRO cal
	HTGYROstartCal(HTGYRO);
	alive();
	return;
}
// This task adds to happy_angle to keep the robots current heading in degrees.
// The if statement is so that the value does not pass 360, in the case you make a 360.
task update_gyroSensor()
{
	HTGYROstartCal(gyroSensor);
	while(true)
	{
		float angle = HTGYROreadRot(gyroSensor) / 100.0;
		happy_angle = happy_angle + angle;
		if (happy_angle == 360)
		{
			happy_angle = 0;
		}
		wait1Msec(1);
	}
}
void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  Gyro_offset = HTGYROstartCal(HTGYRO);   //start calibration, but do not trust results
  for (int i = 0 ; i < 10; i++)           //instead read 10 values and generate average offset
  {
    sum = sum + HTGYROreadRot(HTGYRO);
    wait1Msec(50);
  }
  Gyro_offset = sum / 10.0;

  return;
}
void initializeRobot()
{
	HTGYROstartCal(HTGYRO);
	LSsetActive(lineFollower);
	setWrist (.95);
	while (SensorValue [liftDownSensor] == 0) motor [linearSlides]=-100;
	motor [linearSlides]=0;

	wait1Msec (500);

	// Place code here to sinitialize servos to starting positions.
	// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

	return;
}
void initializeRobot()
{
	disableDiagnosticsDisplay();
	RequestedScreen=S_MISSION;			// display mission selection screen at startup
	StartTask(buttons);							// start user interface = buttons
	StartTask(display);							// start user interface - display
	StartTask(sensors);							// sensors task runs continuously
	motor[lightMotor] = 50;					// drop the light sensoor
	wait1Msec(500);									// wait for it to move down
	motor[lightMotor] = 0;					// and shut off the motor
	wait1Msec(100);									// give some stabilizing time before doing GYRO cal
	HTGYROstartCal(HTGYRO);
	alive();
	return;
}
Beispiel #25
0
void turn(int angle, short speed)  // Steven Mostovoy
{
	if (angle > 0)
		angle += 65;
	else
	  angle -= 65;
	if (angle < 0)
	{
		angle *= -1;
		speed *= -1;
	}
	wait1Msec(200);

	HTGYROstartCal(S2);

	wait1Msec(200);
	int gyro[3] = {0,0,0};
	int gyroMid[3] = {0,0,0};
	int gyroFinal[4] = {0,0,0,0};
	motor[driveLeft] = speed;
	motor[driveRight] = -speed;
	int integral = 0;
	while (true)
	{
		for (int i = 2; i > 0; --i)
			gyro[i] = gyro[i-1];
		gyro[0] = HTGYROreadRot(S3);

		float mu = 10.0/11.0;

		gyroMid[0] = gyro[0];
		for (int i = 1; i < 3; ++i)
			gyroFinal[i] = mu * (gyro[i] - gyroMid[i-1]) + gyroMid[i-1];

		for (int i = 3; i > 0; --i)
			gyroFinal[i] = gyroMid[i-1];
		gyroFinal[0] = gyroMid[1];

		integral += (gyroFinal[0] + gyroFinal[1]*2 + gyroFinal[2]*2 + gyroFinal[3])/6;

		//writeDebugStreamLine("%d", integral);
		if (abs(integral * 0.0604) > angle) break;
		wait1Msec(10);
	}
	motor[driveLeft] = 0;
	motor[driveRight] = 0;
}
Beispiel #26
0
task main()
{
	  int heading = 0, y, x, interval;
		HTGYROstartCal(gyro);
		while(1)
		{
			y = nPgmTime;
			nxtDisplayTextLine(1, "y: %d", y);
			x = HTGYROreadRot(gyro);
		  nxtDisplayTextLine(3, "%d", x);
		  wait10Msec(100);
		  interval = nPgmTime - y;
		  nxtDisplayTextLine(4, "time: %d", nPgmTime);
		  nxtDisplayTextLine(5, "Time-y: %f", interval);
		  wait10Msec(100);
	  }
}
Beispiel #27
0
task Gyro()
{
	HTGYROstartCal(sensor_gyro);
	float vel_curr = 0.0;
	float vel_prev = 0.0;
	float dt = 0.0;
	int timer_gyro = 0;
	Time_ClearTimer(timer_gyro);

	while (true) {
		vel_prev = vel_curr;
		dt = (float)Time_GetTime(timer_gyro)/(float)1000.0; // msec to sec
		Time_ClearTimer(timer_gyro);
		vel_curr = (float)HTGYROreadRot(sensor_gyro);
		heading += ((float)vel_prev+(float)vel_curr)*(float)0.5*(float)dt;
		Time_Wait(1);
	}
}
void initializeRobot()
{
  //resetHang(); //hang arm
  //resetBucket(); //DO NOT reset the NXT motors!!

	//reset drive train encoders
	nMotorEncoder[leftDrive] = 0;
	nMotorEncoder[rightDrive] = 0;

	HTGYROstartCal(gyroSensor); //calibrate gyro

	StartTask(Arm);

	//beep to signal end of initialization
	PlayTone(440, 30);

  return;
}
task main()
{

	HTGYROstartCal(GYRO);
	StartTask(getHeading);

//Drive forword for 5 sec//
  motor[motorD] = 100;
  motor[motorE] = 100;

  wait1Msec(5000);

//Stop motors//
  motor[motorD] = 0;
  motor[motorE] = 0;

//Turn right 90 degrees//
  TurnAngle(90, 100);

//Move forwar for 2.5 sec//
  motor[motorD] = 100;
  motor[motorE] = 100;

  wait1Msec(2500);

//Stop motors//
  motor[motorD] = 0;
  motor[motorE] = 0;

//Turn right 90 degrees//
  TurnAngle(90, 100);

//Move forward for 2.2 sec//
  motor[motorD] = 100;
  motor[motorE] = 100;

  wait1Msec(2200);

//Stop motors//
  motor[motorD] = 0;
  motor[motorE] = 0;


}
task main () {

  nxtDisplayCenteredTextLine(0, "HiTechnic");
  nxtDisplayCenteredBigTextLine(1, "GYRO");
  nxtDisplayCenteredTextLine(3, "SMUX Test");
  nxtDisplayCenteredTextLine(5, "Connect SMUX to");
  nxtDisplayCenteredTextLine(6, "S1 and sensor to");
  nxtDisplayCenteredTextLine(7, "SMUX Port 1");

  wait1Msec(2000);
  eraseDisplay();
  time1[T1] = 0;
  while(true) {
    if (time1[T1] > 1000) {
      eraseDisplay();
      nxtDisplayTextLine(1, "Resetting");
      nxtDisplayTextLine(1, "heading");
      wait1Msec(500);

      // Start the calibration and display the offset
      nxtDisplayTextLine(2, "Offset: %4d", HTGYROstartCal(HTGYRO));
      PlaySound(soundBlip);
      while(bSoundActive) EndTimeSlice();
      time1[T1] = 0;
    }

    while(nNxtButtonPressed != kEnterButton) {
      eraseDisplay();

      nxtDisplayTextLine(1, "Reading");
      // Read the current calibration offset and display it
      nxtDisplayTextLine(2, "Offset: %4d", HTGYROreadCal(HTGYRO));

      nxtDisplayClearTextLine(4);
      // Read the current rotational speed and display it
      nxtDisplayTextLine(4, "Gyro:   %4d", HTGYROreadRot(HTGYRO));
      nxtDisplayTextLine(6, "Press enter");
      nxtDisplayTextLine(7, "to recalibrate");
      wait1Msec(100);
    }
  }
}