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);
}
}
