View unanswered posts | View active topics It is currently Wed Jun 20, 2018 8:29 pm






Reply to topic  [ 6 posts ] 
PID Speed Control for corte 
Author Message
Rookie

Joined: Mon Dec 21, 2015 1:13 am
Posts: 4
Post PID Speed Control for corte
Hi, I am new to PID control and RobotC. I have few problems when trying to program two motors with IME encoder using PID control. First, in Motor and Sensor Configuration, there is a PID control option but when I search online, the document says I have to use nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg; to enable PID control -- Which way is the right way to turn on internal PID control? Second, is it OK to use internal PID control to control the speed of two motors or I have to program PID algorithm myself -- if so, can anyone give a sample of PID speed control code? I am really confused right now. I really appreciate any help! Thanks a lot!


Mon Dec 21, 2015 1:21 am
Profile
Moderator
Moderator

Joined: Tue May 19, 2015 3:07 pm
Posts: 91
Post Re: PID Speed Control for corte
RyanPoint wrote:
Hi, I am new to PID control and RobotC. I have few problems when trying to program two motors with IME encoder using PID control. First, in Motor and Sensor Configuration, there is a PID control option but when I search online, the document says I have to use nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg; to enable PID control -- Which way is the right way to turn on internal PID control? Second, is it OK to use internal PID control to control the speed of two motors or I have to program PID algorithm myself -- if so, can anyone give a sample of PID speed control code? I am really confused right now. I really appreciate any help! Thanks a lot!

Using the internal PID control is perfectly fine :)

nMotorPIDSpeedCtrl is the manual way to set the internal PID control but it has the same purpose as setting it motor and sensor setup. I would suggest using Motor and Sensor Setup as it is more user friendly and cleaner. If you in the future wanted to turn off PID for some reason nMotorPIDSpeedCtrl is really good because you can't adjust the graphical motor setup mid program.


Tue Dec 22, 2015 7:24 am
Profile
Rookie

Joined: Mon Dec 21, 2015 1:13 am
Posts: 4
Post Re: PID Speed Control for corte
Tabor473 wrote:
RyanPoint wrote:
Hi, I am new to PID control and RobotC. I have few problems when trying to program two motors with IME encoder using PID control. First, in Motor and Sensor Configuration, there is a PID control option but when I search online, the document says I have to use nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg; to enable PID control -- Which way is the right way to turn on internal PID control? Second, is it OK to use internal PID control to control the speed of two motors or I have to program PID algorithm myself -- if so, can anyone give a sample of PID speed control code? I am really confused right now. I really appreciate any help! Thanks a lot!

Using the internal PID control is perfectly fine :)

nMotorPIDSpeedCtrl is the manual way to set the internal PID control but it has the same purpose as setting it motor and sensor setup. I would suggest using Motor and Sensor Setup as it is more user friendly and cleaner. If you in the future wanted to turn off PID for some reason nMotorPIDSpeedCtrl is really good because you can't adjust the graphical motor setup mid program.

I now can't change motor speed after PID is enabled using Motor and Sensor Configuration -- the motor always run on the same speed even if I change the value of motor[] in launchBallLeft task
Code:
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  mode1,          sensorDigitalIn)
#pragma config(Sensor, dgtl2,  mode2,          sensorDigitalIn)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           baseR,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           baseR,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           lift1,         tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           launch1,       tmotorVex393TurboSpeed_MC29, PIDControl, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port6,           launch2,       tmotorVex393TurboSpeed_MC29, PIDControl, encoderPort, I2C_2)
#pragma config(Motor,  port7,           grab,          tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           bassL,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           baseL,         tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

int firstSpeed;
int secondSpeed;
int thirdSpeed;
int V1;

void setSpeed(int a, int b, int c){
   firstSpeed = a;
   secondSpeed = b;
   thirdSpeed = c;
}
task baseControl(){
   while(true){

      motor[port2] = vexRT[Ch3]-vexRT[Ch1]*1.5; //right
      motor[port3] = vexRT[Ch3]-vexRT[Ch1]*1.5;
      motor[port8] = vexRT[Ch3]+vexRT[Ch1]*1.5;
      motor[port9] = vexRT[Ch3]+vexRT[Ch1]*1.5;

   }
}

task shiftBase(){
   while(true){
   if(vexRT[Btn8R] == 1){
      clearTimer(T1);
      while(time1[T1] < 150){
      motor[port2] = -75;
      motor[port3] = -75;
      motor[port8] = 75;
      motor[port9] = 75;
   }
   }
   if(vexRT[Btn8L] == 1){
         clearTimer(T1);
      while(time1[T1] < 150){
      motor[port2] = 75;
      motor[port3] = 75;
      motor[port8] = -75;
      motor[port9] = -75;
   }
   }
   else if(time1[T1] >150){
      motor[port2] = 0;
      motor[port3] = 0;
      motor[port8] = 0;
      motor[port9] = 0;
   }
}
}

task lift(){

   while(true){
      if(vexRT[Btn6U] == 1){
         motor[port4] = 127;
      }
      if(vexRT[Btn6D] == 1){
         motor[port4] = -127;
      }
      else if(vexRT[Btn6D] == 0 && vexRT[Btn6U] == 0){
         motor[port4] = 0;
      }
   }
}


task launchBallLeft(){
   int speed = 0;
   resetSensor(I2C_1);
   while(true){
      if(vexRT[Btn7U] == 1){
         speed = firstSpeed;
         motor[port5] = speed;
         motor[port6] = speed;
      }
      if(vexRT[Btn7L] == 1){
         speed = secondSpeed;
         motor[port5] = speed;
         motor[port6] = speed;
      }
      if(vexRT[Btn7R] == 1){
         speed = thirdSpeed;
         motor[port5] = 50;
         motor[port6] = 50;
      }

      else if(vexRT[Btn7D] == 1){
         speed = 0;
         motor[port5] = speed;
         motor[port6] = speed;
      }

   }
}

task launchBallRight(){

   int speed = 0;
   setPIDforMotor(launch1,true);
   setPIDforMotor(launch2,true);
   while(true){
      if(vexRT[Btn8U] == 1){
         speed = firstSpeed;
         motor[port5] = speed;
         motor[port6] = speed;
      }
      if(vexRT[Btn8D] == 1){
         speed = secondSpeed;
         motor[port5] = speed;
         motor[port6] = speed;
      }
      else if(vexRT[Btn8D] == 0 && vexRT[Btn8U] == 0){
         speed = 0;
         motor[port5] = speed;
         motor[port6] = speed;
      }

   }
}
task grabControl(){
   while(true){
      if(vexRT[Btn5U] == 1){
         motor[port7] = -127;
      }
      else if(vexRT[Btn5D] == 1){
         motor[port7] = 127;
      }
      else if(vexRT[Btn5D] == 0 && vexRT[Btn5U] == 0){
         motor[port7] = 0;
      }
   }
}
task autoMode1(){
   clearTimer(timer1);
   while(true){
   motor[port5] = 80;
   motor[port6] = 80;
   motor[port7] = 0;
   if(time1[T1] == 3000){
      while(time1[T1]<=3500){
     motor[port7] = -127;
   }
      clearTimer(timer1);
   }
   }
}
void pre_auton()
{
   // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
   // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
   bStopTasksBetweenModes = true;

   // All activities that occur before the competition starts
   // Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
task autonomous(){
   if(SensorValue[mode1] == 0){
   startTask(autoMode1);
}
if(SensorValue[mode2] == 0){
   while(true){
      motor[port2] = 80;
      motor[port3] = 80;
      motor[port8] = 80;
      motor[port9] = 80;
   }
}
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
   // User control code here, inside the loop
   setSpeed(99,94,50);
   while (true)
   {
      V1 = getMotorVelocity(launch1);
      if(vexRT[Btn7U] == 1 || vexRT[Btn7L] == 1 || vexRT[Btn7R] == 1){
         startTask(launchBallLeft);
         stopTask(launchBallRight);
      }
      else if(vexRT[Btn8U] == 1 || vexRT[Btn8D] == 1){
         stopTask(launchBallLeft);
         startTask(launchBallRight);
      }
      startTask(baseControl);
      startTask(grabControl);
      startTask(lift);
      startTask(shiftBase);

   }
}


Tue Dec 22, 2015 3:36 pm
Profile
Moderator
Moderator

Joined: Tue May 19, 2015 3:07 pm
Posts: 91
Post Re: PID Speed Control for corte
Okay so wow. I would very much recommend you take a look at this thread on multitasking.
viewtopic.php?f=11&t=3341

Reading through your program it is almost certainly a multitasking problem because so many tasks are running pointlessly on top of each other. My first guess would be autoMode1 is not stopped ever so in driver control it is still controlling the flywheel.


Sun Dec 27, 2015 5:50 pm
Profile
Rookie

Joined: Mon Dec 21, 2015 1:13 am
Posts: 4
Post Re: PID Speed Control for corte
Tabor473 wrote:
Okay so wow. I would very much recommend you take a look at this thread on multitasking.
http://robotc.net/forums/viewtopic.php?f=11&t=3341

Reading through your program it is almost certainly a multitasking problem because so many tasks are running pointlessly on top of each other. My first guess would be autoMode1 is not stopped ever so in driver control it is still controlling the flywheel.


I am confused -- so the multitasking or multifunction is different from that of language like Java?


Wed Jan 06, 2016 9:18 pm
Profile
Moderator
Moderator

Joined: Tue May 19, 2015 3:07 pm
Posts: 91
Post Re: PID Speed Control for corte
So in preparing myself to try to explain this concept (its hard to conceptualize the first time) I looked up some terms to get myself acclimated with explanations and actually I found one I just already really like so.

"A race condition is a special condition that may occur inside a critical section. A critical section is a section of code that is executed by multiple threads and where the sequence of execution for the threads makes a difference in the result of the concurrent execution of the critical section"

Multiple tasks running at the same time that both set a variable(like a motor) can run into issues.
Imagine for instance you walk up to a deck of cards and put an ace on top. Then your teammate walks up and puts a king on top. At any point in this game the robot checks the top card and assigns a motor to that value. Neither of you know whose card was chosen and its unclear to the robot if this is all happening over and over whose card should be chosen.

I made a handful of minor tweaks to the program to get this point across
Code:
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  mode1,          sensorDigitalIn)
#pragma config(Sensor, dgtl2,  mode2,          sensorDigitalIn)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           baseR,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           baseR,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           lift1,         tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           launch1,       tmotorVex393TurboSpeed_MC29, PIDControl, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port6,           launch2,       tmotorVex393TurboSpeed_MC29, PIDControl, encoderPort, I2C_2)
#pragma config(Motor,  port7,           grab,          tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           bassL,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           baseL,         tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

int firstSpeed;
int secondSpeed;
int thirdSpeed;
int V1;

void setSpeed(int a, int b, int c){
   firstSpeed = a;
   secondSpeed = b;
   thirdSpeed = c;
}
bool shiftingBase=false;//variable to tell base control not to control the base because shift base is
task baseControl(){
   while(true){
      if(shiftingBase==false){
         motor[port2] = vexRT[Ch3]-vexRT[Ch1]*1.5; //right
         motor[port3] = vexRT[Ch3]-vexRT[Ch1]*1.5;
         motor[port8] = vexRT[Ch3]+vexRT[Ch1]*1.5;
         motor[port9] = vexRT[Ch3]+vexRT[Ch1]*1.5;
      }
   }
}
task shiftBase(){
   while(true){
      if(vexRT[Btn8R] == 1){
         clearTimer(T1);
         shiftingBase=true;
         while(time1[T1] < 150){

            motor[port2] = -75;
            motor[port3] = -75;
            motor[port8] = 75;
            motor[port9] = 75;
         }
      }
      if(vexRT[Btn8L] == 1){
         clearTimer(T1);
         shiftingBase=true;
         while(time1[T1] < 150){

            motor[port2] = 75;
            motor[port3] = 75;
            motor[port8] = -75;
            motor[port9] = -75;
         }
      }
      else if(time1[T1] >150){

         motor[port2] = 0;
         motor[port3] = 0;
         motor[port8] = 0;
         motor[port9] = 0;
      }
      shiftingBase=false;//yield control of motor back to other task
      wait1Msec(50);//Don't hog CPU
   }
}

task lift(){

   while(true){
      if(vexRT[Btn6U] == 1){
         motor[port4] = 127;
      }
      if(vexRT[Btn6D] == 1){
         motor[port4] = -127;
      }
      else if(vexRT[Btn6D] == 0 && vexRT[Btn6U] == 0){
         motor[port4] = 0;
      }
   }
}


task launchBallLeft(){
   int speed = 0;
   while(true){
      if(vexRT[Btn7U] == 1){
         speed = firstSpeed;
         motor[port5] = speed;
         motor[port6] = speed;
      }
      if(vexRT[Btn7L] == 1){
         speed = secondSpeed;
         motor[port5] = speed;
         motor[port6] = speed;
      }
      if(vexRT[Btn7R] == 1){
         speed = thirdSpeed;
         motor[port5] = 50;
         motor[port6] = 50;
      }

      else if(vexRT[Btn7D] == 1){
         speed = 0;
         motor[port5] = speed;
         motor[port6] = speed;
      }

   }
}

task launchBallRight(){

   int speed = 0;
   while(true){
      if(vexRT[Btn8U] == 1){
         speed = firstSpeed;
         motor[port5] = speed;
         motor[port6] = speed;
      }
      if(vexRT[Btn8D] == 1){
         speed = secondSpeed;
         motor[port5] = speed;
         motor[port6] = speed;
      }
      else if(vexRT[Btn8D] == 0 && vexRT[Btn8U] == 0){
         speed = 0;
         motor[port5] = speed;
         motor[port6] = speed;
      }
      wait1Msec(50);//Don't hog CPU
   }
}
task grabControl(){
   while(true){
      if(vexRT[Btn5U] == 1){
         motor[port7] = -127;
      }
      else if(vexRT[Btn5D] == 1){
         motor[port7] = 127;
      }
      else if(vexRT[Btn5D] == 0 && vexRT[Btn5U] == 0){
         motor[port7] = 0;
      }
      wait1Msec(50);//Don't hog CPU
   }
}
task autoMode1(){
   clearTimer(timer1);
   while(true){
      motor[port5] = 80;
      motor[port6] = 80;
      motor[port7] = 0;
      if(time1[T1] == 3000){
         while(time1[T1]<=3500){
            motor[port7] = -127;
            wait1Msec(50);//Don't hog CPU
         }
         clearTimer(timer1);
      }
   }
}
void pre_auton()
{
   // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
   // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
   bStopTasksBetweenModes = true;

   // All activities that occur before the competition starts
   // Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
task autonomous(){
   if(SensorValue[mode1] == 0){
      startTask(autoMode1);
   }
   if(SensorValue[mode2] == 0){
      while(true){
         motor[port2] = 80;
         motor[port3] = 80;
         motor[port8] = 80;
         motor[port9] = 80;
      }
   }
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
   stopTask(autoMode1);//If nothing tells this task to stop it will still be setting the motors constantly
   startTask(baseControl);//All of these only need to be started once
   startTask(grabControl);
   startTask(lift);
   startTask(shiftBase);


   // User control code here, inside the loop
   setSpeed(99,94,50);
   while (true)
   {
      V1 = getMotorVelocity(launch1);
      if(vexRT[Btn7U] == 1 || vexRT[Btn7L] == 1 || vexRT[Btn7R] == 1){
         startTask(launchBallLeft);
         stopTask(launchBallRight);
      }
      else if(vexRT[Btn8U] == 1 || vexRT[Btn8D] == 1){
         stopTask(launchBallLeft);
         startTask(launchBallRight);
      }
      wait1Msec(50);//Don't hog CPU

   }
}


Then I made a few slightly more drastic changes to show how eliminating multitasking makes the program far simpler. It also runs using (something like) 1/5 of the processing power.

Code:
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  mode1,          sensorDigitalIn)
#pragma config(Sensor, dgtl2,  mode2,          sensorDigitalIn)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           baseR,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           baseR,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           lift1,         tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           launch1,       tmotorVex393TurboSpeed_MC29, PIDControl, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port6,           launch2,       tmotorVex393TurboSpeed_MC29, PIDControl, encoderPort, I2C_2)
#pragma config(Motor,  port7,           grab,          tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           bassL,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           baseL,         tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

int firstSpeed;
int secondSpeed;
int thirdSpeed;
int V1;

void setSpeed(int a, int b, int c){
   firstSpeed = a;
   secondSpeed = b;
   thirdSpeed = c;
}
bool shiftingBase=false;//variable to tell base control not to control the base because shift base is
void baseControl(){
   if(shiftingBase==false){
      motor[port2] = vexRT[Ch3]-vexRT[Ch1]*1.5; //right
      motor[port3] = vexRT[Ch3]-vexRT[Ch1]*1.5;
      motor[port8] = vexRT[Ch3]+vexRT[Ch1]*1.5;
      motor[port9] = vexRT[Ch3]+vexRT[Ch1]*1.5;
   }

}

task shiftBase(){//because this is its own task all the while statements are not necessary, you could just put wait commands
   while(true){
      if(vexRT[Btn8R] == 1){
         clearTimer(T1);
         shiftingBase=true;
         motor[port2] = -75;
         motor[port3] = -75;
         motor[port8] = 75;
         motor[port9] = 75;
         wait1Msec(150);
      }
      if(vexRT[Btn8L] == 1){
         shiftingBase=true;
         motor[port2] = 75;
         motor[port3] = 75;
         motor[port8] = -75;
         motor[port9] = -75;
         wait1Msec(150);
      }
      /*else if(time1[T1] >150){  //Didn't seem necessary so I removed it

         motor[port2] = 0;
         motor[port3] = 0;
         motor[port8] = 0;
         motor[port9] = 0;
      }*/
      shiftingBase=false;//yield control of motor back to other task
      wait1Msec(50);//Don't hog CPU
   }
}

void lift(){
   if(vexRT[Btn6U] == 1){
      motor[port4] = 127;
   }
   if(vexRT[Btn6D] == 1){
      motor[port4] = -127;
   }
   else if(vexRT[Btn6D] == 0 && vexRT[Btn6U] == 0){
      motor[port4] = 0;
   }

}


void launchBallLeft(){
   int speed = 0; //is useless as every case already sets speed

   if(vexRT[Btn7U] == 1){
      speed = firstSpeed;
      motor[port5] = speed;
      motor[port6] = speed;
   }
   if(vexRT[Btn7L] == 1){
      speed = secondSpeed;
      motor[port5] = speed;
      motor[port6] = speed;
   }
   if(vexRT[Btn7R] == 1){
      speed = thirdSpeed;
      motor[port5] = 50; // is this on purpose?
      motor[port6] = 50;
   }

   else if(vexRT[Btn7D] == 1){
      speed = 0;
      motor[port5] = speed;
      motor[port6] = speed;
   }

}

void launchBallRight(){//this is streamlined version, see Left for equivalent of original
   if(vexRT[Btn8U] == 1){
      speed = firstSpeed;
   }
   if(vexRT[Btn8D] == 1){
      speed = secondSpeed;
   }
   else if(vexRT[Btn8D] == 0 && vexRT[Btn8U] == 0){
      speed = 0;
   }
   motor[port5] = speed; //every case does this so moved it outside for cleanlyness
   motor[port6] = speed;
}
void grabControl(){
   if(vexRT[Btn5U] == 1){
      motor[port7] = -127;
   }
   else if(vexRT[Btn5D] == 1){
      motor[port7] = 127;
   }
   else if(vexRT[Btn5D] == 0 && vexRT[Btn5U] == 0){
      motor[port7] = 0;
   }
}
task autoMode1(){
   clearTimer(timer1);
   while(true){
      motor[port5] = 80;
      motor[port6] = 80;
      motor[port7] = 0;
      if(time1[T1] == 3000){
         while(time1[T1]<=3500){
            motor[port7] = -127;
            wait1Msec(50);//Don't hog CPU
         }
         clearTimer(timer1);
      }
   }
}
void pre_auton()
{
   // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
   // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
   bStopTasksBetweenModes = true;

   // All activities that occur before the competition starts
   // Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
task autonomous(){
   if(SensorValue[mode1] == 0){
      startTask(autoMode1);
   }
   if(SensorValue[mode2] == 0){
      while(true){
         motor[port2] = 80;
         motor[port3] = 80;
         motor[port8] = 80;
         motor[port9] = 80;
      }
   }
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
   stopTask(autoMode1);//If nothing tells this task to stop it will still be setting the motors constantly
   startTask(shiftBase);//only use tasks when you need to have hard coded delays inside an action, this is a decent use of one


   // User control code here, inside the loop
   setSpeed(99,94,50);
   while (true)
   {
      baseControl();//these are now getting called constantly inside the while loop and because they take no time an additional task and while loop is not needed
      grabControl();
      lift();
      V1 = getMotorVelocity(launch1);//You never use this but I will just leave it here for later
      if(vexRT[Btn7U] == 1 || vexRT[Btn7L] == 1 || vexRT[Btn7R] == 1){
         launchBallLeft();
      }
      else if(vexRT[Btn8U] == 1 || vexRT[Btn8D] == 1){
         launchBallRight();
      }
      wait1Msec(50);//Don't hog CPU

   }
}


Fri Jan 08, 2016 10:59 am
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.