View unanswered posts | View active topics It is currently Tue Sep 30, 2014 2:10 pm






Reply to topic  [ 12 posts ] 
my functions seem to be causing my loops to get stuck. 
Author Message
Rookie

Joined: Tue Oct 29, 2013 11:13 am
Posts: 12
Post my functions seem to be causing my loops to get stuck.
the loop:
Code:
while((SensorValue[sonar]) > 40){ //search
  turn(10,700,0);
   turn(10,1400,1);
   turn(10,700,0);
}


when I change the inside of the while loop to a few motor[]'s it seems to work perfectly, but with the functions in use it attempts to finish the functions before it quits the loop.

the function turn()
Code:
   void turn(int power,int distance, bool direction){
   resetEncoders();
   //right
     if(direction == 1){
   while(nMotorEncoder[motorD] > -distance || nMotorEncoder[motorE] < distance){
      motor[motorD] = -power;
      motor[motorE] = power;
        }
       }
   //left
   if(direction == 0){
      while(nMotorEncoder[motorD] < distance || nMotorEncoder[motorE] > -distance){
      motor[motorD] = power;
      motor[motorE] = -power;
          }
     }
   brake();
           }


Tue Nov 12, 2013 11:58 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: my functions seem to be causing my loops to get stuck.
kevtastic wrote:
the loop:
Code:
while((SensorValue[sonar]) > 40){ //search
  turn(10,700,0);
   turn(10,1400,1);
   turn(10,700,0);
}


when I change the inside of the while loop to a few motor[]'s it seems to work perfectly, but with the functions in use it attempts to finish the functions before it quits the loop.

the function turn()
Code:
   void turn(int power,int distance, bool direction){
   resetEncoders();
   //right
     if(direction == 1){
   while(nMotorEncoder[motorD] > -distance || nMotorEncoder[motorE] < distance){
      motor[motorD] = -power;
      motor[motorE] = power;
        }
       }
   //left
   if(direction == 0){
      while(nMotorEncoder[motorD] < distance || nMotorEncoder[motorE] > -distance){
      motor[motorD] = power;
      motor[motorE] = -power;
          }
     }
   brake();
           }

Looking at your code, I can see several issues. First turning with 10% power is not going to move the robot. 10% power is way too weak. Try at least 30% or more. The turn function you have is really inefficient. Let's take a look at your code. Since you did not label which motor controls the left wheel and which controls the right, it makes the code harder to understand. But judging from the code, it looks like motorD is the right wheel and motorE is the left. Let's fix the indentation and change the names so the code is more readable.
Code:
void turn(int power,int distance, bool direction)
{
    resetEncoders();

    //turning right
    if(direction == 1)
    {
        while(nMotorEncoder[rightMotor] > -distance || nMotorEncoder[leftMotor] < distance)
        {
            motor[rightMotor] = -power;
            motor[leftMotor] = power;
        }
    }

    //turning left
    if(direction == 0)
    {
        while(nMotorEncoder[rightMotor] < distance || nMotorEncoder[leftMotor] > -distance)
        {
            motor[rightMotor] = power;
            motor[leftMotor] = -power;
        }
    }
    brake();
}

Now your code is checking for direction (0 means left and 1 means right). First of all, you checked for 1 to do right turn, then you checked for 0 to do left turn. You don't really have to check twice. What if you replace the "if (direction == 0)" with "else"?
Also, the right turn code and the left turn code are almost identical. Could it be combined to handle both left and right turn? Yes, we could. First, we need to get rid of "direction". We can implicitly specify the direction by "distance". If "distance" is positive, we turn right. If "distance" is negative, we turn left. Also, it is really not "distance", so let's change the name to "target". So if "target" is positive, we turn right. That means left wheel gets positive power and right wheel gets negative power. In other words, we need to make sure power is positive when turning right. To do that we can use this code:
Code:
    power = (target >= 0)? abs(power): -abs(power);

If you are not familiar with the <condition>? <value if true>: <value if false> syntax, you can totally do this with if-then-else.
Code:
if (target >= 0)
{
    //
    // Just to make sure power is positive.
    //
    power = abs(power);
}
else
{
    //
    // Just to make sure power is negative.
    //
    power = -abs(power);
}

Now how do we check if we reached target? In a turning scenario, the forward turning wheel will have increasing encoder counts and the backward turning wheel will have decreasing encoder counts. So if we subtract one from the other, we will get either an "increasing" positive number or an "increasing" negative number. The sign is irrelevant here. The interesting thing is the magnitude is increasing. So we can compare the magnitude of the difference of the two encoders to the magnitude of the target.
Code:
if (abs(nMotorEncoder[leftMotor] - nMotorEncoder[rightMotor]) < abs(target))

If you subtract the left and right encoders, the magnitude is changing twice as fast. So just to make the code equivalent to yours, we will divide the difference by 2.
Code:
if (abs(nMotorEncoder[leftMotor] - nMotorEncoder[rightMotor])/2 < abs(target))

It really doesn't matter, it only affects the caller if you call turn with a target of 1400 or 2800.
Finally, we need to turn the left and the right motors in different directions to do the turn. At the beginning, we already established the convention that positive power means turning right. So it means the left wheel gets power and the right wheels gets -power.
To put that all together, here is the code:
Code:
void turn(int target, int power)
{
    //
    // Before we start, let's reset the encoders.
    //
    nMotorEncoder[leftMotor] = 0;
    nMotorEncoder[rightMotor] = 0;
    //
    // If target is positive, we will turn right, otherwise we will turn left.
    // Set power such that it is positive if turning right.
    //
    power = (target >= 0)? abs(power): -abs(power);
    while (abs(nMotorEncoder[leftMotor] - nMotorEncoder[rightMotor])/2 < abs(target))
    {
        motor[leftMotor] = power;
        motor[rightMotor] = -power;
        wait1Msec(50);
    }
    //
    // The turn is completed, so stop the motors.
    //
    motor[leftMotor] = 0;
    motor]rightMotor] = 0;
}


Tue Nov 12, 2013 4:15 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: my functions seem to be causing my loops to get stuck.
BTW, it might be friendlier if the turn function takes "degrees" as a parameter instead of target encoder count. You can do this:
Code:
#define ENCODER_COUNT_PER_DEGREE                35.4

void turn(float degrees, int power)
{
    int target = degrees*ENCODER_COUNT_PER_DEGREE;
    //
    // Before we start, let's reset the encoders.
    //
    nMotorEncoder[leftMotor] = 0;
    nMotorEncoder[rightMotor] = 0;
    //
    // If target is positive, we will turn right, otherwise we will turn left.
    // Set power such that it is positive if turning right.
    //
    power = (target >= 0)? abs(power): -abs(power);
    while (abs(nMotorEncoder[leftMotor] - nMotorEncoder[rightMotor])/2 < abs(target))
    {
        motor[leftMotor] = power;
        motor[rightMotor] = -power;
        wait1Msec(50);
    }
    //
    // The turn is completed, so stop the motors.
    //
    motor[leftMotor] = 0;
    motor]rightMotor] = 0;
}

You need to experiment to determine what the proper value of ENCODER_COUNT_PER_DEGREE should be. But once that's determined, you can now call the turn function like this:
Code:
//
// Turn right 90 degrees
//
turn(90.0, 50);
//
// Turn left 45 degrees
//
turn(-45.0, 50);


Tue Nov 12, 2013 4:39 pm
Profile
Rookie

Joined: Tue Oct 29, 2013 11:13 am
Posts: 12
Post Re: my functions seem to be causing my loops to get stuck.
wow, thank you for the advice :bigthumb: I modified my code to use your logic but it still gets stuck in the while loop for ultrasonic. 10 power is moving the robot well enough, the only problem seems to be my ultrasonic sensor, it reads a value every time the loop starts but if the condition is met then it doesn't exit the loop itself.

Code:
while((SensorValue[sonar]) > 40){ //search
  turn(-700,10);
   turn(1400,10);
   turn(-700,10);
}


Mon Nov 18, 2013 12:05 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: my functions seem to be causing my loops to get stuck.
Then you need to debug this by showing your ultrasonic sensor value, like the following and put your hand in front of the ultrasonic sensor to see if the value changes.
Code:
while((SensorValue[sonar]) > 40){ //search
   nxtDisplayTextLine(2, "us=%d", SensorValue[sonar]);
//   turn(-700,10);
//   turn(1400,10);
//   turn(-700,10);
}

If you said the loop doesn't exit, it means the sonar value never go below or equal to 40. If that's true, you either have a defective ultrasonic sensor, or you did not connect or configure the ultrasonic sensor correctly.


Mon Nov 18, 2013 4:32 pm
Profile
Rookie

Joined: Tue Oct 29, 2013 11:13 am
Posts: 12
Post Re: my functions seem to be causing my loops to get stuck.
so using that code, it exits perfectly, but when i add in the turns, it only changes value when the program comes back to the beginning of the loop.


Wed Nov 20, 2013 10:56 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: my functions seem to be causing my loops to get stuck.
Are you saying the code is stuck inside the turn function? If so, that's why I said 10% power is too weak. If you look at the turn() function closely, you will see there is a while loop in there. What if it doesn't exit that loop? If that's the case, it means the turn never reaches the target angle. Try putting nxtDisplayTextLine statements in the turn function like this:
Code:
void turn(float degrees, int power)
{
    int target = degrees*ENCODER_COUNT_PER_DEGREE;
    //
    // Before we start, let's reset the encoders.
    //
    nMotorEncoder[leftMotor] = 0;
    nMotorEncoder[rightMotor] = 0;
    //
    // If target is positive, we will turn right, otherwise we will turn left.
    // Set power such that it is positive if turning right.
    //
    power = (target >= 0)? abs(power): -abs(power);
    while (abs(nMotorEncoder[leftMotor] - nMotorEncoder[rightMotor])/2 < abs(target))
    {
        nxtDisplayTextLine(3, "LE=%d,RE=%d", nMotorEncoder[leftMotor], nMotorEncoder[rightMotor]);
        motor[leftMotor] = power;
        motor[rightMotor] = -power;
        wait1Msec(50);
    }
    //
    // The turn is completed, so stop the motors.
    //
    motor[leftMotor] = 0;
    motor]rightMotor] = 0;
}

This code will show you the reading of the encoders and you can check if they meet the exit requirement of the loop.


Wed Nov 20, 2013 2:16 pm
Profile
Rookie

Joined: Tue Oct 29, 2013 11:13 am
Posts: 12
Post Re: my functions seem to be causing my loops to get stuck.
the robot acts as expected. it turns left 700 ticks, then right 1400 ticks, then left 700 ticks and loops. Yet when i put my hand in front of the ultrasonic sensor, the sensors value doesn't update until it loops back to the start of the loop. If it helps, when the condition of the while loop is met before it begins the turns, it exits the loop instantly, but if it enters the loop it refuses to exit.


Thu Nov 21, 2013 11:50 am
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post Re: my functions seem to be causing my loops to get stuck.
hi,
could you pls post your whole current source code?

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Thu Nov 21, 2013 12:09 pm
Profile
Rookie

Joined: Tue Oct 29, 2013 11:13 am
Posts: 12
Post Re: my functions seem to be causing my loops to get stuck.
Code:
#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     touch,          sensorTouch)
#pragma config(Sensor, S3,     light,          sensorLightActive)
#pragma config(Sensor, S4,     sonar,          sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//BRAKE FUNCTION
   void brake(){
      motor[motorD] = 0;
     motor[motorE] = 0;
     wait1Msec(500);
             }

//ENCODER RESET
   void resetEncoders(){
      nMotorEncoder[motorE] = 0;
     nMotorEncoder[motorD] = 0;
                     }

//MOVEMENT FUNCTION
   void movePowerValueSensor(int power, int senValue,char sensor){
   resetEncoders();
    while(SensorValue[sensor] != senValue){
      motor[motorD] = power;
      motor[motorE] = power;
         }
   brake();
                            }

//MOVEMENT FUNCTION
   void movePowerDistance(int power, int distance,int dir){
   resetEncoders();
    while(nMotorEncoder[motorD] <= distance || nMotorEncoder[motorE] <= distance){
      motor[motorD] = power*dir;
      motor[motorE] = power*dir;
        }
   brake();
                         }

//TURN FUNCTION
void turn(int target, int power)
{
    resetEncoders();
    // If target is positive, turn right, otherwise turn left.
    power = (target >= 0)? abs(power): -abs(power);
    while (abs(nMotorEncoder[motorE] - nMotorEncoder[motorD])/2 < abs(target))
    {
        motor[motorE] = power;
        motor[motorD] = -power;
        wait1Msec(50);
    }
    brake();
}




//*******MAIN TASK*******//
task main(){

int x,motorRotations,dir;
//servo[servo1] = 100;
//movePowerValueSensor(50,30,light); //move to black line
//turn(-800,70);
//movePowerValueSensor(50,1,touch); //move to troll
//PlaySoundFile("pig squeal.rso");
//wait10Msec(1000);
//turn(-850,70);
//movePowerValueSensor(50,30,light); //move to second black line
//turn(-800,70);
//movePowerValueSensor(50,55,light); //move to silver tape
//movePowerDistance(50,2200,1);
//turn(-850,50);
//movePowerDistance(30,7500,1); //move to grail area
while((SensorValue[sonar]) > 40){ //search

    resetEncoders();
    while (abs(nMotorEncoder[motorE] - nMotorEncoder[motorD])/2 < abs(700)){
        motor[motorE] = -10;
        motor[motorD] = 10;
    }

    resetEncoders();
    while (abs(nMotorEncoder[motorE] - nMotorEncoder[motorD])/2 < abs(1400)){
        motor[motorE] = 10;
        motor[motorD] = -10;
    }

    resetEncoders();
    while (abs(nMotorEncoder[motorE] - nMotorEncoder[motorD])/2 < abs(700)){
        motor[motorE] = -10;
        motor[motorD] = 10;
    }
    }
if(nMotorEncoder[motorD] > nMotorEncoder[motorE]){ //Recording turn distance
   motorRotations = nMotorEncoder[motorD];
  dir=1;
}
else{
   motorRotations = nMotorEncoder[motorE];
  dir=-1;
}
//moving towards grail
 resetEncoders();
    while(SensorValue[sonar] > 20){
      nxtDisplayCenteredBigTextLine(3,"%d",SensorValue[sonar]);
      motor[motorD] = 20;
      motor[motorE] = 20;

         }
 brake();

x = nMotorEncoder[motorD]; //recording distance to grail
movePowerDistance(50,(-x),(-1)); //back to search position
turn(motorRotations,50); //turn back

}



I just attempted a simpler loop and it worked perfectly.

Code:
while((SensorValue[sonar]) > 40){
motor[motorE] = 10;
motor[motorD] = -10;
}


no idea what that means


Thu Nov 21, 2013 12:22 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post Re: my functions seem to be causing my loops to get stuck.
kevtastic wrote:
the robot acts as expected. it turns left 700 ticks, then right 1400 ticks, then left 700 ticks and loops. Yet when i put my hand in front of the ultrasonic sensor, the sensors value doesn't update until it loops back to the start of the loop.


so is your code now working correctly or is there still an issue?

if there is, could you please mark the loop which must be stopped or left prematurely depending on the US sensor value (by your hand in front) ?

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Thu Nov 21, 2013 12:44 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: my functions seem to be causing my loops to get stuck.
kevtastic wrote:
the robot acts as expected. it turns left 700 ticks, then right 1400 ticks, then left 700 ticks and loops. Yet when i put my hand in front of the ultrasonic sensor, the sensors value doesn't update until it loops back to the start of the loop. If it helps, when the condition of the while loop is met before it begins the turns, it exits the loop instantly, but if it enters the loop it refuses to exit.

I see. That's what multi-tasking is about. The way your code is structured cannot do multi-tasking. The turn function is a blocking call. So while the robot is turning, it will just look at the encoder and ignore the sonar. If you want it to monitor both, you need to restructure your code. What exactly are you trying to do? From looking at the whole code, it sounds like you are trying to scan left and right and look for an obstacle that is within 40cm in front. If that's the case, you need to have the code monitor both sensors. For example:
Code:
bool SweepForObstacle(int power, int encoderTarget, int sonarTarget)
{
    bool foundObstacle = false;
    //
    // Before we start, let's reset the encoders.
    //
    nMotorEncoder[leftMotor] = 0;
    nMotorEncoder[rightMotor] = 0;

    //
    // If target is positive, we will turn right, otherwise we will turn left.
    // Set power such that it is positive if turning right.
    //
    power = (target >= 0)? abs(power): -abs(power);

    while (abs(nMotorEncoder[leftMotor] - nMotorEncoder[rightMotor])/2 < abs(target))
    {
        nxtDisplayTextLine(3, "LE=%d,RE=%d", nMotorEncoder[leftMotor], nMotorEncoder[rightMotor]);
        if (SensorValue[sonar] <= sonarTarget)
        {
            foundObstacle = true;
            break;
        }
        motor[leftMotor] = power;
        motor[rightMotor] = -power;
        wait1Msec(50);
    }
    //
    // Either the turn is completed or found obstacle, so stop the motors.
    //
    motor[leftMotor] = 0;
    motor]rightMotor] = 0;

    return foundObstacle;
}   //SweepForObstacle

task main()
{
    bool foundObstacle = false;

    while (!foundObstacle)
    {
        foundObstacle = SweepForObstacle(10, -700, 40) ||
                        SweepForObstacle(10, 1400, 40) ||
                        SweepForObstacle(10, -700, 40);
    }
}

The above code shows you one way of achieving your goal. In general, I don't like that approach because it makes the code still not multi-tasking friendly. To make it multi-tasking friendly, you should never have any loops or wait statements in your functions. So the trick is to write a function that does things in steps. It will return immediately and will never wait. The next time it is called, it will continue doing the next step and then return. Now for your problem, what if you have a SweepTask whose only job is to scan left and right? Every time you call the SweepTask, it will just turn the robot to one direction. It will also check if the robot reaches the sweep limit, then it reverses the sweeping direction. This will go back and forth. As in any multi-tasking friendly functions, the function must remember the state so that when it is called again, it will know how to continue where it's left off last time. In this SweepTask, the state is "sweepDir" remembering which direction it is sweeping (negative is to the left and positive is to the right). By declaring a "static int" variable, this variable will remember its value even when the function exits. If you don't understand static variables, just declare it as a global variable outside of any function.
Code:
void SweepTask(int sweepPower, int sweepRange)
{
    static int sweepDir = -1;  //starting with sweeping left
    //
    // If sweepDir is positive, we will turn right, otherwise we will turn left.
    // Set power such that it is positive if turning right.
    //
    sweepPower = (sweepDir >= 0)? abs(sweepPower): -abs(sweepPower);

    if (abs(nMotorEncoder[leftMotor] - nMotorEncoder[rightMotor])/2 < abs(sweepRange))
    {
        nxtDisplayTextLine(3, "LE=%d,RE=%d", nMotorEncoder[leftMotor], nMotorEncoder[rightMotor]);
        motor[leftMotor] = sweepPower;
        motor[rightMotor] = -sweepPower;
    }
    else
    {
        //
        // We have reached the sweep range limit, reverse the direction.
        //
        motor[leftMotor] = -sweepPower;
        motor[rightMotor] = sweepPower;
        sweepDir *= -1;
    }
}   //SweepTask

task main()
{
    //
    // Before we start, let's reset the encoders.
    //
    nMotorEncoder[leftMotor] = 0;
    nMotorEncoder[rightMotor] = 0;

    while (SensorValue[sonar] > 40)
    {
        SweepTask(10, 700);
        wait1Msec(50);
    }
    //
    // Found obstacle, so stop the motors.
    //
    motor[leftMotor] = 0;
    motor]rightMotor] = 0;
}


Thu Nov 21, 2013 3:34 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 12 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.