MFloatPointArray ropeGenerator::createHalfRope( int pointsCount, float radius ) { MFloatPointArray points; MPoint baseVector( 1,0,0 ); baseVector = baseVector * radius; points.append( MFloatPoint( baseVector.x, baseVector.y, baseVector.z, 1.0 ) ); float fbaseAngle = 180.0 / float( pointsCount ); for (int d = 1; d < pointsCount; d++) { if (d == 1) { MAngle baseAngle((fbaseAngle * 0.25), MAngle::kDegrees ); MVector vVector( baseVector ); vVector = vVector.rotateBy( MVector::kYaxis, baseAngle.asRadians() ); points.append( MFloatPoint( vVector.x, vVector.y, vVector.z, 1.0 ) ); } MAngle baseAngle((fbaseAngle * float( d ) ), MAngle::kDegrees ); MVector vVector( baseVector ); vVector = vVector.rotateBy( MVector::kYaxis, baseAngle.asRadians() ); points.append( MFloatPoint( vVector.x, vVector.y, vVector.z, 1.0 ) ); if ( d == pointsCount - 1 ) { MAngle baseAngle((fbaseAngle * ( d + 0.75 )), MAngle::kDegrees ); MVector vVector( baseVector ); vVector = vVector.rotateBy( MVector::kYaxis, baseAngle.asRadians() ); points.append( MFloatPoint( vVector.x, vVector.y, vVector.z, 1.0 ) ); } } return points; }
void FPSTask(void* ignore) { /** x coordinate of the center of the goal */ int netx=72; /** y coordinate of the center of the goal */ int nety=72; /** checks what side we are starting on */ bool lside=leftSide(); /** sets the starting position of the robot*/ position=(Location){0,0};//determineXInit(lside), determineYInit()}; /** sets the starting position of the net*/ Location net=netPos(lside, netx, nety); /** variable that increments whenever we move a linear distance */ int movement=0; /** variable that keeps track of what the encoder value was a certain number of iterations prior */ int previousEncoder=0; /** variable that sets the minimum amount the encoder value needs to change for us to count it as a definite movement */ int thresEncoder=5; /** variable that sets the minimum amount the gyroscope value needs to change for us to count it as a definite turn */ int thresGyro=5; /** variable that keeps track of what the gyroscope value was a certain number of iterations prior */ int previousGyro=0; /** variable that keeps track of how many times the main code has looped */ int iteration=0; /** variable for the angle between the robot and the net */ int angle=0; /** variable that sets the number of iterations we let pass before checking for movement */ int refreshrate=10; while(1) { //printf("In FPSTask\n\r"); //FPSBase.correction.localAngle = getLocalAngle(gyroscope); /** increments the iteration variable */ iteration++; /** checks if a certain number of iterations has passed to check for a change */ if (iteration%refreshrate==0){ printf("FPS 1\n\r"); printf("1[chX:%4d, chY:%4d]\n\r", FPSBase.direction.chX, FPSBase.direction.chY); //printf("axisTurn: %d\n\r", (int)axisTurn); /** checks if we have not turned and/or are not turning since the last check for movement */ //if (abs(baseAngle(FPSBase)-previousGyro)<=thresGyro){ if (FPSBase.axis != axisTurn){ /** checks if we have moved a linear distance since the last check for movement */ //if (abs(encoderValue()-previousEncoder)>thresEncoder){ //if (FPSBase.axis != none){ if (true){ /** increments the movement value by the difference between the current encoder value and the encoder value from the previous check for movement */ movement+=encoderValue()-previousEncoder; /** changes the x coordinate of the robot by the movement in the x direction */ position.x+=disMoved(movement)*cos(baseAngle(FPSBase)); /** changes the y coordinate of the robot by the movement in the y direction */ position.y+=disMoved(movement)*sin(baseAngle(FPSBase)); /** sets the current value of the encoder to the previous encoder value*/ previousEncoder=encoderValue(); printf("encoders:[left:%6d, right:%6d]\n\r", encoderGet(encoderBaseLeft), encoderGet(encoderBaseRight)); printf("2[chX:%4d, chY:%4d]\n\r", FPSBase.direction.chX, FPSBase.direction.chY); printf("movement:%6d, X:%4d, Y:%4d\n\r", movement, position.x, position.y); } } else{ /** sets the change in linear movement to 9 */ movement=0; previousEncoder=encoderValue(); } /** sets the current value of the gyroscope to the previous gyroscope value*/ previousGyro=baseAngle(FPSBase); } /** sets the angle variable to the angle between the current position of the robot and the net */ angle=shooterAngle_Xaxis(position, distance(position, net), net); /** checks if the 7L or 7R buttons on the controller have been pressed; one or the other, not both */ if ((joystickGetDigital(1,7,JOY_LEFT)) ^ (joystickGetDigital(1,7,JOY_RIGHT))){ /** if the 7L button has been pressed, the position is reset by the ultrasonic sensors (left and back) to minimize error */ if ((joystickGetDigital(1,7,JOY_LEFT))){ position.x=getSonarLeft(); position.y=getSonarBack(); } /** if the 7R button has been pressed, the position is reset by the ultrasonic sensors (right and back) to minimize error */ else{ position.x=getSonarRight(); position.y=getSonarBack(); } } printf("\n\r"); wait(TIME_DELAY); } }