Robotc Multitasking Issue

Hello,
I have attempted to apply multitasking to my program, but some unexpected behavior occurred and I am unable to discover why or where the error occurs. The program should flow as follows:

When a button is pressed, it is determined whether or not the button has been pressed before. If so, the


 task speedMotor

task is activated and regulates the speed of the motors, spinning them up to motor power 127 when the


getVelocity()

function returns a value less than the value of


max_vel

, the value returned at motor power 67. When


getVelocity()

returns a value greater than or equal to


max_vel

, the motors are set to 67. This system is supposed to reduce the spin-up time for the motors so that vex balls can be fired faster. The problems comes when the button that activates this process is pressed again to turn it off. When it is pressed again, the motors cut off momentarily before turning back on again, as if


 task speedMotor

was never stopped, but when the button is held down, the motors slow to a stop without jolting back on again. I don’t know if it’s a simple fix and I’ve been staring at a screen for far too long, or that the logic is flawed, but any help would be appreciated. The relevant code is posted below:


#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port7,           leftFireMech,  tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port8,           rightFireMech, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)

#include "JoystickDriver.c"

bool button_toggle = false;//toggle button boolean
float max_vel = 0.616; //velocity of motors at power level 67

float getVelocity(){
   nMotorEncoder[leftFireMech] = 0; //reset encoder

        //check to see if the encoders have moved
   clearTimer(T1); 
   while(time1[T1] < 50) {}
   if(nMotorEncoder[leftFireMech] == 0) return 0;
       
        //get the time it takes for the encoder to count a revolution (vex 393 motor)
   clearTimer(T1);
   while(abs(nMotorEncoder[leftFireMech]) < 627.2){}

   return 627.2 / time1[T1];
}

task speedMotors(){
   while(true){
      if(getVelocity() < max_vel){
         motor[leftFireMech] = 127;
         motor[rightFireMech] = 127;
      }else{
         motor[leftFireMech] = 67;
         motor[rightFireMech] = 67;
      }
      EndTimeslice();
   }
}

task main(){
   while(true){
      getJoystickSettings(joystick);

      if(vexRT[Btn8U] == 1){
         if(button_toggle == false){
            startTask(speedMotors);
            button_toggle = true;
         }else if(button_toggle == true){
            stopTask(speedMotors);
            motor[leftFireMech] = 0;
            motor[rightFireMech] = 0;
            button_toggle = false;
         }
      }
   }
}

The variable button_toggle will change every time around that loop, wait for the button release and you may be ok. Also, you don’t need the getJoystickSettings call unless you are using a PC joystick.

task main(){
   while(true){
     
      if(vexRT[Btn8U] == 1){
         if(button_toggle == false){
            startTask(speedMotors);
            button_toggle = true;
         }else if(button_toggle == true){
            stopTask(speedMotors);
            motor[leftFireMech] = 0;
            motor[rightFireMech] = 0;
            button_toggle = false;
         }
         
         // Wait for button release
         while(vexRT[Btn8U])
           wait1Msec(25);
      }
   }
}