void Turn(int power, long distance)
{
writeDebugStreamLine("new *****");
//writeDebugStreamLine("value: %d", SensorValue[Gyro]);
//int zero = initialize() + 59;//59 is added to perfectly 0 it.
long sum = 0;
//writeDebugStreamLine("delta: %d", SensorValue[Gyro] - zero);// begining delta is 59 +-1 often
//writeDebugStreamLine("zero: %d", zero);
//writeDebugStreamLine("value: %d", SensorValue[Gyro]);
//nMotorEncoder[motorB] = 0;
motor[motorD] = power;
motor[motorE] = -1 * power;
while( abs(sum) < abs(distance))// +-8400 is 90 degrees
{
   //writeDebugStreamLine("%d", SensorValue[Gyro] - zero);
   sum = sum + deadband(HTGYROreadRot(HTGYRO), 5);
   wait1Msec(10);// update interval
   writeDebugStreamLine("sum: %d", sum);
   writeDebugStreamLine("Gyro:   %4d", HTGYROreadRot(HTGYRO));
   nxtDisplayTextLine(4, "Gyro:   %4d", HTGYROreadRot(HTGYRO));
}
//writeDebugStreamLine("sum: %d", sum);
motor[motorD] = 0;
motor[motorE] = 0;
wait10Msec(1); // just to let the encoders reset when the robot is still
nMotorEncoder[motorE] = 0;
nMotorEncoder[motorD] = 0;
}
Ejemplo n.º 2
0
void turnTowardsRobot() {
	//int tmpUS = USScanVal;
	time1[T2] = 0;

	while(searchingForBot) {
		while (USScanVal > 92 ) {

			wait1Msec(20);

			if (abs(rotSpeed) > 3) {
			rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
			heading += (rotSpeed * 0.02) / 0.3;//find gyro heading in degrees??
			}

			//PlayTone(400, 500);
			motor[motorLeft] = -5;
			motor[motorRight] = 5;
			if (heading >= USScanVal) {
			    motor[motorLeft] = 0;
			    motor[motorRight] = 0;
				searchingForBot = false;

			}//end if
			break;
		}//end while

		while (USScanVal < 92) {

			wait1Msec(20);

			if (abs(rotSpeed) > 3) {
			rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
			heading += (rotSpeed * 0.02);//find gyro heading in degrees??
			}

			//PlayTone(400, 500);
			motor[motorLeft] = 5;
			motor[motorRight] = -5;
			if (heading <= USScanVal) {
				motor[motorLeft] = 0;
			  motor[motorRight] = 0;
				searchingForBot = false;

			}//end if
			break;
		}//end while

		if (USScanVal == 92) {
			//do nothing
			break;
		}
	}//end search
}//end void
Ejemplo n.º 3
0
void turnTowardsCenter() {

	if (turnedLeft) {
		while(heading > 92) {

			wait1Msec(20);

			if (abs(rotSpeed) > 3) {
			rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
			heading += (rotSpeed * 0.02) * 0.7;//find gyro heading in degrees??
			}

			motor[motorLeft] = 5;
			motor[motorRight] = -5;

			if (heading <= 92) {
				motor[motorLeft] = 0;
				motor[motorRight] = 0;
				break;
			}
		}
		turnedLeft = false;
		turnedRight = false;
	}

	if (turnedRight) {
		while(heading < 92) {

			wait1Msec(20);

			if (abs(rotSpeed) > 3) {
			rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
			heading += (rotSpeed * 0.02) / 0.3;//find gyro heading in degrees??
			}

			motor[motorLeft] = -5;
			motor[motorRight] = 5;

			if (heading >= 92) {
				motor[motorLeft] = 0;
				motor[motorRight] = 0;
				break;
			}
		}
		turnedLeft = false;
		turnedRight = false;
	}
}
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);
  }
}
Ejemplo n.º 5
0
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
}
Ejemplo n.º 6
0
//------------
//=================================================
// Task to handle the gyro and the other sensors hooked up to the sensor mux
//=================================================
task gyro()
{
	float currDir = 0.0; float prevDir = 0.0;
	long currtime,prevtime;
	ir_mux_status=HTSMUXreadPowerStatus(IR_MUX);				// read the sensor multiplexor status
	gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX);		// read the sensor multiplexor status
	while (ir_mux_status || gyro_mux_status)  					// check good battery power on both muxes
	{
		//PlayTone(750,25);
		wait1Msec(500);
	}

	wait1Msec(300);

	for (int i=0;i<5;i++)            // check if there is too much spread in the data
	{
		if (gyro_noise>10)
		{
			PlayTone (250,25);
			wait1Msec(500);
		}
	}
	calibrate = 2;
	prevtime = nPgmTime;
	while(true)
	{
		currtime=nPgmTime;
		rawgyro = (float)HTGYROreadRot(HTGYRO);
		constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
		relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
		prevtime = currtime;
		wait1Msec(1);
		prevDir = currDir;
	}
}
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;
}
Ejemplo n.º 8
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);

  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 keepHeading(){
	float rotSpeed = 0;
	// Calibrate the gyro, make sure you hold the sensor still
	while(true){
		while (time1[T3] < 20)
			wait1Msec(1);

		// Reset the timer
		time1[T3]=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.
		globalHeading += rotSpeed * 0.02;
		nxtDisplayCenteredTextLine(1,"GH:%f",globalHeading);
		wait1Msec(1);
		if(globalHeading>180)
			globalHeading = -180;
		else if(globalHeading<-180)
			globalHeading =  180;
	}
}
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;
	}
}
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;
		}
	}
}
Ejemplo n.º 12
0
task update_gyro ()
{
	HTGYROstartCal(Gyro);
	while(true)
	{
		float angle=HTGYROreadRot(Gyro)/100.0;
		happy_angle=happy_angle+angle;
		wait1Msec (10);
	}
}
Ejemplo n.º 13
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;


}
Ejemplo n.º 14
0
void initializeRobot()
{
	// Finds average base gyro reading
	for(int i = 0; i < 100; i++)
	{ //sums up first hundred gyro readings
		initial += HTGYROreadRot(HTGYRO);
		wait10Msec(1);
	}
	initial = initial / 100; //divides by 100 to find the average reading
  return;
}
Ejemplo n.º 15
0
task main()
{
  initializeRobot();

  while(true)
  {
    Gyro_value = HTGYROreadRot(HTGYRO);
    wait1Msec(10);
  }

}
Ejemplo n.º 16
0
task gyroTask()
{
  float lastTime = nPgmTime;
  while(1)
  {
    gyroData.degsec = HTGYROreadRot(GYRO);
    gyroData.deg += dbc(gyroData.degsec,1)*((nPgmTime-lastTime)/1000);
    lastTime = nPgmTime;
    wait1Msec(1);
  }
}
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);
	}
}
Ejemplo n.º 18
0
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;
}
Ejemplo n.º 19
0
task gyros() {//Updates the forwardsAngle global variable (degrees) with the current deviation from 90 degrees.
	nSchedulePriority = kHighPriority;//This exemplifies my hate of ROBOTC, but it pretty much just sets it to high CPU priority.

	INTR intr;//Integrator.
	initIntr(intr);

	forwardsAngle = 0;

	while(true) {//Constantly integrating gyro rotational velocity reading in degrees/sec to get current heading in degrees.
		forwardsAngle = integrate(intr, HTGYROreadRot(forwardsTilt));
		wait1Msec(5);
	}
}
Ejemplo n.º 20
0
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    GyroTurn
//
// This is a generic movement routine that turns the robot a specific number of degrees using a gyro
// sensor to measure angular velocity
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void GyroTurn(int iSpeed, int iDegrees, driveState iCmd)
{
	float vcurrposition = 0;
	int   vprevtime     = nPgmTime;
	int   vcurrtime;
	float vcurrRate;
	int   voffset;
	float deltaSecs;
	float degChange;

	if (iCmd == LEFT)
	{
		motor[DriveLeft]  = -1 * iSpeed;
		motor[DriveRight] = iSpeed;
		voffset = -1;
	}
	else
	{
		motor[DriveLeft]  = iSpeed;
		motor[DriveRight] = -1 * iSpeed;
		voffset = 1;
	}

	while (vcurrposition < iDegrees)
	{
		// This tells us the current rate of rotation in degrees per
		// second.
		vcurrRate = HTGYROreadRot(gyro) * voffset;

		// How much time has elapsed since we last checked, which we use
		// to determine how far we've turned
		vcurrtime = nPgmTime;

		deltaSecs = (vcurrtime - vprevtime) / 1000.0;
		if (deltaSecs < 0)
		{
			deltaSecs = ((float)((vcurrtime + 1024) - (vprevtime + 1024))) / 1000.0;
		}

		// Calculate how many degrees the heading changed.
		degChange = vcurrRate * deltaSecs;
		vcurrposition = vcurrposition + degChange;
		vprevtime = vcurrtime;
	}

	motor[DriveLeft]  = 0;
	motor[DriveRight] = 0;

	//The following line is used to pause the robot in between movements
	wait1Msec(100);
}
Ejemplo n.º 21
0
void turn(int iSpeed, int iDegrees, int code)
{
	float vcurrposition = 0;
	int   vprevtime     = nPgmTime;
	int   vcurrtime;
	float vcurrRate;
	int   voffset;
	float deltaSecs;
	float degChange;

	if (code == L_CODE)
	{
		motor[mLeft]  = iSpeed;
		motor[mRight] = -iSpeed;
		voffset = 1;
	}
	else if(code == R_CODE)
	{
		motor[mLeft]  = -iSpeed;
		motor[mRight] = iSpeed;
		voffset = -1;
	}

	while (vcurrposition < iDegrees)
	{
		// This tells us the current rate of rotation in degrees per
		// second.
		vcurrRate = HTGYROreadRot(gyro) * voffset;

		// How much time has elapsed since we last checked, which we use
		// to determine how far we've turned
		vcurrtime = nPgmTime;

		deltaSecs = (vcurrtime - vprevtime) / 1000.0;
		if (deltaSecs < 0)
		{
			deltaSecs = ((float)((vcurrtime + 1024) - (vprevtime + 1024))) / 1000.0;
		}

		// Calculate how many degrees the heading changed.
		degChange = vcurrRate * deltaSecs;
		vcurrposition = vcurrposition + degChange;
		vprevtime = vcurrtime;
	}

	motor[mLeft]  = 0;
	motor[mRight] = 0;

	wait1Msec(100);
}
Ejemplo n.º 22
0
// 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);
	}
}
task main()
{
  initializeRobot();

  waitForStart(); // Wait for the beginning of autonomous phase.

  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////
  ////                                                   ////
  ////    Add your robot specific autonomous code here.  ////
  ////                                                   ////
  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////

  while (true)
  {
        PlaySound(soundBlip);                          // Play the sound, 'soundBeepBeep'.
        Gyro_value=HTGYROreadRot(HTGYRO);              //read the gyro sensor
        IR_value=HTDIRreadDCDir(S3);

        for(int i = 0; i < 200; i++)
        {
          motor[leftMotor] = 50;                         // Travel straight
          motor[rightMotor] = 50;
          wait1Msec(50);
        }

      // Wait for 2.5 seconds

        motor[leftMotor] = 0;                          // Stop
        motor[rightMotor] = 0;

        for (int i = 0; i < 200; i++)                  // Start looking for IR beacon
        {
          IR_value = HTDIRreadDCDir(S3);
          IR_offset = IR_value - 5;                    // 5 is center value
          motor[leftMotor] = 50 - 5*IR_offset;
          motor[rightMotor] = 50 + 5*IR_offset;
          wait1Msec(50);                               // Wait before looping again.
        }

        motor[leftMotor] = 0;                          // Stop
        motor[rightMotor] = 0;

        PlaySound(soundBlip);                          // Play the sound, 'soundBeepBeep'.

        while(true) {wait1Msec(1);}                    // Loop forever doing nothing
  }
}
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;
}
Ejemplo n.º 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;
}
Ejemplo n.º 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);
	  }
}
//====================================
// Gyro Calibration helper function
//====================================
float  MyHTCal(long caltime)
{
	long highest = -1000, lowest = 10000;
	float average = 0;
	long starttime = nPgmTime;
	long samples=0;
	long data;
	while (nPgmTime < starttime+caltime)			// loop for the requested number of seconds
	{
		samples +=1;														// count the number of iterations for averaging
		data = HTGYROreadRot(HTGYRO);						// get a new reading from the GYRO
		average += (float)data;									// add in the new value to the average
		if (highest < data) highest = data;			// adjust the highest value if necessary
			if (lowest> data) lowest = data;				// likewise for the lowest value
	}
	gyro_noise=abs(highest-lowest);						// save the spread in the data for diagnostic display
	return average/samples;										// and return the average drift
}
Ejemplo n.º 28
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);
	}
}
//=================================================================
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
float  MyHTCal(long caltime)
{
	float average = 0;
	long starttime = nPgmTime;
	long samples=0;
	long data;
	highest =  -10000;
	lowest = 10000;
	while (nPgmTime < starttime+caltime)
	{
		samples +=1;
		data = HTGYROreadRot(HTGYRO);
		average += (float)data;
		if (highest < data) highest = data;
		if (lowest> data) lowest = data;
	}
	return average/samples;
}
Ejemplo n.º 30
0
void turn(int direction) {
	int mul = direction;

	resetEncoders();
	//float degreesToTurn = degrees - (degrees/27);
	motor[motorD] = 15 * mul;
	motor[motorE] = -15 * mul;

	float current_rot = HTGYROreadRot(HTGyro) +14;
	float time = time1[T1];

	float distance = ((prev_rot+current_rot)/2)*time/1000;
	total_distance += distance;
	prev_rot = current_rot;
	clearTimer(T1);
	wait1Msec(10);
	motor[motorD] = 0;
	motor[motorE] = 0;
}