All the lab works are here!
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 48:cd3463dcca04
- Parent:
- 47:f0fe58571e4a
- Child:
- 49:d61da2bc8882
--- a/main.cpp Tue May 30 15:17:19 2017 +0000 +++ b/main.cpp Tue May 30 15:51:38 2017 +0000 @@ -354,7 +354,14 @@ float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition; anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2)); - + + if(alphaBeforeAdjustment>pi) + alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi; + if(alphaBeforeAdjustment<-pi) + alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi; + + //float anglePointToSonar2=atan2(y-ys,x-xs)-theta; + //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS //check if absolute difference between the angles is no more than Omega/2 if( distancePointToSonar < (RANGE_SONAR)&& (anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= rad_angle_check(-ANGLE_SONAR/2))){ @@ -707,7 +714,7 @@ float angularLeft=0; go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c); - pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c); + //pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c); leftMotor(sign2(angularLeft),abs(angularLeft)); rightMotor(sign2(angularRight),abs(angularRight)); @@ -779,7 +786,7 @@ bool inFront=true; //atan2 gives the angle beetween pi and -pi float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap); - pc.printf("angleToTarget=%f",angleToTarget); + //pc.printf("angleToTarget=%f",angleToTarget); if(angleToTarget < 0)//the target is in front inFront=false;