Timing problems with Task Control

alex404 · Thu Feb 28, 2008 6:06 am

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

Timing problems with Task Control

Who is online