FREE Servo RC Controller code [ALMOST WORKING]

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

#16 Post by spookyrufus » 03 Jun 2008 22:25

I'm using HiTec HS-422 Servos. Timings are

- 1500ms Center position
- 900 lower bound
- 2100 upper bound

I'm building an hexapod robot, just for information.

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

#17 Post by xor » 03 Jun 2008 22:30

Thanks... I can work with that.

I can throw something together in mB quickly based on 1-2ms 90 degree rotation (per the HS422 specsheet).

It appears that your USART Byte is used for Servo ID (upper 4 bits) and the lower 4 bits are for the position.... giving a possible 16 positions of resolution for each servo.
[color=darkred][b]xor[/b][/color]
[url=http://circuit-ed.com]CircuitED -[/url]

sonnyk
Posts: 245
Joined: 08 Nov 2005 19:25
Location: Sweden

#18 Post by sonnyk » 03 Jun 2008 23:54

"trust issues" , you can use a ordinary "cruise control" vacuum actuator :wink:

"spookyrufus" , RC servos in general and older types of servos (old analog) like yours Hitec are prone to "buzz" and move slightly even if the PPM pulse is stable.

Belive me, I have use them all for some 30 years now, from cars to boats and planes, and now helicopters :D

Even if I take my newest Hurricane heli , with state of the art digital servos, they can buzz really bad from time to time, nothing to pay any attention to really.

Worse is if they move 2 much, so the gizmo you are controlling gets nervous or twitchy, then you may have a problem.
//SonnyK

-If assembler looks blurry...... try 2 C# ;)

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

#19 Post by spookyrufus » 04 Jun 2008 08:19

xor: yes you are right, that was almost my idea. In fact, in the code I posted, only the lower 3 bits are used to select motor position. However, it would be great to obtain up 16 positions .


sonnyk: Well, surely it's no more than 2 years I play around with electronics, but I'm sure that these servos (I've never tried anything else) have no random jitting at all.
In fact, on my robot with ~15 cm legs, this 'jitting' becomes really noticeable and makes the whole thing almost unusable.

Thanks a lot for your help guys.

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

Any news?

#20 Post by spookyrufus » 06 Jun 2008 20:00

Hello there? Any news from xor or other people?
I was asked for some additional infos on my servomotors and I thought that someone would have told me something more.

Thanks a lot to anyone replying.

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

a

#21 Post by spookyrufus » 07 Jun 2008 11:16

I also don't understand why someone thought this post would have been the right place to talk about his projects of driving a car with a servo.
It's interesting, but in fact this made this post die.

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

#22 Post by xor » 07 Jun 2008 15:17

The jitter is due to not refreshing your servos ON every 20ms.

That means that you make sure that each of the 12 servos turns ON every 20ms (not necessarily all servos at the same time). Also, each servo when turned ON, must stay ON a minimum of 900us and a maximum of 1100us. Then the servo stays OFF the remaining time until the 20ms is over.... and the cycle starts over again.

Can you do that? I have provided many examples of doing this with different code methods. If you don't understand these concepts, then someone will have to write your code for you. Am I correct?
[color=darkred][b]xor[/b][/color]
[url=http://circuit-ed.com]CircuitED -[/url]

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

yah but

#23 Post by spookyrufus » 07 Jun 2008 15:28

Yeah I can understand quite well the PWM mechanism needed to position a motor.
BUT I can't figure out why my code doens work as expected.
I've also reduced the motor numbero to 6, but jitter still exists in some (apparently random) positions.

TMR0 overflows every 20/250 ms.
Now, motor counters are set to be at ~50 distance one to other

Everytime a counter reaches 250 (=20ms have passed), a routine is called to set to 1 motor pin, and after correct delay, reset it to 0.

where's the problem in doing so? Isn't it precise?

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

#24 Post by xor » 07 Jun 2008 15:36

Each servo must be refreshed every 20ms, not 50ms, not 250ms apart.
[color=darkred][b]xor[/b][/color]
[url=http://circuit-ed.com]CircuitED -[/url]

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

Re: a

#25 Post by trust issues » 07 Jun 2008 22:30

spookyrufus wrote:I also don't understand why someone thought this post would have been the right place to talk about his projects of driving a car with a servo.
It's interesting, but in fact this made this post die.
I spotted a couple of people who knew specific servo details and asked a question. It's a public forum for users to ask any question, it's not just yours.

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

#26 Post by xor » 08 Jun 2008 07:17

I posted a lo-res 12 servo driver example in the mB forum. The code is well commented and should be able to be converted to mC relatively easily. A $2 PIC can drive the whole thing. Code is only 300 ROM... so it can be a project for anyone with a free compiler.
[color=darkred][b]xor[/b][/color]
[url=http://circuit-ed.com]CircuitED -[/url]

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

Ok thanks

#27 Post by spookyrufus » 08 Jun 2008 08:10

Thanks xor for your help, I'm really graceful.

trust problems, I agree with you, this is a free forum and everyone can write what he wants.
However, we were talking about controling servos and more specifically about my piece of code, and you thrust related problems are definitely off-topic. Starting new topics is as free as posting replies to existing ones, and maintains readability for future forum users.

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

#28 Post by xor » 08 Jun 2008 20:17

So everyone doesn't think I'm a hard nose when it comes to C: 8)

Here is the driver written in MikroC. Found a couple mistakes in the original mB which should be corrected in this version.

Code: Select all

/*
//'*******************************************************************************
//'  Program to Control 12- 50Hz Servos @ 16 Positions Each With USART Updates
//'*******************************************************************************
//'           Code compiled with mikroC 8.2 and tested on 16F887 @ 8MHz
//'           By Warren Schroeder  June 5, 2008
//'*******************************************************************************
//'*******************************************************************************
//'
//'           Each USART RX Byte:
//'              Servo ID        = Upper 4 bits  (0 to 11)   12 Servos
//'              Servo Position  = Lower 4 bits  (0 to 15)   16 Positions
//'
//'   HITECH-HS422 50Hz Servo: 940us to 2140us for min and max servo rotation
//'                            16 positions (940us= Position#0 + (15 x 80us))
//'                            Position#7 = 1500us Center
//'
//'       Timer1 Setup for 1us ticks  (Prescaler=2)
//'
//'       CCP1 is set up for Special Event Compare Mode
//'       On Compare-Match between TIMER1 and CCPR1L:CCPR1H registers
//'       CCP1IF is Set (=1) and TIMER1 is RESET (=0)
//'
//'
//'*******************************************************************************
//'*******************************************************************************
*/

#define Servo0  PORTB.f0
#define Servo1  PORTB.f1
#define Servo2  PORTB.f2
#define Servo3  PortB.f3
#define Servo4  PORTB.f4
#define Servo5  PORTB.f5
#define Servo6  PORTB.f6
#define Servo7  PORTB.f7
#define Servo8  PORTD.f0
#define Servo9  PORTD.f1
#define Servo10 PORTD.f2
#define Servo11 PORTD.f3

unsigned int register volatile
	CCPR1	  absolute 0x0015             ;
unsigned short AllOn                     ;
unsigned short frame80                   ;
unsigned short t0                        ;
unsigned short t1                        ;
unsigned short t2                        ;
unsigned short ServoPos[12]              ;  //  12 Servo Position Array from RX
unsigned short ServoWrk[12]              ;  //  12 Servo Position Work Array


void interrupt() {
    if (AllOn) {                            //  If AllOn flag=true then
           PORTB = 255                   ;  //  All PORTB servos ON
           PORTD = 15                    ;  //  All PORTD servos ON
           AllOn = ~AllOn                ;  //  reset AllOn flag
           frame80 = 0                   ;  //  80us frame counter
           CCPR1 = 940                   ;  //  940us = 0 position
           for (t2=0;t2<12;t2++) {          //  load work array from USART RX array
              ServoWrk[t2] = ServoPos[t2];  //
           }
    }                                       //
    else {                                  //
           CCPR1 = 80                    ;  //  80us frame delay
           FSR = (unsigned short)&ServoWrk ;  //  servo work array pointer.. first servo
           if (INDF-- == 0) Servo0  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo1  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo2  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo3  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo4  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo5  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo6  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo7  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo8  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo9  = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo10 = 0  ;  //  turn each servo OFF on 0; decrement counter
           FSR++                         ;  //  next servo
           if (INDF-- == 0) Servo11 = 0  ;  //  turn each servo OFF on 0; decrement counter

           if (++frame80 == 16) {           //  finished 15 80us position periods?
               CCPR1 = 18800             ;  //  load remaining time for 20ms interrupt
               AllOn = ~AllOn            ;  //  turn ON all servos at next 20ms interrupt
           }
    }
    PIR1.CCP1IF = 0                      ;  //  clear CCP1 interrupt flag
}


void CCP1_Setup() {
    CCP1CON      = 11         ;  //  CCP1 Compare MODE with special event trigger; resets Timer1 on match
    CCPR1        = 65000      ;  //  preload for 65ms delay before servo startup
    T1CON        = 16         ;  //  Timer1 Prescaler = 2 = 1us ticks
    PIE1.CCP1IE  = 1          ;  //  Enable CCP1 interrupt
    PIR1.CCP1IF  = 0          ;  //  Clear CCP1 Interrupt Flag
    INTCON       = 192        ;  //  Global & Peripheral interrupts enabled
    T1CON.TMR1ON = 1          ;  //  Start Timer1
}



void main() {

    ANSEL   = 0               ;   //  disable adc's
    ANSELH  = 0               ;   //  disable adc's
    CM1CON0 = 0               ;   //  disable comparators
    CM2CON0 = 0               ;   //  disable comparators
    PORTB   = 0               ;
    PORTD   = 0               ;
    TRISB   = 0               ;
    TRISD   = 0               ;
    AllOn   = 1               ;   //  all servos ON flag
    

    USART_INIT(19200)         ;
    CCP1_Setup()              ;

    while(1) {
       if (USART_DATA_READY) {           // test for RX byte
          t0 = USART_READ()            ; // save new RX byte
          t1 = t0 >> 4                 ; // extract servo ID from RX byte
          ServoPos[t1] =  t0 && 15     ; // save Position Value per ID
        }
    }

}
[color=darkred][b]xor[/b][/color]
[url=http://circuit-ed.com]CircuitED -[/url]

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

#29 Post by Puggs » 09 Jun 2008 00:29

Hi spookyrufus

What you will notice about xor's code is that the control of the servos is in the interrupt not the main line code.

Also that all servos are turned on at once and depending on their position are then turned off as needed.

There is no use of the BLOCKING Delay_us() function.

The cycle repeats every 20ms

And lastly, he has a nice neat coding style...

Puggs

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

#30 Post by xor » 09 Jun 2008 02:04

Thanks Puggs.

This little bit of code can be easily be adapted for up to 16 servos @ 16 positions. I found a couple optimizations that make it possible with the existing code structure.

You are correct about the use of non-blocking delays which is done with the CCP1 module. The interrupts do not interfere with communications since USART is hardware based and the RX buffer is 2 deep.

The USART RX buffer is also continually checked in a timely manner with no chance of missing communications. The code as it is presently exists should be able to stand up to 115KBaud speeds without missing bytes, but probably is not practical or necessary.

The project as it is right now is perfect for a 16F627A 18-pin PIC, which only costs about $2USD. Add a little wireless for less than $20USD, and you have some inexpensive control for a complex bot.
[color=darkred][b]xor[/b][/color]
[url=http://circuit-ed.com]CircuitED -[/url]

Post Reply

Return to “mikroC General”