8servo.zip Control 8 servos simultaneously with rs232 commands AT90s2313
Charlie Krauter
Charlie Krauter
| CODE |
;8-ch RCservo driver w/ RS-232 interface ; input protocol is 2 byte packets: ; first byte(command byte) is: 1000aaam where a=address bit for channel(0-7) ; and m is MSB of position command ; second byte: 0ddddddd where d=bits 0 to 6 of position command ; timing assumes 4Mhz clock, 9600baud RS-232 ; optional data line only powers up servos when all channels have been ;loaded with valid command bytes ;PWM signals appear on portb: bits 0-7 ;portd, bit 6 is the optional power enable line, it will be low after ;reset and will only go high after all channels have received valid commands .include "2313def.inc" .dseg .def timint_out = r2 .def timint_sreg_save = r3 .def timint_high = r4 .def timint_low = r5 .def recint_sreg_save = r6 .def recint_channel = r7 .def recint_value = r8 .def timint_temp = r16 .def validinputs = r17;each bit is set when corresponding ;channel receives a valid input .def tempreg = r18 .def tempcount = r19 .def timreset_temp = r20 .def channel_iter = r21 .def flags = r22 .def recint_byte = r23 command: .byte 8 ;current settings for servo channels .equ power_en = 6;portd,6 is the power up line for the servos .equ heartbeat = 5;portd, 5 should oscillate during normal operation .equ got_byte1 = 0 ;bit 0 of flags register signals that ;the first byte of a packet was received .equ got_byte1_mask = 0x01 .cseg .org 0 reset: rjmp setup .org OC1addr rjmp handle_timer .org URXCaddr rjmp handle_receive .org 16 setup: ldi tempreg, RAMEND out spl, tempreg ;setup stack pointer clr flags clr validinputs;all channels are invalid cbi portd, power_en ;servos are initially powered down sbi ddrd, power_en ;configure power_en as output cbi portd, heartbeat sbi ddrd, heartbeat ;ready heartbeat as output clr tempreg out portb, tempreg ldi tempreg, 255 out ddrb, tempreg ;set servo signals to zerod outputs ldi XH, command/256 ;make sure timer handler points to right page ;zero out commands ldi YL, command&255 ldi YH, command/256 clr tempreg ldi tempcount, 8 command_init: st Y+,tempreg dec tempcount brne command_init ldi tempreg, 25 out ubrr, tempreg ;set UART to 9600baud sbi ucr, rxen ;enable UART receive sbi ucr, rxcie ;enable UART receive interrupt ;init output compare to a reasonable value clr tempreg out ocr1al, tempreg ldi tempreg, 16 out ocr1ah, tempreg rcall reset_timer1 ldi tempreg, 1<<ocie1a out timsk, tempreg ;enable output compare interrupt for timer 1 clr channel_iter ;start with channel 0 sei ;away we go run_loop: sbi portd, heartbeat nop nop cbi portd, heartbeat rjmp run_loop reset_timer1: clr timreset_temp out tccr1b, timreset_temp ;halt counter out tcnt1h, timreset_temp out tcnt1l, timreset_temp ldi timreset_temp, 1 out tccr1b, timreset_temp ;restart counter ret ;timer handler uses X reg, so receive handler will use Y handle_timer: in timint_sreg_save, sreg clr timint_out out portb, timint_out ;turn off last pulse ;set up for next timeout: timeout is 16*command + 3952(0x0f70) ;will produce close to 1-2ms pulses with 4Mhz clock ldi XL, command&255 add XL, channel_iter ld timint_low, X clr timint_high clc rol timint_low ;16-bit multiply by 16 rol timint_high rol timint_low rol timint_high rol timint_low rol timint_high rol timint_low rol timint_high ldi timint_temp, 0x70 ;add 3952 add timint_low, timint_temp ldi timint_temp, 0x0f adc timint_high, timint_temp out ocr1ah, timint_high ;load up output compare registers out ocr1al, timint_low ;according to datasheet, high has ;to be wriiten first clr timint_out ;get the pulse bit into the right place mov timint_temp, channel_iter inc timint_temp sec timer_outputmask_loop: rol timint_out dec timint_temp brne timer_outputmask_loop inc channel_iter cpi channel_iter, 8 brne timer_keepgoing clr channel_iter ;rollover to channel 0 timer_keepgoing: rcall reset_timer1 out portb, timint_out ;start pulse ldi timint_temp, 64; out tifr, timint_temp ;explicitly clear OC int flag out sreg, timint_sreg_save reti ;uses Y regiater to set channel values in SRAM handle_receive: in recint_sreg_save, sreg sei ;timer interrupt could still occur sbis udr, 7 ;is it a command byte or data? rjmp receive_byte2 recieve_byte1: ori flags, got_byte1_mask ;we got the beginning of a packet in recint_byte, udr ;get byte clr recint_value ror recint_byte ror recint_value; ;puts MSB into recint_value andi recint_byte, 7 ;mask address mov recint_channel, recint_byte;save channel to update rjmp receive_cleanup receive_byte2: sbrs flags, got_byte1 rjmp receive_cleanup ;ignore byte2 if byte1 has not been sent andi flags, 255 - got_byte1_mask ;clear flag to require another byte1 in recint_byte, udr ;get byte or recint_value, recint_byte ldi YL, command&255 add YL, recint_channel st Y, recint_value ;mark channel as receiving a valid byte sbic portd, power_en rjmp receive_cleanup ;only if power hasn't been turned on yet ;reuse recint_byte clr recint_byte inc recint_channel sec receive_setvalid_loop: rol recint_byte dec recint_channel brne receive_setvalid_loop or validinputs, recint_byte ;add in new input valid bit cpi validinputs, 255 brne receive_cleanup sbi portd, power_en ;all inputs valid, enable power receive_cleanup: out sreg, recint_sreg_save reti |