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






Reply to topic  [ 6 posts ] 
Timing problems with Task Control 
Author Message
Rookie

Joined: Wed Feb 06, 2008 11:51 pm
Posts: 2
Post Timing problems with Task Control
Hey guys, I'm having a problem with RobotC that I can't seem to overcome.
Basically, the way I have it setup right now, I have three tasks running
(not including main): sensor_control, side_control, and engine_control.
Sensor_control essentially sends commands/readings to side_control, and
side_control sends commands to engine_control. The robot tries to follow
the wall on its left side, and I'm trying to get it to peak ahead of itself
with the sonar every once and a while to perform collision avoidance
as well.

Now, when I turn off the engine control (by commenting it out in task
main) the sonar_control works fine. However, when I turn on the
engine_control the timing of sonar_control gets thrown off; namely, the
head which swivel back on forth with a certain rhythm simply does so as
fast as possible. There's essentially a for loop which it never goes
through (either that or most of my wait commands are being ignored)
purely in virtue of engine_control being activated, despite
engine_control having no control over sensor_control. So I'm guessing
there's something I'm not understanding about multitasking, that's
making these two processes interfere despite the fact that they should
be independent.
Code:
//*!!Sensor,    S1,         sonar_sensor, sensorSONAR,      ,                    !!*//
//*!!Sensor,    S4,         touch_sensor, sensorTouch,      ,                    !!*//
//*!!Motor,  motorA,         sensor_motor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!Motor,  motorB,          right_motor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!Motor,  motorC,           left_motor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors sonar_sensor         = (tSensors) S1;   //sensorSONAR        //*!!!!*//
const tSensors touch_sensor         = (tSensors) S4;   //sensorTouch        //*!!!!*//
const tMotor   sensor_motor         = (tMotor) motorA; //tmotorNxtEncoderClosedLoop //*!!!!*//
const tMotor   right_motor          = (tMotor) motorB; //tmotorNxtEncoderClosedLoop //*!!!!*//
const tMotor   left_motor           = (tMotor) motorC; //tmotorNxtEncoderClosedLoop //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

                                 /*********************************/

/* The following are global constants such as max speed, desired wall distance, etc.
Altering these are the likely the easiest way to alter the robot's behaviour. */

const int max_speed = 100;
const int min_wall_distance = 30;
const int wall_distance_error = 2;

int sonar_state = 0; // 0 corresponds to the side facing state, 1 corresponds to ahead.

/* The following are variables used to control the motor task. RobotC doesn't appear to have
a decent way of implementing it, so I tried to get something that looked like message passing
from one task to another. Basically, when motor_updated is false, it means that the motor needs
to update its state with these values. */

bool motor_updated = false;
int turn_ratio = 100;
int sc_motor_power = 0;
int fc_motor_power = 100;

/* These are control variables for the side control. */

bool side_control_updated = false;
int side_sonar_measurement[2];
int side_integral = 0;
int side_differential = 0; // Measured in sonar centimetres per second.
int side_proportional = 0;

/* These are control variables for the front control. */

bool front_control_updated = false;
int front_sonar_measurement[2];
int front_integral = 0;
int front_differential = 0; // Measured in sonar centimetres per second.
int front_proportional = 0;

/* A min function. */
int min1(int i1, int i2) {

   int min = (i1 + i2 - abs(i2 - i1))/2;
   return min;
}

int min2(int i1, int i2) {

   int min = (i1 + i2 - abs(i2 - i1))/2;
   return min;
}

/* A max function. */
/*
int max(int i1, int i2) {

   int max = (i1 + i2 + abs(i2 - i1))/2;
   return max;
} */

/* This is the engine control task. All actual changes to motor variables
are done through this task, via commands (through global variables) from
other sub-systems. */
task engine_control() {

   int left_wheel_ratio;
   int right_wheel_ratio;
   int motor_power;

   while(true) {

      // Determine if the motor needs to be updated.
      if (!motor_updated) {

         // Determine how to interpret the turn ratio.
         if (turn_ratio > 100) {

            left_wheel_ratio = 200 - turn_ratio;
            right_wheel_ratio = 100;

         } else {

            right_wheel_ratio = turn_ratio;
            left_wheel_ratio = 100;

         }

         // Initializations for engine tuning
         nMotorPIDSpeedCtrl[left_motor] = mtrSpeedReg;
         nMotorPIDSpeedCtrl[right_motor] = mtrSpeedReg;
         nMotorEncoder[left_motor] = 0;
         nMotorEncoderTarget[left_motor] = 0;
         nMotorEncoder[left_motor] = 0;
         nMotorEncoderTarget[left_motor] = 0;

         // Incorporating changes

         motor_power = min1(sc_motor_power, fc_motor_power);
         motor[left_motor] = (motor_power * left_wheel_ratio) / 100;
         motor[right_motor] = (motor_power * right_wheel_ratio) / 100;
         motor_updated = true;

         nxtDisplayTextLine(5, "Motor Power: " + motor_power);
         nxtDisplayTextLine(6, "Turn Ratio: " + turn_ratio);

         wait1Msec(10); // Note that this wait controls a minimum sample rate.
      }
   }
}

/* This task moves the sonar from side to side, updating the readings to the appropriate controller. */
task sonar_control() {

   while (true) {

      // If the robot is looking left
      if (sonar_state == 0) {


         for(int i = 0; i < 15; i++) {
         // Get readings.

            nxtDisplayTextLine(5, "Foo: " + i);

            side_sonar_measurement[0] = SensorValue[sonar_sensor];
            side_sonar_measurement[1] = nPgmTime;

            wait1Msec(100);

            side_control_updated = false;
         }

         // Move sensor into different position.
         nMotorEncoder[sensor_motor] = 0;
         nMotorEncoderTarget[sensor_motor] = -60;
         motor[sensor_motor] = -60;
         sonar_state = 1;

         wait1Msec(200);

         // If the robot is looking forward.
      } else if (sonar_state == 1) {


         for(int i = 0; i < 4; i++) {
         // Get readings.

            front_sonar_measurement[0] = SensorValue[sonar_sensor];
            front_sonar_measurement[1] = nPgmTime;

            wait1Msec(100);

            front_control_updated = false;
         }

         // Move sensor into different position.
         nMotorEncoder[sensor_motor] = 0;
         nMotorEncoderTarget[sensor_motor] = 60;
         motor[sensor_motor] = 60;
         sonar_state = 0;

         wait1Msec(200);

      }
   }
}

/* This task attempts to run in parallel to a wall, using P control. */
task side_control() {

   int old_sonar_measurement[2];
   old_sonar_measurement[0] = 0;
   old_sonar_measurement[1] = 0;

   int sp_mod;
   int sd_mod;

   while (true) {

         if (!side_control_updated) {
         // Derive new set of values

         side_proportional = (side_sonar_measurement[0] - min_wall_distance);
         side_differential = (side_sonar_measurement[0] - old_sonar_measurement[0]) / ((side_sonar_measurement[1] - old_sonar_measurement[1]) / 100);
         side_integral = side_integral + (side_sonar_measurement[0] * (side_sonar_measurement[1] - old_sonar_measurement[1])) / 1000;

         // Create scaled variables for engine use.

         sp_mod = min2(side_proportional * 2, 40);
         sd_mod = (side_differential / abs(side_differential)) * min2(abs(side_differential), 5);

         // Issue engine command

         turn_ratio = 100 + sp_mod + sd_mod;
         sc_motor_power = 40; //100 / (2 + abs(sd_mod));
         motor_updated = false;

         // Store old measurement

         old_sonar_measurement[0] = side_sonar_measurement[0];
         old_sonar_measurement[1] = side_sonar_measurement[1];

         nxtDisplayTextLine(1, "Sonar Value: " + side_sonar_measurement[0]);
         nxtDisplayTextLine(2, "Proportional: " + side_proportional);
         nxtDisplayTextLine(3, "Differential: " + side_differential);
         nxtDisplayTextLine(4, "Integral: " + side_integral);

         wait1Msec(10);

         side_control_updated = true;
      }
   }
}

/* task forward_control() {

} */

/* Task main initializes a few variables and otherwise simply starts all the other tasks up.  */
task main {

   // Some initializations.
   side_sonar_measurement[0] = SensorValue[sonar_sensor];
   side_sonar_measurement[1] = nPgmTime;

   StartTask(engine_control);
   StartTask(side_control);
   StartTask(sonar_control);


   while (true) { wait1Msec(1000); }

}


Thu Feb 07, 2008 12:02 am
Profile
Creator
Creator

Joined: Fri Feb 09, 2007 9:21 am
Posts: 615
Post 
Sorry for taking so long to get to this issue. I've been looking at it and there is a bug in the ROBOTC compiler in that the temporary variable allocated to hold results of string concatenation
Code:
        nxtDisplayTextLine(5, "Motor Power: " + motor_power);

is also being allocated to regular variables including "i". So every time the "engine_countol" task was being allocated, 'i' was being set to large value.

I'm looking at the fix now.

Update 2008/03/09. This is fixed in version 1.17.


Last edited by Dick Swan on Wed Mar 12, 2008 3:40 am, edited 1 time in total.



Wed Feb 27, 2008 12:15 am
Profile
Rookie

Joined: Wed Dec 12, 2007 3:32 pm
Posts: 8
Post 
Hi,

I have a similar problem with tasking and timing.

The wait10Msec function doesn't seem to work in my tasks.
They are ignored.

Here my testprogramm I have written to make some tests with tasking:

Code:
//*!!Sensor,    S1,                  Ta1, sensorTouch,      ,                    !!*//
//*!!Motor,  motorA,                   MA, tmotorNxtEncoderClosedLoop,           !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors Ta1                  = (tSensors) S1;   //sensorTouch        //*!!!!*//
const tMotor   MA                   = (tMotor) motorA; //tmotorNxtEncoderClosedLoop //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

const int wait = 500;
const int Speed1 = 50;
const int Speed2 = -50;

task Motor1()
{
         motor[MA]=Speed1;
         wait10Msec(wait);
         motor[MA]=0;
         wait10Msec(wait);
}

task Motor2()
{
         motor[MA]=Speed2;
         wait10Msec(wait);
         motor[MA]=0;
         wait10Msec(wait);
}

task main()
{
   while(true)
   {
      if(SensorValue(Ta1))
         StartTask(Motor1);
      if(!SensorValue(Ta1))
         StartTask(Motor2);
   }
}

A fix would be nice :)


Thu Feb 28, 2008 6:06 am
Profile
Creator
Creator

Joined: Fri Feb 09, 2007 9:21 am
Posts: 615
Post 
CHW wrote:
I have a similar problem with tasking and timing.
. . .
A fix would be nice :)

I believe you have a programming error. :(

In your main task you can repeatedly call the "StartTask" function for your secondary tasks. This will "start" or "restart" (if already running) your secondary task.

Becasue the two secondary tasks are continually being restarted, they always look like they're hung in the first "wati10Msec". They're always at this point becase they are continuously restarted.

You could verify this with a counter that you increment at the beginning of a task. It should be continuously incremented.

This is the same problem that "alex404" has encountered.


Thu Feb 28, 2008 6:34 am
Profile
Rookie

Joined: Wed Dec 12, 2007 3:32 pm
Posts: 8
Post 
Thank you!!!! :)

I couldn't explain me why this function didn't work.

That was a big help for me and it sounds evident.

So I can try to find another sollution.


Thu Feb 28, 2008 7:46 am
Profile
Rookie

Joined: Wed Feb 06, 2008 11:51 pm
Posts: 2
Post 
Thanks for your help, Dick. Things proceeded very well after I was able to get around the bug.


Mon Mar 10, 2008 5:24 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 6 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.