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;
}
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);
}
}
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);
}
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;
}
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;
}
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;
}
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();
}
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;
}
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;
}
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);
}
//----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];
}
}
}
}
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);
}
}
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;
}
}
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");
}
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);
}
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);
}
}
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);
}
}
/**
* 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);
}
}
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);
}
}
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;
}
void position_add_distance(Position* p, float distance) {
p->x += distance * cosDegrees(p->angle);
p->y += distance * sinDegrees(p->angle);
return;
}
//----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
}
}
}
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
();
}
}
void addPolarEdge(GPolygon poly, double r, double theta) {
addEdge(poly, r * cosDegrees(theta), -r * sinDegrees(theta));
}
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;
}
