Smooth Dynamic Servo Motion

Our robot requires servos to move smoothly based on user input whithout interrupting any other functions. I wrote this code, and felt that maybe beginner programmers would benifit from it. Also, if anybody has a way to make this more efficient (without using multitasking, because this is for beginner programmers) please let me know.

We wanted to move the servo up if button 6U is pressed, and down if button 6D is pressed. This is more complicated than it sounds, because if you do this:


int servoPos = 0;
while(true){
  if(vexRT[btn6U]) servopos += 1; //vexRT[btn6U] returns true or false based
                                              //on whether it is pressed or not
  if(vexRT[btn6D]) servopos -= 1;
  servo[arm] = servoPos;
}

then the servo will go fully up as soon as you press the button, because the microprocessor cycles through the loop hundreds of times while you are holding the button.

now if we do this:


int servoPos = 0;
while(true){
  if(vexRT[btn6U]){
    servopos += 1;
    wait1MSec(100);//stops the program for a tenth of a second to stop 
                           //servoPos from incrementing too quickly
  }
  if(vexRT[btn6D]){
    servopos -= 1;
    wait1MSec(100);
  }
  servo[arm] = servoPos;
}

then the other controls get interrupted while the program is waiting, and time sensetive functions get messed up.

My solution was to use timers like this:



int servoPos = 110;
int upperLimit = 110;
int lowerLimit = 200;

while(true){

if(vexRT[btn6U]){
  if(time1[T1] > 10){
    ClearTimer(T1);
  }
  else if(time1[T1] < 5 && incServo){
    //move the servo only 5 milliseconds out of 10
    if(servoPos < upperLimit) servoPos += 1;
    //stop servoPos from exeeding upperLimit
  }
}
else if(vexRT[btn6D]){
  if(time1[T1] > 10){
    ClearTimer(T1);
  }
  else if(time1[T1] < 5 && incServo){
    if(servoPos > lowerLimit) servoPos -= 1;
    //stop servoPos from going below lowerLimit
  }
}

servo[arm] = servoPos;

That’s it. Comment if you have questions or see a problem. Thanks.

Of course, the program can be modified to make the servo move faster or slower based on the amount of time that the timer is running before it resets.

Hey dude, nice code. Thanks for sharing it.

This is a thing that many newer programmers find difficult to do.

Haven’t used a servo in a while (kept stripping the gears on them). But back when I had the same problem, I did something similar. From memory my code looked like this:

int damper = 200;
int upperpos = 200;
int lowerpos = 100;

while(true){

     if (vexRT[btn6U] && ClearTimer[T1] > damper && motor[servo1] < upperpos){
       motor[servo1] ++;
       ClearTimer[T1];
     }else if (vexRT[btn6D] && ClearTimer[T1] > damper && motor[servo1] > lowerpos){
       motor[servo1] --;
       ClearTimer[T1];
     }

}

Pikey ~ Team aMEss, Massey University, New Zealand

What exactly is your code trying to do? From what I see, it wouldn’t even compile.

Sorry, made a small mistake in the last post wrote ClearTimer instead of Time1.

It was pretty straight forward. I am just adding a time delay.

If the button is pressed, **and **the timer is above the time delay, **and **the servo is not past it’s limit, then increment or decrement the servo position and reset the timer.

#pragma config(Motor,  port1,           servo1,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int damper = 200;
int upperpos = 200;
int lowerpos = 100;

task main(){
	while(true){
	
	  if (vexRT[btn6U] && time1[T1] > damper && motor[servo1] < upperpos){
	    motor[servo1] ++;
	    ClearTimer(T1);
	  }else if (vexRT[btn6D] && time1[T1] > damper && motor[servo1] > lowerpos){
	    motor[servo1] --;
	    ClearTimer(T1);
	  }
	}
}