예제 #1
0
파일: Auto.c 프로젝트: FTC7155/2014
void turn(int angle) {									// use +90 turn right and -90 to turn left (or any other angle, positive means rigth turns)...

	float compassAngle, compassTarget, compassOrigin; //used for turning and configuring compass
	float error;

	compassOrigin = SensorValue(compass);
	compassTarget = (compassOrigin - angle);

	if(compassTarget<0) 							//make sure the angle is between 0 and 360 (adding or subtracting 360 results in the same heading)
		compassTarget += 360;

	if(compassTarget>360)
		compassTarget -= 360;

	error = atan2(sinDegrees(compassTarget)-sinDegrees(compassAngle),cosDegrees(compassTarget)-cosDegrees(compassAngle));
	motor[leftWheel]  =  sgn(angle)*15*error;
	motor[rightWheel] = -sgn(angle)*15*error;

	while(abs(error)<PI/90.0){
		motor[leftWheel]  =  sgn(angle)*15*error;
		motor[rightWheel] = -sgn(angle)*15*error;
		error = atan2(sinDegrees(compassTarget)-sinDegrees(compassAngle),cosDegrees(compassTarget)-cosDegrees(compassAngle));
	}

	motor[leftWheel] = 0;
	motor[rightWheel] =  0;
}
예제 #2
0
task MapRoom()
{

	float botLastXCoordinate = 0.0;
	float botLastYCoordinate = 0.0;
	float botCurrentXCoordinate = 0.0;
	float botCurrentYCoordinate = 0.0;
	int wallXCoordinate = 0;
	int wallYCoordinate = 0;
	int xCoordinateDisplayOffset = 50;
	int yCoordinateDisplayOffset = 40;

	startGyro();
	resetGyro();
	eraseDisplay();

	while(true)
	{
		//Robot position
		botCurrentXCoordinate = botLastXCoordinate + EncoderDistance(ReadEncoderValue()) * sinDegrees(readGyro());
		botCurrentYCoordinate = botLastYCoordinate + EncoderDistance(ReadEncoderValue()) * cosDegrees(readGyro());

		//Wall mapping
		wallXCoordinate = botCurrentXCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * cosDegrees(readGyro());
		wallYCoordinate = botCurrentYCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * sinDegrees(readGyro());

		nxtSetPixel(wallXCoordinate + xCoordinateDisplayOffset, wallYCoordinate + yCoordinateDisplayOffset);
		ResetEncoderValue();
		botLastXCoordinate = botCurrentXCoordinate;
		botLastYCoordinate = botCurrentYCoordinate;
		wait1Msec(20);
	}
}
예제 #3
0
void getMotorSpeeds(int &motorSpeedD, int &motorSpeedE, int &motorSpeedF, int &motorSpeedG, int angle, int Vb) {
  float Vw1, Vw2, Vw3, Vw4, norm_factor;

  Vw1 = Vb*cosDegrees(angle);
  Vw2 = Vb*(cosVal[0]*cosDegrees(angle) + sinVal[0]*sinDegrees(angle));
  Vw3 = Vb*(cosVal[1]*cosDegrees(angle) + sinVal[1]*sinDegrees(angle));
  Vw4 = Vb*(cosVal[2]*cosDegrees(angle) + sinVal[2]*sinDegrees(angle));

  norm_factor = 1.0;

  if (Vw1 > MAXMOTORSPEED) {
    norm_factor = MAXMOTORSPEED / Vw1;
  } else if (Vw2 > MAXMOTORSPEED) {
    norm_factor = MAXMOTORSPEED / Vw2;
  } else if (Vw3 > MAXMOTORSPEED) {
    norm_factor = MAXMOTORSPEED / Vw3;
  } else if (Vw4 > MAXMOTORSPEED) {
    norm_factor = MAXMOTORSPEED / Vw4;
  }

  motorSpeedD = round(Vw1 * norm_factor);
  motorSpeedE = round(Vw2 * norm_factor);
  motorSpeedF = round(Vw3 * norm_factor);
  motorSpeedG = round(Vw4 * norm_factor);
}
예제 #4
0
파일: gobjects.c 프로젝트: kzidane/spl
static GRectangle getBoundsGArc(GArc arc) {
   double rx, ry, cx, cy, p1x, p1y, p2x, p2y, xMin, xMax, yMin, yMax;

   rx = arc->width / 2;
   ry = arc->height / 2;
   cx = arc->x + rx;
   cy = arc->y + ry;
   p1x = cx + cosDegrees(arc->u.arcRep.start) * rx;
   p1y = cy - sinDegrees(arc->u.arcRep.start) * ry;
   p2x = cx + cosDegrees(arc->u.arcRep.start + arc->u.arcRep.sweep) * rx;
   p2y = cy - sinDegrees(arc->u.arcRep.start + arc->u.arcRep.sweep) * ry;
   xMin = fmin(p1x, p2x);
   xMax = fmax(p1x, p2x);
   yMin = fmin(p1y, p2y);
   yMax = fmax(p1y, p2y);
   if (containsAngle(arc, 0)) xMax = cx + rx;
   if (containsAngle(arc, 90)) yMin = cy - ry;
   if (containsAngle(arc, 180)) xMin = cx - rx;
   if (containsAngle(arc, 270)) yMax = cy + ry;
   if (arc->filled) {
      xMin = fmin(xMin, cx);
      yMin = fmin(yMin, cy);
      xMax = fmax(xMax, cx);
      yMax = fmax(yMax, cy);
   }
   return createGRectangle(xMin, yMin, xMax - xMin, yMax - yMin);
}
void MoveRobot(int angle, int Vb, int rotSpeed) {
    float Vw1, Vw2, Vw3, norm_factor;

    // Adjust the angle to make it more intuitive
    angle-=90;

    // Calculate the individual motor speeds
    Vw1 = rotSpeed + Vb * cosDegrees(angle);
    Vw2 = rotSpeed + Vb * (-0.6 * cosDegrees(angle) + 0.8 * sinDegrees(angle));
    Vw3 = (rotSpeed + Vb * (-0.6 * cosDegrees(angle) - 0.8 * sinDegrees(angle))) * -1;

    // This normalises all of the values to make sure
    // no motor value peaks over 100%
    if (Vw1 > MAXMOTORSPEED) {
        norm_factor = MAXMOTORSPEED / Vw1;
    } else if (Vw2 > MAXMOTORSPEED) {
        norm_factor = MAXMOTORSPEED / Vw2;
    } else if (Vw3 > MAXMOTORSPEED) {
        norm_factor = MAXMOTORSPEED / Vw3;
    } else {
        norm_factor = 1.0;
    }

    // Power the motors.
    motor[Thing1] = Vw1 * norm_factor;
    motor[Thing2] = Vw2 * norm_factor;
    motor[CatintheHat] = Vw3 * norm_factor;
}
// Use some trig to culcalate the required power for each motor
void MoveRobot(int angle, int Vb, int rotSpeed) {
  float Vw1, Vw2, Vw3, maxSpeed, norm_factor;
  int iVw1, iVw2, iVw3;

  // This is where the magic happens.  The actual formula is
  // Vw = rotSpeed + Vb * ((cosDegrees(wheelAngle) * cosDegrees(movementAngle)) + (sinDegrees(wheelAngle) * sinDegrees(movementAngle)))
  // I have optimised the formulae below to reduce the number of operations required to calculate the motor speeds
  // since the motorAngle value never changes and cancels some things out, further simplifying the calculations.
  // I like simple!

  Vw1 = rotSpeed + Vb * cosDegrees(angle);
  Vw2 = rotSpeed + Vb * (-0.5 * cosDegrees(angle) + 0.866025 * sinDegrees(angle));
  Vw3 = rotSpeed + Vb * (-0.5 * cosDegrees(angle) - 0.866025 * sinDegrees(angle));

  // This makes sure the motor values never exceed the maximum (MAXMOTORSPEED)
  maxSpeed = max3(abs(Vw1), abs(Vw2), abs(Vw3));

  norm_factor = (maxSpeed > MAXMOTORSPEED) ? (MAXMOTORSPEED / maxSpeed) : 1.0;

  iVw1 = round(Vw1 * norm_factor);
  iVw2 = round(Vw2 * norm_factor);
  iVw3 = round(Vw3 * norm_factor);

  motor[motorA] = iVw1;
  motor[motorB] = iVw2;
  motor[motorC] = iVw3;
}
예제 #7
0
파일: auto3.c 프로젝트: FTC7155/2014
void turn(int angle) { // use +90 turn right and -90 to turn left

	float compassTarget, compassOrigin; //used for turning and configuring compass
	float error;

	compassOrigin = (float) SensorValue(compass);
	compassTarget = (compassOrigin - angle); // in degrees

	if(compassTarget<0) //make sure the angle is between 0 and 360
		compassTarget += 360;

	if(compassTarget>360)
		compassTarget -= 360;

	error = atan2(sinDegrees(compassTarget)-sinDegrees(compassOrigin),cosDegrees(compassTarget)-cosDegrees(compassOrigin));
	motor[leftWheel]  =  sgn(angle)*(15*error+5);
	motor[rightWheel] = -sgn(angle)*(15*error+5);

	while(abs(error)<PI/90.0){
		motor[leftWheel]  =  sgn(angle)*(15*error+5);
		motor[rightWheel] = -sgn(angle)*(15*error+5);
		error = atan2(sinDegrees(compassTarget)-sinDegrees(compassOrigin),cosDegrees(compassTarget)-cosDegrees(compassOrigin));
	}

	motor[leftWheel] = 0;
	motor[rightWheel] =  0;
}
예제 #8
0
void GObject::Matrix2D::applyRotate(double theta) {
    // Counterintuitive sign weirdness
    // because positive y-axis points downward.
    double m00 = cosDegrees(theta) * m[0][0] - sinDegrees(theta) * m[0][1];
    double m01 = sinDegrees(theta) * m[0][0] + cosDegrees(theta) * m[0][1];
    double m10 = cosDegrees(theta) * m[1][0] - sinDegrees(theta) * m[1][1];
    double m11 = sinDegrees(theta) * m[1][0] + cosDegrees(theta) * m[1][1];
    m[0][0] = m00;
    m[0][1] = m01;
    m[1][0] = m10;
    m[1][1] = m11;
}
예제 #9
0
void move(int dist)
{
	int distance = abs(dist);
	if(dist>0)
	{
		motor[LFW] = 127;
		motor[LRW] = 127;
		motor[RFW] = 127;
		motor[RRW] = 127;
	}
	else
	{
		motor[LFW] = -127;
		motor[LRW] = -127;
		motor[RFW] = -127;
		motor[RRW] = -127;
	}
	nMotorEncoder[LFW] = 0;
	while(abs(nMotorEncoder[LFW])/627.2<distance/(4.0*PI))
	{
		wait1Msec(10);
	}
	locX+=cosDegrees(rotation)*distance;
	locY+=sinDegrees(rotation)*distance;
	freeze();
}
예제 #10
0
void runMotorSpeeds(int &motorSpeedD, int &motorSpeedE, int &motorSpeedF, int &motorSpeedG, int angle, int Vb) {
  float Vw1, Vw2, Vw3, Vw4, norm_factor;

  Vw1 = Vb*cosDegrees(angle);
  Vw2 = Vb*sinDegrees(angle);
  Vw3 = -Vw1;
  Vw4 = -Vw2;

  norm_factor = 1.0;

  if (Vw1 > MAXMOTORSPEED) {
    norm_factor = MAXMOTORSPEED / Vw1;
  } else if (Vw2 > MAXMOTORSPEED) {
    norm_factor = MAXMOTORSPEED / Vw2;
  } else if (Vw3 > MAXMOTORSPEED) {
    norm_factor = MAXMOTORSPEED / Vw3;
  } else if (Vw4 > MAXMOTORSPEED) {
    norm_factor = MAXMOTORSPEED / Vw4;
  }

  motorSpeedD = roundit(Vw1 * norm_factor);
  motorSpeedE = roundit(Vw2 * norm_factor);
  motorSpeedF = roundit(Vw3 * norm_factor);
  motorSpeedG = roundit(Vw4 * norm_factor);

  motor[motorD] = motorSpeedD;
  motor[motorE] = motorSpeedE;
  motor[motorF] = motorSpeedF;
  motor[motorG] = motorSpeedG;
}
예제 #11
0
파일: MCL.c 프로젝트: xdanx/dandroid
float calculate_likelihood(float x, float y, float theta, float z)
{

	int closestWallIndex = -1;
	// We take into consideration the greatest distance
	float shortestDistance = 20000.0;
	float m;
	int i = 0;

	for (i = 0; i < NUMBER_OF_WALLS; i++)
	{
		float a_x = wallAxArray[i], a_y = wallAyArray[i];
		float b_x = wallBxArray[i], b_y = wallByArray[i];
		m = ((b_y - a_y)*(a_x - x) - (b_x - a_x)*(a_y - y)) / ((b_y - a_y)*cosDegrees(theta) - (b_x - a_x)*sinDegrees(theta));

		float px = x + m*cosDegrees(theta);
		float py = y + m*sinDegrees(theta);

		if( px >= min(a_x,b_x) && px <= max(a_x,b_x) && py >= min(a_y,b_y) && py <= max(a_y,b_y) && m >= 0)
		{
			if ( m <= shortestDistance )
			{
				shortestDistance = m;
				closestWallIndex = i;
			}
		}
	}
	float likelihood = exp(-pow(z - shortestDistance, 2) * 1.0 / (2 * pow(0.44, 2))) + 0.1;
	return likelihood;

}
예제 #12
0
void calc_koordinaten_Drive()
{
	int preHitpositionX;
	int preHitpositionY;
	int DegreeTemp = _degree_Hit /10;



	if(_degree_Hit >= 0)
	{
		preHitpositionX = 0;
		preHitpositionY = 0;
	}
	else
	{
		DegreeTemp = _degree_Hit / -10;

		preHitpositionX = (int)(50.0 * (sinDegrees(DegreeTemp)));
		preHitpositionY = (int)(50.0 * (cosDegrees(DegreeTemp)));
	}

	_distance_X2Drive = _distance_X2Ball + preHitpositionX;
	_distance_Y2Drive = _distance_Y2Ball - preHitpositionY; //da nicht so weit zum Ball zu fahren ist

	nxtDisplayStringAt(0, 8, "DX:%02d DY:%02d  ",_distance_X2Drive,_distance_Y2Drive);

}
예제 #13
0
파일: b4 rat.c 프로젝트: mjs513/rsnxt08
//----Shifts the activity in the pose structure----//
void pathIntegrateCell(char xp, char yp, char thetap, float deltaTheta, float translation)
{
  //initialise loop variables
	char relativeX;
	char relativeY;
	char relativeTheta;

	char x;
	char y;
	char theta;

	float deltaPoseX = (cosDegrees(currentDirection) * translation) / 0.5;/// (lengthX / sizeX);
  float deltaPoseY = (sinDegrees(currentDirection) * translation) / 0.5; //(lengthX / sizeX);
	float deltaPoseTheta = deltaTheta / 60;//(360 / sizeTheta);

  int intOffsetX = (int) deltaPoseX; //only a whole number of cells moved
  int intOffsetY = (int) deltaPoseY;
  int intOffsetTheta = (int) deltaPoseTheta;

  getActivationDistribution(deltaPoseX - intOffsetX, deltaPoseY - intOffsetY, deltaPoseTheta - intOffsetTheta);

  for(relativeX = 0; relativeX < 2; relativeX++)
  {
  	x = getWrappedX(xp + intOffsetX + relativeX);
    for(relativeY = 0; relativeY < 2; relativeY++)
    {
      y = getWrappedY(yp + intOffsetY + relativeY);
    	for(relativeTheta = 0; relativeTheta < 2; relativeTheta++)
    	{
    	  theta = getWrappedTheta(thetap + intOffsetTheta + relativeTheta);
    	  tempPose[x].array2D[y][theta] += distribution[relativeX].array2D[relativeY][relativeTheta] * poseWorld.poseActivity[xp].array2D[yp][thetap];
    	}
    }
  }
}
예제 #14
0
파일: rvwPos.c 프로젝트: l4cuatro/lnSide
task posTrack() {
	static int timeLast,
		time,
		dt,
		thetaLast,
		gyroVal,
		lEnc,
		rEnc;

	while(1) {
		thetaLast = gyroVal;
		timeLast = time;
		gyroVal = SensorValue[gyro];
		time = nSysTime;
		dt = time - timeLast;
		lEnc = SensorValue[lEnc];
		rEnc = SensorValue[rEnc];

		upAvg(&driveVelAvg,
			avg(diff(&lDriveDiff, lEnc, dt),
				diff(&rDriveDiff, rEnc, dt)
			)
		);

		if((fabs(gyroVal - thetaLast) / dt) > SensorBias[gyro])
			theta += (gyroVal - thetaLast);
		x += driveVelAvg.mean * dt * sinDegrees(theta);
		y += driveVelAvg.mean * dt * cosDegrees(theta);
	}
}
예제 #15
0
파일: MCL.c 프로젝트: xdanx/dandroid
void points_update(float value, int state) {
	// Value is distance in cm when state is MOVE_STATE
	//and degrees when state is ROTATE_STATE
	int i;
	float e, f;

	switch (state) {
		case MOVE_STATE:
			for(i=0; i<NUMBER_OF_PARTICLES; i++) {
				e = sampleGaussian(0.0, 0.881);
				f = sampleGaussian(0.0, 0.881);
				xArray[i] = xArray[i] + (value + e) * cosDegrees(thetaArray[i]);
				yArray[i] = yArray[i] + (value + e) * sinDegrees(thetaArray[i]);
				thetaArray[i] = thetaArray[i] + f;
			}
			break;
		case ROTATE_STATE:
			for( i=0; i<NUMBER_OF_PARTICLES; i++) {

				e = sampleGaussian(0.0, 0.881);
				thetaArray[i] = normalize_angle_value(thetaArray[i] + value + e);
			}
			break;
	}
}
예제 #16
0
파일: MCL.c 프로젝트: xdanx/dandroid
void print_10_points()
{
	int i;
	for (i=0; i<10; ++i)
		writeDebugStream("x:%f y:%f theta:%f cos:%f, sin: %f\n",xArray[i], yArray[i], thetaArray[i],
																	cosDegrees(thetaArray[i]), sinDegrees(thetaArray[i]));
	writeDebugStream("-------------\n");
}
예제 #17
0
void calc_koordinaten_Ball(int Degree, unsigned int Distance)
{
	Degree /= 10;
	if(Degree >=0)
	{
		_distance_Y2Ball = (int)((float)Distance * (cosDegrees(Degree)));
		_distance_X2Ball = (int)((float)Distance * (sinDegrees(Degree)));
	}
	else
	{
		Degree *= -1;

		_distance_Y2Ball = (int)((float)Distance * (cosDegrees(Degree)));
		_distance_X2Ball = (int)((float)Distance * (sinDegrees(Degree)));
		_distance_X2Ball *= -1;
	}

	nxtDisplayStringAt(0, 16, "X:%02d Y:%02d  ",_distance_X2Ball,_distance_Y2Ball);
}
예제 #18
0
void Backward40cm(){
  motor[rightWheel] = ForwardSpeed*-1;
  motor[leftWheel] = ForwardSpeed*-1;
  int i;
  for ( i = 0 ; i < StraightTime ; i+=timeUnit) {
    nxtSetPixel( x , y );
    x -= cosDegrees(angle);
    y -= sinDegrees(angle);
    //wait1Msec(timeUnit);
  }
}
예제 #19
0
void Forward40cm(){
  motor[rightWheel] = ForwardSpeed;
  motor[leftWheel] = ForwardSpeed;
  int i;
  for (i = 0; i < StraightTime; i += timeUnit) {
    nxtSetPixel( x , y );
    x += cosDegrees(angle);
    y += sinDegrees(angle);
    wait1Msec(timeUnit);
  }
}
예제 #20
0
/**
 * This displays an arrow on the screen pointing downwards.
 * @param degreesFromDown the number of degrees from down
 */
void displayArrow(int degreesFromDown)
{
   eraseDisplay();
   // Otherwise, the arrow would point up.
   degreesFromDown = degreesFromDown-180;

   //If you don't know trigonometry, you can ignore this part
   nxtDrawLine(49,
               31,
               (cosDegrees(degreesFromDown     ) * 20) + 49,
               (sinDegrees(degreesFromDown     ) * 20) + 31);
   nxtDrawLine((cosDegrees(degreesFromDown - 20) * 15) + 49,
               (sinDegrees(degreesFromDown - 20) * 15) + 31,
               (cosDegrees(degreesFromDown     ) * 20) + 49,
               (sinDegrees(degreesFromDown     ) * 20) + 31);
   nxtDrawLine((cosDegrees(degreesFromDown + 20) * 15) + 49,
               (sinDegrees(degreesFromDown + 20) * 15) + 31,
               (cosDegrees(degreesFromDown     ) * 20) + 49,
               (sinDegrees(degreesFromDown     ) * 20) + 31);
}
float calcPosY(Angles angles){
	float a = angles.leftAngle;
	float b = angles.rightAngle;
	if(a > 90){
		float aPrime = 180 - a;
		return (dist*sinDegrees(a)*aPrime)/sinDegrees(180-a-b);
	}
	else if(b > 90){
		float bPrime = 180-b;
		float A = ((dist*sinDegrees(a))/sinDegrees(180-a-b));
		return A*sinDegrees(bPrime);
	}
	else if(a == 90){
		return (dist*sinDegrees(b))/sinDegrees(180-a-b);
	}
	else
	{
		return (dist*sinDegrees(a))/sinDegrees(180-a-b);

	}
}
예제 #22
0
task updateHUD () {
  int x = 0;
  int y = 0;

  while (true) {
    nxtEraseRect(4,50, 44,10);
    nxtDisplayTextLine(2, "        H: %3d", angleI/100);
    nxtDisplayTextLine(3, "        X: %3d", x_accel/100);
    nxtDisplayTextLine(4, "        Y: %3d", y_accel/100);
    nxtDisplayTextLine(5, "        Z: %3d", z_accel/100);
    nxtDrawCircle(84, 50, 4);

    nxtDrawCircle(4, 50, 40);
    x = (cosDegrees(-1 * (angleI/100 - 90)) * 20) + 24;
    y = (sinDegrees(-1 * (angleI/100 - 90)) * 20) + 30;
    nxtDrawLine(24, 30, x, y);
    nxtEraseRect(0,0, 99, 8);
    nxtDrawRect(0,0, 99, 8);
    nxtFillRect(50,0, (float)(rotI / 150)/100.0 *50 + 50, 8);
    wait1Msec(100);
  }
}
예제 #23
0
void stopAll(){
	if(move){
		float rot = getCompassValue();

		float x = sinDegrees(rot) * encoder;
		float y = cosDegrees(rot) * encoder;

		nxtDisplayTextLine(0, "%d  %d %d", rot, x, y);
		nxtDisplayTextLine(1, "%d", encoder);

		totalX+=x;
		totalY+=y;

		nMotorEncoder[motorB] = 0;
		move = false;
	}

	stopBack();
	stopF();
	stopLeft();
	stopRight();
	motor[clawm] = 0;
}
예제 #24
0
파일: MCL.c 프로젝트: xdanx/dandroid
void position_add_distance(Position* p, float distance) {
	p->x += distance * cosDegrees(p->angle);
	p->y += distance * sinDegrees(p->angle);
	return;
}
예제 #25
0
파일: b4 rat.c 프로젝트: mjs513/rsnxt08
//----Corrects the Experience Map----//
//So far have used floats but as all structs use ints i may be able to get away with not using floats at all. Yay to the memory savings
void mapCorrection()
{
	float mapCorrectionXY = mapCorrectionRateXY * 0.5;
	float mapCorrectionTheta = mapCorrectionRateTheta * 0.5;

	experience startExperience;
	experience endExperience;
	vector3D startPose;
	vector3D endPose;
	experienceLink link;

	char z; //for loop
	for(z = 0; z <(nextID-1); z++)
	{
		memcpy(startExperience,Map.experienceMap[z],110); //copy experience being manipulated into startExperience
		memcpy(startPose, startExperience.mapPose, 6); //copy mapPose being manipulated into startPose
    char y; //for loop
    for(y = 0; y < numOfLinksPerExperience; y++)
    {
      if(startExperience.outLinks[y] != -1)
      {
        memcpy(link,links[startExperience.outLinks[y]],12);
        memcpy(endExperience,Map.experienceMap[link.endExperienceID],110);
        memcpy(endPose, endExperience.mapPose, 6);

        //expected position of the end experience
        float angleToTargetEnd = startPose.theta + link.translationAngle;
        float targetEndX = startPose.x + link.translationDistance * cosDegrees(angleToTargetEnd);
        float targetEndY = startPose.y + link.translationDistance * sinDegrees(angleToTargetEnd);

        //expected orientation of the end experience
        float targetEndAngle = startPose.theta + link.rotation;

        //Calulate the 'error' between expected and actual position of end experience
        float xError = targetEndX - endPose.x;
        float yError = targetEndY - endPose.y;
        float thetaError = getRotationDegrees(endPose.theta,targetEndAngle);

        //calculate the adjustment to be made for start and end poses
        float xAdjustment = xError * mapCorrectionXY;
        float yAdjustment = yError * mapCorrectionXY;
        float thetaAdjustment = thetaError * mapCorrectionTheta;

        //Apply adjustments then copy back over previous experiences
        startPose.x -= xAdjustment;
        startPose.y -= yAdjustment;
        startPose.theta = wrappedDegrees360(startPose.theta - thetaAdjustment);

        endPose.x += xAdjustment;
        endPose.y += yAdjustment;
        endPose.theta = wrappedDegrees360(endPose.theta + thetaAdjustment);

        memcpy(startExperience.mapPose, startPose, 6);
        memcpy(Map.experienceMap[z], startExperience, 110);

        memcpy(endExperience.mapPose, endPose, 6);
        memcpy(Map.experienceMap[link.endExperienceID],endExperience,110);
      }
      else {break;} //leave loop faster as there are no more links
    }
  }
}
예제 #26
0
파일: MCL.c 프로젝트: xdanx/dandroid
void navigate_to_waypoint(float x, float y)
{
	float med_x = 0, med_y = 0, med_theta = 0;
	float i, z;

	if (DEBUG)
	{
		writeDebugStream("Going towards point %f, %f\n",x,y);
		print_10_points();
		print_10_cwa();
	}

	float cosSum = 0, sinSum = 0;
	// estimate current posistion
	for (i=0; i < NUMBER_OF_PARTICLES; ++i)
	{
		med_x += xArray[i]*weightArray[i];
		med_y += yArray[i]*weightArray[i];
		//med_theta += thetaArray[i] * weightArray[i];

		float theta = thetaArray[i];
		cosSum += cosDegrees(theta) * weightArray[i];
		sinSum += sinDegrees(theta) *weightArray[i];
	}

	// Transform back from vector to angle
	if (cosSum > 0)
		med_theta = atan (sinSum / cosSum);
	else if (sinSum >=0 && cosSum < 0)
		med_theta = atan(sinSum / cosSum) + PI;
	else if (sinSum < 0 && cosSum < 0)
		med_theta = atan (sinSum / cosSum) - PI;
	else if (sinSum > 0 && cosSum == 0)
		med_theta = PI;
	else if (sinSum < 0 && cosSum == 0)
		med_theta = -PI;
	else
		med_theta = 0;

	if (DEBUG)
		writeDebugStream("Averages: x: %f y:%f theta:%f\n", med_x, med_y, med_theta);

	// calculate difference
	float dif_x = x - med_x; // dest - curr_pos
	float dif_y = y - med_y;

	// Setting the dif_? thresholds
	if ( abs(dif_x) < 0.01 )
		dif_x = 0.0;
	if ( abs(dif_y) < 0.01 )
		dif_y = 0.0;

	writeDebugStream("Dif_x: %f, dif_y: %f\n",dif_x, dif_y);
	float rotate_degs;
	// get the nr of degrees we want to turn. But in which direction ?!

	// Use of atan2
	if ( dif_x > 0)
		rotate_degs = atan (dif_y / dif_x);
	else if (dif_y >=0 && dif_x < 0)
		rotate_degs = atan( dif_y / dif_x) + PI;
	else if (dif_y < 0 && dif_x < 0)
		rotate_degs = atan (dif_y / dif_x) - PI;
	else if (dif_y > 0 && dif_x == 0)
		rotate_degs = PI;
	else if (dif_y < 0 && dif_x == 0)
		rotate_degs = -PI;
	else
		rotate_degs = 0;

	rotate_degs = rotate_degs * 180.0 / PI;
	rotate_degs = normalize_angle_value(rotate_degs - med_theta);
	writeDebugStream("Rotate angle: %f\n",rotate_degs);

	// Print 10 points before rotation
	if (DEBUG)
		print_10_points();
	// Rotate towards the correct position
	rotate(rotate_degs);
	points_update(rotate_degs, ROTATE_STATE);
	// Print 10 points after rotation , to make sure that we rotate correctly
	if (DEBUG)
		print_10_points();

	// move forward
	float move_distance = sqrt(dif_x * dif_x + dif_y * dif_y);
	if (DEBUG)
		writeDebugStream("Moving distance: %f\n", move_distance);
	move_forward(MOVE_POWER, move_distance);

	// Update the position to the new points
	points_update(move_distance, MOVE_STATE);

	//wait1Msec(2000);
	PlaySound(soundBeepBeep);

	// Sonar measurement!
	z = SensorValue[sonar];
	for ( i = 0; i < NUMBER_OF_PARTICLES; i++ )
	{
		weightArray[i] = calculate_likelihood(xArray[i], yArray[i], thetaArray[i], z);
	}

	// Resampling after calculating likelihood
	resample();
	if (DEBUG)
	{
		writeDebugStream("After Resampling\n");
		print_10_cwa
		();
	}

}
예제 #27
0
파일: gobjects.c 프로젝트: kzidane/spl
void addPolarEdge(GPolygon poly, double r, double theta) {
   addEdge(poly, r * cosDegrees(theta), -r * sinDegrees(theta));
}
예제 #28
0
void edgeToPoint(Edge & e, Point & p) {
	p.x = e.length * sinDegrees(e.angle);
	p.y = e.length * cosDegrees(e.angle);
}
float tan(float theta)
{
  return sinDegrees(theta)/cosDegrees(theta);
}
float LightSensorToDistanceOut(int count){ //returns distance in inches
	int cntdiff = C_SVLIGHTSENSORIN-count;
	float degrees = (180.0*cntdiff)/255.0;
	float dist = sinDegrees(degrees)*6.5;
	return dist;
}