1 | //==========================================================================
|
2 | // Author : Ng Hock Yuan
|
3 | // Project : Sample code for SC16A using 16F877A
|
4 | // Project description : This source code is used to control 16 servos.
|
5 | // The 16 servos will rotate from one end to another end one by one
|
6 | // Position reporting subroutine is used to wait for servo reach the other end.
|
7 | //
|
8 | //==========================================================================
|
9 | // This is the sample code for controlling 16 channel of servo using SC16A
|
10 | /**************************************************************
|
11 | * Communication Protocol *
|
12 | **************************************************************
|
13 | *
|
14 | * Position and Speed Command
|
15 | *
|
16 | * UART is chosen as the interface to this module. In order to
|
17 | * change the position of a servo, the host (master) needs to
|
18 | * write 4 bytes of data to this module. The data format is:
|
19 | *
|
20 | * Byte 1 Byte 2 Byte 3 Byte 4
|
21 | * -------------- --------------- -------------- -------------
|
22 | * / Servo channel \/ Position \/ Position \/ Speed value \
|
23 | * \ (0x41 - 0x60) /\ (Higher Byte) /\ (Lower Byte) /\ (0-63) /
|
24 | * -------------- --------------- -------------- -------------
|
25 | *
|
26 | * Servo channel: 0b01XX XXXX
|
27 | * Higher Byte: 0b00XX XXXX
|
28 | * Lower Byte: 0b00XX XXXX
|
29 | * Speed Value: 0b00XX XXXX
|
30 | *
|
31 | * The position for the servo is in 12-bit and the valid range
|
32 | * is from 0 (0.5mS) to the resolution defined below (2.5mS).
|
33 | * It is the host responsibility to make sure the position will
|
34 | * not stall the servo motor.
|
35 | *
|
36 | *
|
37 | * Position Reporting Command
|
38 | *
|
39 | * This command is use to keep update the current position of servo.
|
40 | * In order to get the feedback of current position from servo, the host needs to
|
41 | * send 2 bytes of data to SC16A.
|
42 | *
|
43 | * Byte 1 Byte 2
|
44 | * -------------- ---------------
|
45 | * / Start byte \/ Servo channel \
|
46 | * \ '@' or 0x40 /\ (0x41 - 0x60) /
|
47 | * -------------- ---------------
|
48 | * Start byte: 0b0100 0000 or '@' in ASCII
|
49 | * Servo channel: 0b01XX XXXX
|
50 | *
|
51 | * After received this 2 bytes, SC16A will transmit the current position of requesting servo in 3 bytes
|
52 | *
|
53 | * Byte 1 Byte 2 Byte 3
|
54 | * -------------- --------------- --------------
|
55 | * / Servo channel \/ Position \/ Position \
|
56 | * \ (0x41 - 0x60) /\ (Higher Byte) /\ (Lower Byte) /
|
57 | * -------------- --------------- --------------
|
58 | * Servo channel: 0b01XX XXXX
|
59 | * Higher Byte: 0b00XX XXXX
|
60 | * Lower Byte: 0b00XX XXXX
|
61 | *
|
62 | * The host needs to receive and process this 3 bytes.
|
63 | *
|
64 | *
|
65 | **************************************************************/
|
66 | // include
|
67 | //==========================================================================
|
68 | #include <16F877A.h> // this sample code is using 16F877A !!
|
69 |
|
70 | // configuration
|
71 | //==========================================================================
|
72 | __CONFIG ( 0x3F32 ); //configuration for the microcontroller
|
73 |
|
74 |
|
75 | // define
|
76 | //==========================================================================
|
77 | #define sw1 RB0
|
78 |
|
79 |
|
80 | // global variable
|
81 | //=========================================================================
|
82 | static volatile unsigned int received_servo_position[0x11]; // Array declared to store the feedback position of servo
|
83 |
|
84 |
|
85 | // function prototype (every function must have a function prototype)
|
86 | //==========================================================================
|
87 |
|
88 | void send_cmd(unsigned char num, unsigned int data, unsigned char ramp); //UART transmit 4 bytes: servo number, higher byte position, lower byte position and speed
|
89 | void delay(unsigned long data); //delay function, the delay time
|
90 | void uart_send(unsigned char data); //UART transmit
|
91 | unsigned char uart_rec(void); //UART receive
|
92 |
|
93 | void request_feedback(unsigned char num); //UART transmit 2 bytes: start byte '@' and servo number 0x41-0x60 to request position
|
94 | void get_position(void); //UART receive 3 bytes: servo number 0x41-0x60, higher byte and lower byte for position value to update the current position of servo
|
95 |
|
96 | // main function (main fucntion of the program)
|
97 | //==========================================================================
|
98 | void main(void)
|
99 | {
|
100 | unsigned int j;
|
101 | unsigned char i;
|
102 | //set IO port for led and switch
|
103 | TRISC = 0b10000000; //set input or output
|
104 | TRISB = 0b00000001;
|
105 |
|
106 | //setup USART
|
107 | BRGH = 1; //baud rate low speed option
|
108 | SPBRG = 0x81; //set boud rate to 9600bps for 10Mhz crystal
|
109 | //SPBRG = 0x81 for 20MHz Crystal
|
110 | //refer to datasheet
|
111 | SPEN = 1; //enable serial port
|
112 | RX9 = 0; //8-bit reception
|
113 | TX9 = 0;
|
114 | CREN = 1; //enable reception
|
115 | TXEN = 1; //enable transmission
|
116 |
|
117 | for(i=0x01;i<0x11;i+=1) //set initial position of servos
|
118 | {
|
119 | send_cmd(i,1300,0);
|
120 | }
|
121 |
|
122 | while(1)
|
123 | {
|
124 |
|
125 | if(sw1==0)
|
126 | {
|
127 | while(sw1==0);
|
128 |
|
129 | while(1)
|
130 | {
|
131 | for(i=0x01;i<0x11;i+=1) //servo number start from 0x01 to 0x10, total 16servos
|
132 | { //servo number will convert to 0x41 to 0x50 in send cmd routine
|
133 | send_cmd(i,200,20); //send position and speed command to SC16A
|
134 | request_feedback(i); //request the position to SC16A
|
135 | get_position(); //receive 3 bytes on position reporting from SC16A to get the current position
|
136 | while(received_servo_position[i]!=200) //test if servo position reach 200 value
|
137 | {
|
138 | request_feedback(i); //if no yet reach, keep request
|
139 | get_position(); //receive 3 bytes on position reporting from SC16A to keep update the current position
|
140 | }
|
141 |
|
142 | }
|
143 |
|
144 | for(i=0x01;i<0x11;i+=1)
|
145 | {
|
146 | send_cmd(i,1300,20);
|
147 | request_feedback(i);
|
148 | get_position();
|
149 | while(received_servo_position[i]!=1300) //test if servo position reach 1300 value
|
150 | {
|
151 | request_feedback(i); //if no yet reach, keep request
|
152 | get_position(); //receive 3 bytes on position reporting from SC16A to keep update the current position
|
153 | }
|
154 |
|
155 | }
|
156 | }//while loop
|
157 | }//if loop
|
158 |
|
159 | }//while loop
|
160 |
|
161 | }//main loop
|
162 |
|
163 | //subroutine
|
164 | //============================================================================
|
165 |
|
166 |
|
167 | void send_cmd(unsigned char num, unsigned int data, unsigned char ramp) //send 4 bytes of command to control servo's position and speed
|
168 | {
|
169 | unsigned char higher_byte=0, lower_byte=0;
|
170 |
|
171 | //servo channel should start with 0b01XX XXXX
|
172 | //therefore needs to change to 0x41-0x60
|
173 | num=num|0b01000000;
|
174 |
|
175 | //position value from 0-1463 are greater than a byte
|
176 | //so needs two bytes to send
|
177 | higher_byte=(data>>6)&0x003f; //higher byte = 0b00xxxxxx
|
178 | lower_byte=data&0x003f; //lower byte = 0b00xxxxxx
|
179 |
|
180 |
|
181 | uart_send(num); //First byte is the servo channel 0x41-0x60
|
182 | uart_send(higher_byte); //second byte is the higher byte of position 0b00xxxxxx
|
183 | uart_send(lower_byte); //third byte is the lower byte of position 0b00xxxxxx
|
184 | uart_send(ramp); //fourth byte is the speed value from 0-63
|
185 |
|
186 | }
|
187 |
|
188 | void request_feedback(unsigned char num) //send command to request the current position of servo
|
189 | {
|
190 | //servo channel should start with 0b01XX XXXX
|
191 | //therefore needs to change to 0x41-0x60
|
192 | num=num|0b01000000;
|
193 |
|
194 | uart_send('@'); //First byte is the start byte: '@' or 0x40
|
195 | uart_send(num); //Second byte is the requsting servo channle 0x41-0x60
|
196 | }
|
197 |
|
198 | void get_position(void) //receive 3 bytes from SC16A and update the position of servo
|
199 | {unsigned int i;
|
200 | static unsigned int received_servo_num=0, higher_byte=0,lower_byte=0,received_position=0;
|
201 |
|
202 | received_servo_num=uart_rec(); //First byte to receive: Requesting Servo number 0x41-0x60
|
203 | higher_byte=uart_rec(); //Second byte to receive: Requesting Servo higher byte position
|
204 | lower_byte=uart_rec(); //Third byte to receive: Requesting Servo lower byte position
|
205 | received_servo_num=received_servo_num&0b00011111; //Change back to 0x01-0x20
|
206 | received_servo_position[received_servo_num]=((higher_byte<<6)|(lower_byte&0x3F)); //Update the servo position value in corresponding array
|
207 |
|
208 | }
|
209 |
|
210 | unsigned char uart_rec(void) //receive uart value
|
211 | {
|
212 | unsigned char rec_data;
|
213 | while(RCIF==0); //wait for data
|
214 | rec_data = RCREG;
|
215 | return rec_data; //return the received data
|
216 | }
|
217 |
|
218 | void uart_send(unsigned char data)
|
219 | {
|
220 | while(TXIF==0); //only send the new data after
|
221 | TXREG=data; //the previous data finish sent
|
222 | }
|
223 |
|
224 | void delay(unsigned long data) //delay function, the delay time
|
225 | { //depend on the given value
|
226 | for( ;data>0;data-=1);
|
227 | }
|