varunme
Advanced Member level 3
I got the following code from net,
i dont know how to send the data to control the motor
kindly someone help
i dont know how to send the data to control the motor
kindly someone help
Code:
//'*******************************************************************************
//'16F628A Program to Control 6 - 50Hz Servos @ 16 Positions Each With USART Updates
//'*******************************************************************************
//' Code compiled with mikroC 8.2 and tested on 16F628A @ 8MHz external clock
//' original code written By Warren Schroeder June 5, 2008
//'*******************************************************************************
//'*******************************************************************************
//' Each USART RX Byte:
//' Servo ID = Upper 4 bits (0 to 5) 6 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 PORTA.f0
#define Servo1 PORTA.f1
#define Servo2 PORTA.f2
#define Servo3 PORTA.f3
#define Servo4 PORTA.f6
#define Servo5 PORTB.f7
#define Servo PORTA.f4 // not in use but works as dummy servo
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] ; // 6 Servo Position Array from RX
unsigned short ServoWrk[12] ; // 6 Servo Position Work Array
unsigned int counting = 0;
void interrupt() {
//unsigned int CCPR1;
if (AllOn) { // If AllOn flag=true then
PORTA = 255; //WAS 19
PORTB = 255; ; // All PORTB 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 (x 15 total = 1200us)
FSR = (unsigned short)&ServoWrk ; // servo pointer.. point to first servo pos
if (INDF == 0) Servo0 = 0 ; // turn servo OFF on 0
INDF-- ; // decrement position counter
FSR++ ; // point to next servo
if (INDF == 0) Servo1 = 0 ; // turn servo OFF on 0
INDF-- ; // decrement position counter
FSR++ ; // point to next servo
if (INDF == 0) Servo2 = 0 ; // turn servo OFF on 0
INDF-- ; // decrement position counter
FSR++ ; // point to next servo
if (INDF == 0) Servo3 = 0 ; // turn servo OFF on 0
INDF-- ; // decrement position counter
FSR++ ; // point to next servo
if (INDF == 0) Servo4 = 0 ; // turn servo OFF on 0
INDF-- ; // decrement position counter
FSR++ ; // point to next servo
if (INDF == 0) Servo5 = 0 ; // turn servo OFF on 0
INDF-- ;
FSR++ ; // point to dummy servo
if (INDF == 0) Servo = 0 ; // turn servo OFF on 0
INDF-- ;
FSR++ ; // point to dummy servo
if (INDF == 0) Servo = 0 ; // turn servo OFF on 0
INDF-- ;
FSR++ ; // point to dummy servo
if (INDF == 0) Servo = 0 ; // turn servo OFF on 0
INDF-- ;
FSR++ ; // point to dummy servo
if (INDF == 0) Servo = 0 ; // turn servo OFF on 0
INDF-- ;
FSR++ ; // point to dummy servo
if (INDF == 0) Servo = 0 ; // turn servo OFF on 0
INDF-- ;
FSR++ ; // point to dummy servo
if (INDF == 0) Servo = 0 ; // turn servo OFF on 0
INDF-- ;
if (++frame80 == 16) { // finished 15 80us position periods?
CCPR1 = 17900 ; // 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() {
VRCON = 0;
CMCON = 7;
TRISA = 0;
PORTA = 0 ;
TRISB = 0 ;
AllOn = 255 ; // all servos ON flag
USART_INIT(9600) ;
CCP1_Setup() ;
for (t0=0;t0<12;t0++) { // pre-load servo array with some values
ServoPos[t0] = t0 ; //
}
while(1) {
asm CLRWDT;
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
}
}
}
