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; }
task MotorControl() { //SpeedA = (MaxSpeed-abs(Spin))*cosDegrees(Direction-30); //SpeedB = (MaxSpeed-abs(Spin))*cosDegrees(Direction-150); //SpeedC = (MaxSpeed-abs(Spin))*cosDegrees(Direction+90); while (Run ==1) { SpeedA = (MaxSpeed-abs(Spin))*cosDegrees(Direction-150)*StopMove; SpeedB = (MaxSpeed-abs(Spin))*cosDegrees(Direction+90)*StopMove; SpeedC = (MaxSpeed-abs(Spin))*cosDegrees(Direction-30)*StopMove; SpeedA = SpeedA+Spin; SpeedB = SpeedB+Spin; SpeedC = SpeedC+Spin; motor[A] = SpeedA; motor[B] = SpeedB; motor[C] = SpeedC; if (SensorValue[Touch] ==1) { Run =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 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; }
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 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); }
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; }
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 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 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; }
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); } }
int findLine()//Sweeps the servo-mounted light sensor to find a value that is 40 points from the absolute value of the initial reading. { servo[servo2] = 0; int angle = 0; int initValue = 0; int raw = 0; bool isFinished = false; int i = 127; LSsetActive(LightSensor); wait10Msec(100); initValue = LSvalRaw(LightSensor); nxtDisplayBigTextLine(2, "%d", initValue); raw = initValue; for(i = 0; abs(raw-initValue)<=40 && i < 255; i++) { raw = LSvalRaw(LightSensor); nxtDisplayBigTextLine(4, "%d", raw); if(abs(raw - initValue)> 4) { isFinished = true; } wait10Msec(1); servo[servo2] = i; } angle = i * 180/255; int distance = 40*cosDegrees(angle); return distance;//The distance tells us how far we need to move the V-Bucket(Trademark Pending) in order to score autonomously. };
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 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 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); }
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 edgeToPoint(Edge & e, Point & p) { p.x = e.length * sinDegrees(e.angle); p.y = e.length * cosDegrees(e.angle); }
void position_add_distance(Position* p, float distance) { p->x += distance * cosDegrees(p->angle); p->y += distance * sinDegrees(p->angle); return; }
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 (); } }
float tan(float theta) { return sinDegrees(theta)/cosDegrees(theta); }
//=========================================================================================== // Main program - handle joystick inputs and take appropriate actions //=========================================================================================== task main() { int x1, y1; float hypoteneuse; bool abortProgram = false; initializeRobot(); // run all initialisations waitForStart(); // then wait for FCS to tell us to go while(!abortProgram) // handle joystick inputs until the drive team aborts program // drve team can abort program by pressing both "9" buttons at the same time { // run first read of joystick to verify connectivity if(joystick.joy1_TopHat == -1) Joy1Enabled = true; // enable joystick only if tophat responds with -1 // otherwise disable that joystick if(joystick.joy2_TopHat == -1) Joy2Enabled = true; // same test for joystick 2 if(joy1Btn(10) && joy2Btn(10)) abortProgram = true; // handle the abort program condition //================================ getJoystickSettings(joystick); // get joystick data packet x1 = (joystick.joy1_x1*100)/127; // extract X value for joystick one ranged to percentage y1 = (joystick.joy1_y1*100)/127; // extract Y value for joystick one range to percentage hypoteneuse=(float)sqrt((x1*x1)+(y1*y1)); // calculate third side of effective triangle //--------------------- if(joy1Btn(5)) hypoteneuse = hypoteneuse; // max speed only if button 5 is pressed else if(joy1Btn(7)) hypoteneuse = hypoteneuse/(float)5; // button seven gives slow slow speed else hypoteneuse = hypoteneuse/(float)2; // otherwise default is half speed //---------------------thisone if ((abs(x1)<DEADBAND) && (abs(y1)<DEADBAND)) // if joystick is effectively in zero position { drive_speed=0; // then set speed and effective angle to zero drive_angle=0; // } else // joystick is active, so calculate motor power values { drive_angle=radiansToDegrees(atan2(x1,y1)); // calculate drive angle from X and Y joystick values drive_speed=hypoteneuse; // the speed is defined by the extent of the X-Y hypoteneuse } //============================================================== // Code to handle JOG capability using TOPHAT on either joystick //============================================================== if(Joy2Enabled == true && joystick.joy2_TopHat!=-1) // if enabled and pressed then we have a request { if (!Jog_In_Progress) // only accept new requests if one isn't already active { requested_angle = 45*joystick.joy2_TopHat; // calculate and save the requested angle Jog_In_Progress = true; // setup a jog in progress ClearTimer(T1); // and start the timer } if (Jog_In_Progress) // jog movements take precedence over joystick control { if (time1[T1]>JOGTIME) // then the jog is complete { Jog_In_Progress = false; // so disable jog control } else // the jog is still underway { drive_angle = (float)requested_angle; // so the jog angle takes precedence over joystick activity drive_speed = JOGSPEED; // and we also need to setup the default jog speed } } if (drive_speed>100) drive_speed = 100; // limit max speed to 100 percent } else { if(Joy1Enabled == true) { if ((joystick.joy1_TopHat!=-1) && (!Jog_In_Progress)) // we have a new request for a jog { requested_angle = 45*joystick.joy1_TopHat; // calculate and save the requested angle Jog_In_Progress = true; // setup a jog in progress ClearTimer(T1); // and start the timer } if (Jog_In_Progress) // jog movements take precedence over joystick control { if (time1[T1]>JOGTIME) // then the jog is complete { Jog_In_Progress = false; // so disable jog control } else // the jog is still underway { drive_angle = (float)requested_angle; // so the jog angle takes precedence over joystick activity drive_speed = JOGSPEED; // and we also need to setup the default jog speed } } if (drive_speed>100) drive_speed = 100; // limit max speed to 100 percent } } //================================================================================================ // now calculate individual motor speeds based on the motor angle versus the requested drive angle //================================================================================================ float gyro_angle=constHeading; // assume we'll be using field relative mode if(joy1Btn(11) != true) gyro_angle = 0; // otherwise configure robot relative drive LF_speed = cosDegrees(LF_angle-drive_angle+gyro_angle)*drive_speed; // individual motor speeds are calculated RF_speed = cosDegrees(RF_angle-drive_angle+gyro_angle)*drive_speed; // relative to the mounted angle of that LR_speed = cosDegrees(LR_angle-drive_angle+gyro_angle)*drive_speed; // motor, but adjusted for the current RR_speed = cosDegrees(RR_angle-drive_angle+gyro_angle)*drive_speed; // orientation of the robot per the GYRO //==================================================================== // Now handle request for rotation, either standalone or while driving //==================================================================== rotation_factor=0; // assume no rotation is requested int speed_limit=drive_speed; // this is used to determine maximum allowable speed if there is also rotation if (joy1Btn(3)) { if(joy1Btn(5)) rotation_factor=-RotationSpeedFast;// clockwise rotation else rotation_factor=-RotationSpeed; } if (joy1Btn(1)) { if(joy1Btn(5)) rotation_factor=RotationSpeedFast; // counter clockwise rotation else rotation_factor=RotationSpeed; } speed_limit-=abs(rotation_factor); // adjuist spped limit down to allow for rotation (if any) //================================================================================= // now prorate the power levels so the highest equals the requested power level //================================================================================= float highest = max(max(abs(LF_speed),abs(RF_speed)),max(abs(LR_speed),abs(RR_speed))); float adjust_factor=1; if (highest!=0) adjust_factor=speed_limit/highest; LF_speed*=adjust_factor; RF_speed*=adjust_factor; LR_speed*=adjust_factor; RR_speed*=adjust_factor; LF_speed+=rotation_factor; // add in the rotation factor to each drive motor RF_speed+=rotation_factor; LR_speed+=rotation_factor; RR_speed+=rotation_factor; //======================================================================= // finally operate the actual drive motors at the calculated power levels //======================================================================= motor[LF_motor]=(int)LF_speed; motor[RF_motor]=(int)RF_speed; motor[LR_motor]=(int)LR_speed; motor[RR_motor]=(int)RR_speed; //========================================= // ring lifter //========================================= if (abs(joystick.joy2_y1)>DEADBAND) // lift activity has been requested by gunner { if(joystick.joy2_y1>DEADBAND) // they want it to go UP { if(joy2Btn(11)) lifter_state = HIGH_LIFT; // choose between HIGH and LOW levels based on button else lifter_state=LOW_LIFT; } if(joystick.joy2_y1<-DEADBAND) // they want it to go DOWN { if(joy2Btn(11)) lifter_state=OVERRIDE_DOWN; // set override mode - ignore limit switches else lifter_state=MANUAL_DOWN; // otherwise enable down only till limit switch } } else if (abs(joystick.joy1_y2)>DEADBAND) // lift activity has been requested by driver { if(joystick.joy1_y2>DEADBAND) // they want it to go UP { if(joy1Btn(12)) lifter_state = HIGH_LIFT; else lifter_state=LOW_LIFT; } if(joystick.joy1_y2<-DEADBAND) // they want it to go DOWN { if(joy1Btn(12)) lifter_state=OVERRIDE_DOWN; else lifter_state=MANUAL_DOWN; } } else lifter_state = STOPPED; // if no activity then ensure motor is not running //============================= // Gyro reset //============================= if(joy1Btn(9)) // this feature allows the drive team to reset the field { // relative zero position in the event that there is enough constHeading = 0; // gyro noise that it gets messed up. RequestedScreen=S_BIG_NUMS; } //============================= // grabbers //============================= if(joy2Btn(4) || joy1Btn(4)) // set UP position (fully closed) { if(grabbers_state == grabbersMid || grabbersDown) { servoChangeRate[right_servo] = 10; servoChangeRate[left_servo] = 10; } else { servoChangeRate[right_servo] = 4; servoChangeRate[left_servo] = 4; } grabbers_state = grabbersUp; } if(joy2Btn(3) || joy1Btn(2)) // set MID position (release ring) { if(grabbers_state == grabbersDown || grabbers_state == grabbersMid) { servoChangeRate[right_servo] = 10; servoChangeRate[left_servo] = 10; } else { servoChangeRate[right_servo] = 4; servoChangeRate[left_servo] = 4; } grabbers_state = grabbersMid; } if(joy2Btn(2)) // set down position (safety zone) { if(grabbers_state == grabbersUp || grabbers_state == grabbersMid) { servoChangeRate[right_servo] = 4; servoChangeRate[left_servo] = 4; } else { servoChangeRate[right_servo] = 10; servoChangeRate[left_servo] = 10; } grabbers_state = grabbersDown; } if(joy2Btn(6) || joy1Btn(6)) // drive team wants grabbers more closed { if (!actioned1) // then this is a new request { grabbers_state = nudgeUp; // so tell the task to adjust position actioned1 = true; // and indicate that we handled the request } } else actioned1=false; // once buttons are released then we enable a new request if(joy2Btn(8) || joy1Btn(8)) // drive team wants grabbers more open { if (!actioned2) // then this is a new request { grabbers_state = nudgeDown; // so tell the task to adjust position actioned2=true; // and indicate that we handled the request } } else actioned2 = false; // once the buttons are released we enable a new request //if(abs(joystick.joy2_y2)>10) motor[lightStrip] = joystick.joy2_y2; //else motor[lightStrip] = 0; } }
float LightSensorToDistanceIn(int count){ //returns distance in inches int cntdiff = C_SVLIGHTSENSORIN-count; float degrees = (180.0*cntdiff)/255.0; float dist = cosDegrees(degrees)*6.5; return dist; }