 | Code: //*!!Sensor, S1, touchSensor, sensorTouch, , !!*//
//*!! !!*//
//*!!Start automatically generated configuration code. !!*//
const tSensors touchSensor = (tSensors) S1; //sensorTouch //*!!!!*//
//******************************************************************************//
// Motors & Sensors //
// //
// [I/O Port] [Name] [Type] [Description] //
// Port 1 touchsensor Touch Front mounted //
// Port A none Motor Right Motor //
// Port B none Motor Left Motor //
// //
//******************************************************************************//
#define navMoveTurn -100 // Mot. synchron
const float WheelDistance= 11.6 ; //the distance between the two wheels
const float wheelDiameter = 5.6;//diameter of the wheel
float TY = 120.0; //The Target Y coordinate
float TX = 70.0; // The Target X xoordinate
float CY = 0.0; // To hold the current Y coordinate
float CX = 0.0;// To hold the current X coordinate
float wheelCircumference; // To hold the wheel circumference
float axisCircumference; // To hold the axis circumference
float cmPerTick; // How many cm the robot travels per rotation tick
float distanceReturned;
float degreesToTurn;
float ticksMoved; //to show at how many ticks the obstacle was encountered
int length; //to store the length of the straight line (Start, Target)
/*these variables store the leave and hit point of the obstacle encountered*/
float obstacleXStart;
float obstacleYStart;
float obstacleXEnd;
float obstacleYEnd;
/*received a distance in centimetres
**and returns the dstance in rotating ticks*/
float calculateRotating( float i){
float distance;
distance = i / cmPerTick;
return distance;
}
/*computes the straight distances
** from the robots current position to T*/
float computeST(float i, float x) {
float length = (i * i) + (x * x);
length = sqrt(length);
return length;
}
/*function to switch of the motors*/
void haltMotors(){
motor[motorA] = 0;
motor[motorB] = 0;
nMotorEncoder[motorA] = 0;
nMotorEncoder[motorB] = 0;
}
/*****************************************************************************************************/
/*function to calculate the circumference of the robot wheels*/
void calWheelCircumference(){
wheelCircumference = wheelDiameter * (float)PI;
}
/*function to calculate the circumference of the circle of the whole robot*/
void calAxisCircumference(){
axisCircumference = WheelDistance * (float)PI;
}
/*function to calculate the distance covered per tick rotation*/
void calCMPerTick(){
cmPerTick = wheelCircumference / 360;
}
/** A function to turn a specified number of degrees
* @param angle Angle to rotate in degrees.
* A positive value rotates left, a negative value right.
*/
void rotate(float angle) {
float axisCMPerDegree = axisCircumference / 360;
float distanceToTravel = axisCMPerDegree * angle;
float ticks = distanceToTravel / cmPerTick;
haltMotors();
if (angle > 0){
nSyncedMotors = synchAB;
nSyncedTurnRatio=-100;
if (angle > 85.0) {
motor[motorA] = 20;
}
else if (angle < 85.0) {
motor[motorA] = 13;
}
while (nMotorEncoder[motorA] < ticks) {}
}
if (angle < 0) {
nxtDisplayTextLine(2,"ticks %f",ticks);
// wait1Msec(5000);
nSyncedMotors = synchBA;
nSyncedTurnRatio = -100;
if (angle * -1 > 85.0) {
motor[motorB] = 20;
}
else if (angle * -1 < 85.0) {
motor[motorB] = 13;
}
while(nMotorEncoder[motorB] < ticks * -1) {nxtDisplayTextLine(2,"angle %f",angle);}
}
nSyncedMotors = synchNone;
nSyncedTurnRatio = 0;
nxtDisplayTextLine(1," ");
haltMotors();
}
void moveBackwards(float distance) {
haltMotors();
nSyncedMotors = synchAB;
nSyncedTurnRatio = +100;
motor[motorA] = -20;
while(nMotorEncoder[motorA] > distance * -1){}
nSyncedMotors = synchNone;
haltMotors();
}
/*function to move straight a number of rotatings*/
int moveStraight(float distance) {
int i;
haltMotors();
nSyncedMotors = synchAB;
nSyncedTurnRatio = +100;
motor[motorA] = 20;
while(nMotorEncoder[motorA] < distance && SensorValue(touchSensor) == 0) {
nxtDisplayTextLine(1,"DR %f", distanceReturned);
}
/*if an obstacle is touched before we have reached the target*/
if (SensorValue(touchSensor) == 1 && nMotorEncoder[motorA] < distance) {
motor[motorA] = 0;
motor[motorB] = 0;
ticksMoved = nMotorEncoder[motorA];
i = 1;
}
/*if the target is reached and no obstacles have been encountered*/
else if ( nMotorEncoder[motorA] == distance || nMotorEncoder[motorA] > distance ) {
motor[motorA] = 0;
motor[motorB] = 0;
ticksMoved = nMotorEncoder[motorA];
i = 0;
}
nSyncedMotors = synchNone;
haltMotors();
return i;
}
/*Target Reachability Test*/
int exchaustObstacle() {
int i;
rotate(degreesToTurn * -1);
i = 1;
degreesToTurn = 0;
moveBackwards(20 / cmPerTick);
CY += 20.0;
while (i == 1) {
rotate(90);
i = moveStraight(10 / cmPerTick);
if (i == 1) {
}
if (i == 0) {
CX -= 10.0;
rotate(-90.0);
i = moveStraight(22 / cmPerTick);
if (i == 1) {
moveBackwards(ticksMoved);
}
if (i == 0) {
CY -= 22.0;
}
}
}//first while i == 1
if (TY > obstacleYStart && TY < CY) {
if (CX == TX) {
}
else if (CX > TX) {
}
else if (CX < TX) {// it the target is right
nxtDisplayTextLine(1,"in 2 if");
wait1Msec(5000);wait1Msec(5000);
i = 1;
while (i == 1 && CY > obstacleYStart) {
rotate(-90.0);
i = moveStraight((TX - CX) / cmPerTick);
if (i == 1) {
moveBackwards(ticksMoved);
rotate(90.0);
i = moveStraight(10 / cmPerTick);
if (i == 0) {
CY -= 10;
i = 1;
}
}
if (i == 0){
CX += TX - CX;
rotate(-90.0);
}
if (obstacleYStart > CY ) {
eraseDisplay();
PlaySound(soundBeepBeep);PlaySound(soundBeepBeep);
powerOff();
}
}
}
}
wait1Msec(2000);wait1Msec(2000);wait1Msec(2000);
if (CX == TX) {
i = 0;
}
else if (CX != TX) {
i = 1;
}
return i;
}
/*this function computes the new straight line (Start, Target)*/
int newSTLine() {
nxtDisplayTextLine(1,"on newSTLine");
wait1Msec(2000);
float tempY;
int i;
float tempX;
if (CX != TX && CY != TY) {
nxtDisplayTextLine(1,"nothing equal");
wait1Msec(2000);
if (TY < CY) {
nxtDisplayTextLine(1,"TY < CY");
rotate(180.0);
tempY = CY - TY;
}
else if (TY > CY) { nxtDisplayTextLine(1,"target is further up");
tempY = TY - CY;
}
if (TX > CX ) {nxtDisplayTextLine(1,"T left");
tempX = TX - CX;
}
else if (TX < CX ) {nxtDisplayTextLine(1,"target right");
tempX = CX - TX;
}
eraseDisplay();
length = computeST(tempY, tempX);
degreesToTurn = ((float)tempX / (float)length);
degreesToTurn = asin(degreesToTurn)* 180.0 / PI;
if ((TX > CX && CY > TY) || (TX < CX && CY < TY)) {
nxtDisplayTextLine(1,"turn opposite");
degreesToTurn = degreesToTurn * -1;
}
eraseDisplay();
nxtDisplayTextLine(1,"Rotating");
nxtDisplayTextLine(2," %f", degreesToTurn);
wait1Msec(5000);//wait1Msec(5000);
rotate(degreesToTurn);
distanceReturned = calculateRotating((float)length);
i = moveStraight(distanceReturned);
if (i == 0) {
haltMotors();
}
if (i == 1) {
haltMotors();
if (TX > CX ) {
CX += ((ticksMoved * cmPerTick) * sinDegrees( degreesToTurn));
}
else if (TX < CX) {
CX -= ((ticksMoved * cmPerTick) * sinDegrees( degreesToTurn));
}
if (TY > CY) {
CY += ((ticksMoved * cmPerTick) * cosDegrees( degreesToTurn));
}
else if (TY < CY) {
CY -= ((ticksMoved * cmPerTick) * cosDegrees( degreesToTurn));
}
if ( TY < CY && TX < CX) {
eraseDisplay();
nxtDisplayTextLine(4,"TarY %f", TY);
i = exchaustObstacle();
if (i == 0) {
if (CY > TY){
rotate(180.0);
i = moveStraight((CY - TY) / cmPerTick);
}
else if ( CY < TY) {
i = moveStraight((TY - CY) / cmPerTick);
}
if (i == 0) {
haltMotors();
}
}
}
else {
rotate(degreesToTurn * -1);
}
}
}
return i;
}
/*a function to decentralize the robot from
**the coordinates of the target if these are equal*/
void decentralize() {
if (CX == TX) {
rotate(90);
int i = moveStraight(10 /cmPerTick);
if (i == 1) {
moveBackwards(ticksMoved + (10.0 / cmPerTick));
rotate(-90.0);
CX -= 10.0;
}
else if (i == 0) {
CX += 10.0;
rotate(-90.0);
}
}
if (TY == CY) {
int k = moveStraight(10 / cmPerTick);
if (k == 1) {
moveBackwards(ticksMoved + (10.0 / cmPerTick));
CY -= 10.0;
}
else if (k == 0) {
CY += 10.0;
}
}
}
void followBoundary(){
float degreesTurned;
float turning;
float opposite;
float adjacent;
float hypotenuse;
int i = 1;
obstacleXStart = CX;
obstacleYStart = CY;
turning = 10.0;
moveBackwards(20.0 / cmPerTick);//move back 20 cm
CY = CY - 20.0; // position changed, update the Y coordinate
adjacent = 20.0;
/*new code*/
while (i == 1) {
eraseDisplay();
rotate(turning);//turn to check obstacle
degreesTurned += turning;//update variables
nxtDisplayTextLine(1,"degTurn " );
nxtDisplayTextLine(2,"%f",degreesTurned );
nxtDisplayTextLine(4,"deg %f",degreesTurned );
hypotenuse = adjacent /cosDegrees(degreesTurned);
opposite = sinDegrees(degreesTurned) * hypotenuse;
i = moveStraight(hypotenuse / cmPerTick);
nxtDisplayTextLine(2,"i == %d",i );
if (i == 1) {nxtDisplayTextLine(1,"i == 1");
moveBackwards(ticksMoved);
if (degreesTurned > 79.0) {nxtDisplayTextLine(1,"dg > 79");
rotate (90.0 - degreesTurned);
degreesTurned += 90.0 - degreesTurned;
int q = moveStraight ((opposite + 10.0) / cmPerTick );
if (q == 1) {nxtDisplayTextLine(1,"q == 1");
rotate(((degreesTurned * -1) * 2));
degreesTurned += ((degreesTurned * -1) * 2);
int w = moveStraight(ticksMoved + ( 20.0 / cmPerTick));
if (w == 1) {nxtDisplayTextLine(1,"w == 1");
rotate (90.0);
degreesTurned += 90.0;
CX = CX - (ticksMoved * cmPerTick);
}
if (w == 0) {nxtDisplayTextLine(1,"w == 0");
rotate (90.0);
degreesTurned += 90.0;
CX = CX - (ticksMoved * cmPerTick);
}
}
else if (q == 0) {nxtDisplayTextLine(1,"q == 01");
CX += opposite + 10.0;
rotate(-90.0);
degreesTurned += -90.0;
int s = moveStraight(20.0 / cmPerTick);
if (s == 0) {nxtDisplayTextLine(1,"s == 0");
obstacleXEnd = CX;
obstacleYEnd = CY;
CY += 20.0;
eraseDisplay();
}
if ( s == 1) {nxtDisplayTextLine(1,"s == 1");
moveBackwards(ticksMoved);
}
}
}
}
else if (i == 0) {nxtDisplayTextLine(1,"else i == 0");
CX += opposite;
CY += 20.0 ;
rotate(degreesTurned * -1);
obstacleXEnd = CX;
obstacleYEnd = CY;
}
}
/*on this point the robot should always by facing the +Y direction (UP)*/
if (CX > TX) {nxtDisplayTextLine(1,"CX > TX");
int x = 0;
while (x == 0){
int a = moveStraight( 10 / cmPerTick);
if (a == 0) {nxtDisplayTextLine(1,"a == 0");
CY += 10.0;
rotate(-90);
int l = moveStraight((opposite / 2) / cmPerTick);
if ( l == 1) {nxtDisplayTextLine(1,"l == 1");
moveBackwards (ticksMoved);
rotate(90.0);
obstacleXEnd = CX - (ticksMoved * cmPerTick);
}
else if ( l == 0) {nxtDisplayTextLine(1,"l == 0");
CX -= opposite / 2;
x = 1;
rotate(90.0);
i = 0;
}
}
else if (a == 1) {nxtDisplayTextLine(1,"a == 1");
CY += ticksMoved * cmPerTick;
}
}
obstacleYEnd = CY; //set the end of the obstacle regarding the Y coordinate
}
degreesTurned = 0;
turning = 0;
opposite = 0;
adjacent = 0;
hypotenuse = 0;
if (TX == CX || TY == CY) {
decentralize();
nxtDisplayTextLine(1,"all ==");
}
i = 0;
}
/*function to check that the neccesary variables
** are initialized*/
void initialize() {
if ( WheelDistance == 0.0 || wheelDiameter == 0.0) {
nxtDisplayTextLine(1,"Error on Predef Variables");
powerOff();
}
calWheelCircumference();
calAxisCircumference();
calCMPerTick();
}
/*main control function*/
task main()
{
initialize();
int i;/*to store the result of the function moveStraight()*/
nMotorEncoder[motorA] = 0;
nMotorEncoder[motorB] = 0;
/*if the Target is in the opposite direction*/
if ( TY < 0) {
nxtDisplayTextLine(1,"Opposite direction");
rotate(180); /*turn to opposite direction.*/
TY = TY * -1; //convert negative to positive number;
}
/*if the target is not straight ahead*/
if ( TX != 0) {
nxtDisplayTextLine(1,"TX != 0");
length = computeST(TY, TX);
if (TX > 0 && TY != 0) {
nxtDisplayTextLine(1,"TX > 0 && TY != 0");
/*calculate number of degrees to turn in order to be heading straight to the Target.*/
degreesToTurn = ((float)TX / (float)length);
degreesToTurn = asin(degreesToTurn)* 180.0 / PI;
rotate(degreesToTurn);
}
else if(TX < 0 && TY != 0) {
nxtDisplayTextLine(1,"TX < 0 && TY != 0");
degreesToTurn = ((float)TX / (float)length);
degreesToTurn = asin(degreesToTurn)* 180.0 / PI;
rotate(degreesToTurn);
}
else if (TX > 0 && TY == 0.0) {
nxtDisplayTextLine(1,"TX > 0 && TY == 0.0");
rotate(90);
length = (int)TX;
}
else if (TX < 0 && TY == 0.0) {
nxtDisplayTextLine(1,"TargetX < 0 && TY == 0.0");
rotate(-90);
length = (int)TX;
}
distanceReturned = calculateRotating(length);
}
/*if the target is straight ahead*/
if (TX == 0) {
nxtDisplayTextLine(1,"TX == 0");
length = (int)TY;
distanceReturned = calculateRotating((float)length);
}
eraseDisplay(); /*clear the screen*/
i = moveStraight(distanceReturned);
if (i == 0) {
nxtDisplayTextLine(1,"i == 0");
haltMotors();
nxtDisplayTextLine(1,"Target Reached");
}
if (i == 1) {
nxtDisplayTextLine(1,"i == 1");
wait1Msec(1000);
rotate(degreesToTurn * -1); //face the obstacle
CX += ((ticksMoved * cmPerTick) * sinDegrees( degreesToTurn));
CY += ((ticksMoved * cmPerTick) * cosDegrees( degreesToTurn));
wait1Msec(5000);//wait1Msec(5000);
int p = 1;
/*while the target is not reached snd reachability test return 0*/
while(p == 1) {
followBoundary();
p = newSTLine();
}
powerOff();
haltMotors();
}
}
| |
BUG2 ALgorithm Implementation
-
