Help with tasks.

VEX V5 Technical Support
1

Okay so we are having an issue with the tasks in our code. We have a task main and a task velocity control. The robot will move around and stuff until we hit the button that starts the velocity control task. Then nothing will work. It’s like it’s stuck in that task instead of multitasking. Can someone help me understand what is wrong?

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  rightFlywheel,   sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  leftFlywheel,  sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           DW1,           tmotorVex393HighSpeed_HBridge, openLoop)
#pragma config(Motor,  port2,           DW2,           tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port3,           DW3,           tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           DW4,           tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           flyWheelL1,    tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port6,           flyWheelL2,    tmotorVex393_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port7,           flyWheelR1,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           flyWheelR2,    tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port9,           IW1,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          IW2,           tmotorVex393HighSpeed_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

static int target;   //defines global variable for target velocity
task velocityControl; //defines velocityControl don't touch this

float FwMotorEncoderGet(int side); // defines the two functions used to calc actual velocity
float FwCalculateSpeed(int side);
void setVelocity(int speed); //function to choose target velocity



task main()  //beginning of driver control
	{
		nMotorEncoder[leftFlywheel] = 0;  //sets the motor encoders to 0 outside of the loop
		nMotorEncoder[rightFlywheel] = 0; //so that it doesn't continually reset
		//starts up the velocity control task
			while(1 == 1){

				//dummy text for flywheel
				if(vexRT[Btn8D] == 1 ){
					setVelocity(70);
					startTask (velocityControl);
				}


 				//Intake code

 					 if(vexRT[Btn7L] == 1){
							motor[IW1] = 127;
							motor[IW2] = 127;
						}
						else if(vexRT[Btn7D] == 1) {
							motor[IW1] = -127;
							motor[IW2] = -127;
						}
						else {
							motor[IW1] = 0;
							motor[IW2] = 0;
						}


				//Wheel control

 					 if(vexRT[Btn6U] == 1 ) {
  						motor[DW1] = -75;
  						motor[DW2] = -75;
  						motor[DW3] = -75;
  						motor[DW4] = -75;
  					}

  					else if(vexRT[Btn5U] == 1) {
	 						 motor[DW1] = 75;
  						 motor[DW2] = 75;
  						 motor[DW3] = 75;
  					   motor[DW4] = 75;
						}

						else if(vexRT[Ch2] > 15) {
							motor[DW1] = -100;
							motor[DW2] = 100;
							motor[DW3] = 100;
							motor[DW4] = -100;
						}

						else if(vexRT[Ch2] < -15)	{
								motor[DW1] = 100;
								motor[DW2] = -100;
								motor[DW3] = -100;
								motor[DW4] = 100;
						}
						else {
								motor[DW1] = 0;
								motor[DW2] = 0;
								motor[DW3] = 0;
								motor[DW4] = 0;
						}
									//end wheel control

		}
		//end drivercontrol
}


task velocityControl(){

			float Lvelocity;					//initilizes and calculates
			float Rvelocity;					//left and right velocities
	

			//initilize all the variables needed in PID
			float kp = 0.01; //proportional constant
			float ki = 0.000000; //integral constant
			float kd =0.00; //derivative constant

			float Lcurrent = 0;
			float Rcurrent = 0;//starts with no power
			float Lerror; //current error constants
			float Rerror;
			float integralActiveZone = 2; //integral error accumulates

			float LerrorT; //total error
			float LlastError; //last error recored
			float RerrorT; //total error
			float RlastError; //last error recored
			float Lproportion; //proportion term
			float Lintegral; //integral term
			float Lderivative; //derivative term
			float Rproportion; //proportion term
			float Rintegral; //integral term
			float Rderivative; //derivative term

			while(true){
				//start of PID algorithm loop
			
			Lvelocity = FwCalculateSpeed(1);
			Rvelocity = FwCalculateSpeed(0);
			
				Lerror = target - Lvelocity; //difference between current vs target velocity
				Rerror = target - Rvelocity;


				if(Lerror<integralActiveZone && Lerror !=0){ //active integral zone
						LerrorT += Lerror; //adds error
				}
						else {
							LerrorT = 0;
						}

				if(Rerror<integralActiveZone && Rerror !=0){ //active integral zone
						RerrorT += Rerror; //adds error
				}		else {
								RerrorT = 0;
						}

				if(LerrorT>50/ki){ //caps error at 50
						LerrorT = 50/ki;
			  }

			 	if(Lerror == 0) { //if error zero then derivative zero
						Lderivative = 0;
				}

				if(RerrorT>50/ki) { //caps error at 50
						RerrorT = 50/ki;
			  }

				if(Rerror == 0) { //if error zero then derivative zero
						Rderivative = 0;
				}

				//A bunch of PID calculations for the left and right sides
								Lproportion = Lerror * kp; //setting proportion term
								Lintegral = LerrorT * ki; //setting integral term
								Lderivative = (Lerror - LlastError)*kd;//setting derivative term
								Rproportion = Rerror * kp; //setting proportion term
								Rintegral = RerrorT * ki; //setting integral term
								Rderivative = (Rerror - RlastError)*kd;//setting derivative term

								LlastError = Lerror; //last error becomes current error for next loop
								RlastError = Rerror; //last error becomes current error for next loop

								Lcurrent = Lproportion + Lderivative + Lintegral; //setting power
								Rcurrent = Rproportion + Rderivative + Rintegral; //setting power

			//take the value from PID and put it into the motors
					motor[flyWheelL1] = motor[flyWheelL2] = Lcurrent; //giving motors power
					motor[flyWheelR1] = motor[flyWheelR2] = Rcurrent;

		wait1Msec(20); //proventing hogging of CPU power
	}
		//end velocity control
}



//function to set desired speed in program
void setVelocity( int speed ) {
		target = speed; //setss what you put into it as the variable target
}



//function to get the actual velocity of each side

float FwCalculateSpeed(int side){

		float 		velocity;								//initializes all the variables
    float     delta_ms;
    float     delta_enc;
    float			nSysTime_last = 0;
    float 		ticks_per_rev = 627.2;
    float 		encoder_counts;
    float 		encoder_counts_last;


    // Get current encoder value for a side
    if (side == 1) {
    encoder_counts = FwMotorEncoderGet(1);
		}
		if (side != 1) {
			encoder_counts = FwMotorEncoderGet(0);
		}


    // This is just used so we don't need to know how often we are called
    // how many mS since we were last here
    delta_ms = nSysTime - nSysTime_last;
    nSysTime_last = nSysTime;

    // Change in encoder count
    delta_enc = (encoder_counts - encoder_counts_last);

    // save last position
    encoder_counts_last = encoder_counts;

    // Calculate velocity in rpm
    velocity = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;


    return velocity; //returns the value so that velocity control can store it in a variable
}


//function to get the value of the motor encoders

float FwMotorEncoderGet(int side) {
		float count; //init variable we store ticks in

		if (side == 1) {												//gets the ticks for one side depending on what you feed into it

		nMotorEncoder[leftFlywheel] = count;
		}
			if (side != 1) {

		nMotorEncoder[rightFlywheel] = count;
		}

		return count;

}
2

Problems can arise when you try to start tasks multiple times, which is what you’re doing with the current code. When the button to start the task is pressed, even for a short time, the main task still has time to cycle through the while loop several times and attempt to start the velocity control task several times.

You can fix this if you simply set a flag to keep track of whether the task is running or not, and then only start the task when the button is pushed AND the task is not yet running.


task main()
{
    bool bVelocityTaskRunning;
    While(true)
    {
        If(vexRT[Btn8D] && bVelocityTaskRunning)
        {
            bVelocityTaskRunning = true;
            setVelocity(70);
            StartTask(velocityControl);
        }
    }
}
3

The thing is before I had the starttask velocity control outside of the the while loop so it only happened once and it was still doing the same thing.

task main()  //beginning of driver control
	{
		nMotorEncoder[leftFlywheel] = 0;  //sets the motor encoders to 0 outside of the loop
		nMotorEncoder[rightFlywheel] = 0; //so that it doesn't continually reset
		//starts up the velocity control task
			startTask (velocityControl);
			while(1 == 1){

				//dummy text for flywheel
				if(vexRT[Btn8D] == 1 ){
					setVelocity(70);
				
				}
4

I tried that anyway and it didn’t work.

5

Use the function “endTimeSlice()”. This splits the time between each task. Some code would look like


task input(){
     while(true){
     //some code
     endTimeSlice();
     }
}
task output(){
     while(true){
     //some code
     endTimeSlice();
     }
}
task velocityControl(){
     while(true){
     //some code
     endTimeSlice();
     }
}
task main(){
     while(true){
     startTask(input);
     startTask(output);
     startTask(velocityControl);
     }
}

Note that the wait1Msec and wait10Msec commands don’t work as expected with this form of multitasking. Instead you will have to use internal timers.

6

Try using a taskVar string that announces to the debugger which task you are currently in so put taskVar=“driver” at the beginning of the driver(main) task while loop and same thing with all your other tasks. It should switch between driver and launcher very fast.

7

velocity control was trying to divide by zero and it was breaking the whole thing. Thanks anyway!

8

Try turning up the gain. You are running in the motor’s velocity and your P constant is the only one initialized, and it’s at 0.01. 0.01x70 = 0.7, which cannot be sent to the motors, and is truncated to zero. Try changing the gain to something higher, probably between 1 & .6 so that it actually gives some power to the motors, and is may begin to oscillate so you can tune it.
Also, I would try to make your code simpler. You have functions running left and right which could be replaced by a single line of code, or even a task that is constantly running. Here is how I do it for my flywheel:


int RPM = 0;
task calcRPM()
{
//calculate RPM constantly with a delay
delay(50);//using this instead of wait1msec causes the task to hold and release to the round-robin
}
task rpmcontrol()
{
//calc error based on RPM found in the calcRPM function and target set in drivermode or autonomous
//set motor speeds
delay(50);// Using delay for both or endtimeslice can make task that don't take 50msec to run end quicker, but start back when needed. 
}

I just keep a constant time interval, and relinquish the tasks to the scheduler, which allows the math behind the RPM to be easy since I don’t have to calculate time changes.

9

I LOVE YOU

10

Why don’t they work?

11

endTimeSlice() works by splitting time between each task. When you have wait functions in there, time cannot be split properly between each task. Strange things happen, like tasks and functions being ignored or getting stuck inside a task.

12

My understanding is that wait statements allow the processor to be used for other tasks until the time is up. Problems arise when the wait time is too small and tasks compete for the same time slices.

13

I don’t believe that is the case. The wait functions release the slice immediately and schedule it to come back after the wait.

14

Really? I’ve tried to implement wait functions with end time slice before and it has never worked for me. I have found that sensors work better for most everything anyways though.

15

Things might happen differently if you use both wait functions and the end time slice function. That should still work though, if I’m not mistaken.

Not sure what sensors have to do with this discussion.

16

Sensors eliminate the need for waits in automated driver functions. Looking back that does seem of topic though.

17

Oh dear, guess I had better explain all of this again, I’m too tired tonight though. I will also link to my original posts on this subject when I have time tomorrow.

Using waits is the/my preferred way of letting the scheduler know that a task is done processing. EndTimeSlice works a little differently but can also be used, all of these techniques play nicely together.

18

…looks like my understanding is way off. Well, glad that I’m still learning new things. Thanks for teaching me @jpearman!

19

Thanks in advance, I certainly want to have a better understanding of this.