FREE Servo RC Controller code [ALMOST WORKING]

General discussion on mikroC.
Author
Message
spookyrufus
Posts: 71
Joined: 10 Jun 2006 13:45

FREE Servo RC Controller code [ALMOST WORKING]

#1 Post by spookyrufus » 02 Jun 2008 19:34

Hi there, I'm going to release this small piece of MikroC code which lets you to control up to 12 Servos using USART.
I've spent a lot of time trying to accomplish that goal and I want to share this with you.
however, before doing that, I've to solve a small problem which is getting annoying:

RC Positions are not so precise: I mean, sometimes when I tell the PIC to move one motor to a certain position, as the motor reaches it, it starts 'vibrating' and seems 'unstable'.

I'm sure this is realted to the way I'm generating the pulse, but I don't know why sometimes this happens, sometimes not.

I also don't know why the result of such a problem should be an 'unstable' motor: even if not correct, the signal should be periodic and constant: why the hell that motor is vibrating?

Any help would be appreciated, and sorry for my bad english.

xor
Posts: 5465
Joined: 18 May 2005 00:59
Location: NYC
Contact:

Re: FREE Servo RC Controller code [ALMOST WORKING]

#2 Post by xor » 02 Jun 2008 21:42

spookyrufus wrote:Hi there, I'm going to release this small piece of MikroC code which lets you to control up to 12 Servos using USART.
I've spent a lot of time trying to accomplish that goal and I want to share this with you.
however, before doing that, I've to solve a small problem which is getting annoying:

RC Positions are not so precise: I mean, sometimes when I tell the PIC to move one motor to a certain position, as the motor reaches it, it starts 'vibrating' and seems 'unstable'.

I'm sure this is realted to the way I'm generating the pulse, but I don't know why sometimes this happens, sometimes not.

I also don't know why the result of such a problem should be an 'unstable' motor: even if not correct, the signal should be periodic and constant: why the hell that motor is vibrating?

Any help would be appreciated, and sorry for my bad english.
Working servo code for control of up to 33 servos on a single PIC with no glitches is not a secret in these forums.

It's hard to get help without showing your code. Kind of takes the guesswork out of your problems.
[color=darkred][b]xor[/b][/color]
[url=http://circuit-ed.com]CircuitED -[/url]

spookyrufus
Posts: 71
Joined: 10 Jun 2006 13:45

Ok...

#3 Post by spookyrufus » 03 Jun 2008 08:15

Well, I know it's not a secret, but I haven't seen any working MikroC code doind that, so I thought someone would be interested.
Long time ago I also tried using your MikroBasic code (If i remember correct) to precisely control up to N servos (where N is reeealy big) but I found it a bit complicated.
So I tried to write my own.
I also didn't post in the hope that this would have been a common problem, and because of course the code is not short.
Well, here is it. If you have questions about it, please tell me. Please consider this is a FIRST version and all optimizations are not there yet (also, variables names are to be reviewed!).

The macro CORRECTION_DELAY is used to empirically correct the positioning error due to instructions execution times. Not a good, but working solution.

I spent a looooong time trying to add comments in English... sorry if code is still unreadable somewhere...

Code: Select all

#define CORRECTION_DELAY 210
#define CENTER 0
#define LOWEND 1
#define HIGHEND 2
#define CENTERLOWEND 3
#define CENTERHIGHEND 4

#define MOTOR1 1
#define MOTOR1_PIN PORTB.F7
#define MOTOR2 2
#define MOTOR2_PIN PORTB.F6
#define MOTOR3 3
#define MOTOR3_PIN PORTB.F5
#define MOTOR4 4
#define MOTOR4_PIN PORTB.F4
#define MOTOR5 5
#define MOTOR5_PIN PORTB.F3
#define MOTOR6 6
#define MOTOR6_PIN PORTB.F2
#define MOTOR7 7
#define MOTOR7_PIN PORTB.F1
#define MOTOR8 8
#define MOTOR8_PIN PORTB.F0
#define MOTOR9 9
#define MOTOR9_PIN PORTC.F0
#define MOTOR10 10
#define MOTOR10_PIN PORTC.F1
#define MOTOR11 11
#define MOTOR11_PIN PORTC.F2
#define MOTOR12 12
#define MOTOR12_PIN PORTC.F3

unsigned motorposition[12];  // Array containing motor positions


unsigned cnt1=0,cnt2=0,cnt3=0,cnt4=0,cnt5=0,cnt6=0,cnt7=0,cnt8=0,cnt9=0,cnt10=0,cnt11=0,cnt12=0;  // motors counters
short command=100; // should not be 0
unsigned char motsel=0,possel=0;

void motor_position(unsigned motor, unsigned position);  // This function will position a motor
void invert_signal_front(unsigned motor); // motor_pin=~motor_pin
void delay(unsigned position); // Creates a suitable delay


void interrupt() {
  cnt1++;   // Updates counters
  cnt2++;
  cnt3++;
  cnt4++;
  cnt5++;
  cnt6++;
  cnt7++;
  cnt8++;
  cnt9++;
  cnt10++;
  cnt11++;
  cnt12++;
  
  TMR0   = 55;          // Reset TMR0 settings
  INTCON = 0b10100000;
}

void main() {
  OPTION_REG = 0b11000000; // Startup settings
  TRISC=0x00;
  PORTC=0x00;
  Usart_Init(2400);
  
  TRISD = 0x00;
  PORTD = 0x00;
  TRISB=0x00;
  PORTB = 0x00;
  TRISA=0x00;
  PORTA=0x00;


  TMR0  = 55;              // Timer0 initial value
  INTCON = 0b10100000;           // Enable TMRO interrupt

  motorposition[0]=CENTER;   // Manual initialization, for some reason, putting this into a for loop won't work (?)
  motorposition[1]=CENTER;
  motorposition[2]=CENTER;
  motorposition[3]=CENTER;
  motorposition[4]=CENTER;
  motorposition[5]=CENTER;
  motorposition[6]=CENTER;
  motorposition[7]=CENTER;
  motorposition[8]=CENTER;
  motorposition[9]=CENTER;
  motorposition[10]=CENTER;
  motorposition[11]=CENTER;


  do {
      if(Usart_Data_Ready()){ // Command ready to be read
           command=Usart_Read();
           if(command>0){ // If a valid command has been received
             command--;
             possel=command & 0b00000111; // Lower 3 bits are motor position
             motsel=command>>3; // 4th to 7th bits are used to select motor
             motorposition[motsel]=possel; // Updates motor position
           }
          PORTD=command; // Reflects command onto PORTD (just for debugging purposes)
          //motorposition[motor_select-1]=motor_pos;


        }


      if(cnt1>=250){  // Updates motors by calling motor_position and resetting counters
                  motor_position(MOTOR1,motorposition[0]);
                  cnt1=0;
                  }
      if(cnt2>=250){
                  motor_position(MOTOR2,motorposition[1]);
                  cnt2=0;
                  }
      if(cnt3>=250) {
                  motor_position(MOTOR3,motorposition[2]);
                  cnt3=0;
                  }
      if(cnt4>=250) {
                  motor_position(MOTOR4,motorposition[3]);
                  cnt4=0;
                  }
      if(cnt5>=250) {
                  motor_position(MOTOR5,motorposition[4]);
                  cnt5=0;
                  }
      if(cnt6>=250) {
                  motor_position(MOTOR6,motorposition[5]);
                  cnt6=0;
                  }
      if(cnt7>=250) {
                  motor_position(MOTOR7,motorposition[6]);
                  cnt7=0;
                  }
      if(cnt8>=250) {
                  motor_position(MOTOR8,motorposition[7]);
                  cnt8=0;
                  }
      if(cnt9>=250) {
                  motor_position(MOTOR9,motorposition[8]);
                  cnt9=0;
                  }
      if(cnt10>=250) {
                  motor_position(MOTOR10,motorposition[9]);
                  cnt10=0;
                  }
      if(cnt11>=250) {
                  motor_position(MOTOR11,motorposition[10]);
                  cnt11=0;
                  }
      if(cnt12>=250) {
                  motor_position(MOTOR12,motorposition[11]);
                  cnt12=0;
                  }
    } while(1);
}

void motor_position(unsigned motor, unsigned position){   // Sposta un motore in una determinata posizione
          invert_signal_front(motor);
          delay(position);
          invert_signal_front(motor);
}

void delay(unsigned position){       // Genera un ritardo
 switch(position){
                  case CENTER:
                       Delay_us(1500-CORRECTION_DELAY);
                       break;
                  case HIGHEND:
                       Delay_us(2100-CORRECTION_DELAY);
                       break;
                  case LOWEND:
                       Delay_us(900-CORRECTION_DELAY);
                       break;
                  case CENTERLOWEND:
                       Delay_us(1200-CORRECTION_DELAY);
                       break;
                  case CENTERHIGHEND:
                       Delay_us(1800-CORRECTION_DELAY);
                       break;
 }
}


void invert_signal_front(unsigned motor){    // Inverte il segnale sul pin del motore
switch(motor){
              case MOTOR1:
                   MOTOR1_PIN=~MOTOR1_PIN;
                   break;
              case MOTOR2:
                   MOTOR2_PIN=~MOTOR2_PIN;
                   break;
              case MOTOR3:
                   MOTOR3_PIN=~MOTOR3_PIN;
                   break;
              case MOTOR4:
                   MOTOR4_PIN=~MOTOR4_PIN;
                   break;
              case MOTOR5:
                   MOTOR5_PIN=~MOTOR5_PIN;
                   break;
              case MOTOR6:
                   MOTOR6_PIN=~MOTOR6_PIN;
                   break;
              case MOTOR7:
                   MOTOR7_PIN=~MOTOR7_PIN;
                   break;
              case MOTOR8:
                   MOTOR8_PIN=~MOTOR8_PIN;
                   break;
              case MOTOR9:
                   MOTOR9_PIN=~MOTOR9_PIN;
                   break;
              case MOTOR10:
                   MOTOR10_PIN=~MOTOR10_PIN;
                   break;
              case MOTOR11:
                   MOTOR11_PIN=~MOTOR11_PIN;
                   break;
              case MOTOR12:
                   MOTOR12_PIN=~MOTOR12_PIN;
                   break;
}
}

Puggs
Posts: 179
Joined: 17 Nov 2007 23:05
Location: Melbourne Australia
Contact:

Stupid Question

#4 Post by Puggs » 03 Jun 2008 12:12

Ok This may be stupid

You have cnt1-12 one for each motor, all start as 0 and all get updated by 1 via at the same timer by an interrupt. when they all reach 250 you do stuff.

This means that they all reach 250 at the same time. so basicly you are using 12 counts when you only need one. So you basicly waisting a lot of instruction cycles.

if you were to stager the starting at 10-18 apart (cnt1 = 0, cnt2 = 15, cnt3 = 30) only one motor will be moving at one time and you will have less of a chance of an interupt being called while a motor was being moved.

Actualy looking at it even closer there is a better design.

1. Set up a interrup and counter to produce an event ever usec

2. if the countdown is 0 then it is set to what every delay value for it's position if it is not set as NOMOVE = 0 (CENTER = 5) once you set the coundown value, the set the psoition to NOMOVE and then turn the motor on.

3. each motor has an its own countdown which is decresed by 1 every us if it is > 0

4. once the coundown = 0 turn the motor off.

5. go to step 2

Your problem with the jitters is that each time your counters reach 250 you tell the motor to move, even if it is meant to move to the same position as before (eg move CENTER, move CENTER, move CENTER)

Hope this is helpful

Puggs

spookyrufus
Posts: 71
Joined: 10 Jun 2006 13:45

?

#5 Post by spookyrufus » 03 Jun 2008 12:21

Servos need the signal to be constant to maintain position. This is why I continue sending the same signal also if the position has already been reached.

Puggs
Posts: 179
Joined: 17 Nov 2007 23:05
Location: Melbourne Australia
Contact:

Re: ?

#6 Post by Puggs » 03 Jun 2008 12:50

spookyrufus wrote:Servos need the signal to be constant to maintain position. This is why I continue sending the same signal also if the position has already been reached.
The problem with you code its all done in the mail loop, so if all motors were set to CENTER it would be at least 14,190uSec before it got back around to sending the position to any motor eg

Code: Select all

     ________         14,190us
M1 _| 1290 us|_______________________________________________|
              ________
M2 __________| 1290 us|_______________________________________
                       ________
M3 ___________________| 1290 us|______________________________
with my sugestion there would be a minimum of 1us between each transition from high to low to high if you didn't use the NOMOVE settings

Puggs

ps. you would not beleive the number of previews i need to get that right :)

trust issues
Posts: 231
Joined: 14 Nov 2004 19:43

#7 Post by trust issues » 03 Jun 2008 13:53

This is an unrelated questions but since you guys know so much about servos :D
Do you know of any decent heavy duty servos? I'd like to use one to move a accelerator/throttle cable in my car and the model servos surely won't do the job. Thanks

spookyrufus
Posts: 71
Joined: 10 Jun 2006 13:45

thx

#8 Post by spookyrufus » 03 Jun 2008 13:55

Ok thanks I think I got where's the problem (I'm still approaching electronics so even simple things may become huge problems in my head).
And your square wave graph is worth all the time you spent on it!

I'm trying to fix the code next days....tomorrow i'm at university for an exam....cross fingers please. :)

Acetronics
Posts: 715
Joined: 27 Dec 2006 14:33
Location: Le Tréport , FRANCE

#9 Post by Acetronics » 03 Jun 2008 14:03

Hi, Trust

The main question will be ... which model servo will be reliable enough to drive my car throttle ???

Now, which are the effort and stroke you want ???

1kg ... 10 kg ???

1cm ... 10 cm ... 25 cm ???

a servo close to its maximum data will be very slow, and also remember the throttle spring will make it work all time ...

so, overheating problems ALSO can easily appear ... :cry:

Alain

Puggs
Posts: 179
Joined: 17 Nov 2007 23:05
Location: Melbourne Australia
Contact:

#10 Post by Puggs » 03 Jun 2008 14:15

trust issues wrote:This is an unrelated questions but since you guys know so much about servos :D
Do you know of any decent heavy duty servos? I'd like to use one to move a accelerator/throttle cable in my car and the model servos surely won't do the job. Thanks
Sorry, don't know a thing about servos, i just can see what he is trying to achive and was able to think of a better way to do it.

Puggs

xor
Posts: 5465
Joined: 18 May 2005 00:59
Location: NYC
Contact:

#11 Post by xor » 03 Jun 2008 18:59

50Hz servos must be updated... turned ON .... every 20ms. So it is important to manage that timing correctly, usually by creating an interrupt that occurs every 20ms. You can also create shorter timed interrupts and then keep track until you reach a total 20ms.

The simplest approach is to turn ALL servos ON at the same time (the start of a 20ms period) and then count down the required time for each servo's ON time, which determines its rotation position.

50Hz servos have a center position based on a 1500us ON time. Once 1500us of powered timeout is reached, then the servo is not powered for 18500us, or until the next 20ms Interrupt occurs. Then the ON time cycles of each servo repeat if you want to hold the motor in the same position, or change the ON time to move the motor into a new position.

Maintaining a consistent 20ms cycle for each servo will remove the jitter. Also, keep the ON time of each servo within the specs of the servo manufacture and all will be well (rotation timing between 1ms to 2ms is always a safe bet).
[color=darkred][b]xor[/b][/color]
[url=http://circuit-ed.com]CircuitED -[/url]

xor
Posts: 5465
Joined: 18 May 2005 00:59
Location: NYC
Contact:

#12 Post by xor » 03 Jun 2008 19:36

trust issues wrote:This is an unrelated questions but since you guys know so much about servos :D
Do you know of any decent heavy duty servos? I'd like to use one to move a accelerator/throttle cable in my car and the model servos surely won't do the job. Thanks
They make 50Hz servos of all different torques, small and large. Just take a look at the mechanics of those 200 pound robot-wars machines, fast and strong. The one you choose likely depends on how fast you need to reach a certain position under load.

You can use a low torque motor and gear it, and make it the little big-man. If the resolution and speed of change under load is sufficient, then it's all you need.
[color=darkred][b]xor[/b][/color]
[url=http://circuit-ed.com]CircuitED -[/url]

trust issues
Posts: 231
Joined: 14 Nov 2004 19:43

#13 Post by trust issues » 03 Jun 2008 20:34

My goal is to use the servo on the throttle cable to maintain a fixed car speed so it will be continually be making very small adjustments. I can only guess the cable moves around 5cm in total so the 10cm servo would work. Torque wise, I haven't got a newton meter lying around to measure the forces on the accerator cable:) Any good servos that you think could match my criteria?

Classic program Robot Wars. That helped strengthen my affection for electronics.

spookyrufus
Posts: 71
Joined: 10 Jun 2006 13:45

mmmm

#14 Post by spookyrufus » 03 Jun 2008 21:50

Can you explain with simpler english your algorithm to correct the jitting problems?
I can't figure out step 2. What is the general idea? I'm quite confused.

thanks for support guys I really admire you.

xor
Posts: 5465
Joined: 18 May 2005 00:59
Location: NYC
Contact:

#15 Post by xor » 03 Jun 2008 22:01

Before moving to any specifics... tell me what servo motor you are using. What are the minimum and maximum ON times for its rotation? What speed oscillator are using with your PIC?
[color=darkred][b]xor[/b][/color]
[url=http://circuit-ed.com]CircuitED -[/url]

Post Reply

Return to “mikroC General”