program servo; {main procedure} {il ciclo del servo dura circa 20 msec (50hz) e l'impulso va da 1 a 2 msec} {per

servo Futaba S148 il punto di mezzo corrisponde a 1520 us, -90 a circa 400 us, +90 a circa 2300 us. Versione per Pic 16F628A, utilizza la seriale UART hardware, ricezione su RB1, trasmissione su RB2. Usa un interrupt su TMR0 con prescaler x8 per temporizzare gli impulsi di comando dei servi. Costanti per oscillatore a 4 mhz: xxxx_4 Costanti per oscillatore a 20 mhz: xxxx_20 ATTENZIONE!! PER 20MHZ SETTARE TIPO OSCILLATORE AD HS ******************** Input seriale: RB1 Output seriale: RB2 Output servo: RB0,RB3-RB7 } const {***** parameters for serial read routine ********} BITRATE : byte = 9600; {serial bitrate} KBITRATE_4 : byte = 25; {coefficient for serial bit rate at 4 mhz} KBITRATE_20 : byte = 129; {coefficient for serial bit rate at 20 mhz} {************* end parameters *****************} N_SERVO : byte = 6; MIN_AD_CODE : byte = 160; {base pattern for the servo address byte} MAX_AD_CODE : byte = 165; NSTEP : byte = 254; MAX_POS : byte = 255; STEP_INCR : byte = 1; {the variation in servo position for every cycle} T_MEDIUM : integer = 1520; {us} T_MIN : integer = 400; {us} T_MAX : integer = 2300; {us} T_FILLER : byte = 18; {ms} {***** Constants for 4 mhz ************} OPTION_REG_4 : byte = %10000010; // | prescaler to TMR0 // | 010 = x8 ; 1 TMR0 unit = 8 us T_MIN_TMR0_4 : byte = 211; FINAL_DELAY_CYCLE_4 : byte = 2; FINAL_DELAY_TMR0_4 : byte = 0; {***** Constants for 20 mhz ************} OPTION_REG_20 :byte = %10000100; // | prescaler to TMR0 // | 100 = x32 ; 1 TMR0 unit = 6,4 us T_MIN_TMR0_20 : byte = 195; FINAL_DELAY_CYCLE_20 : byte = 19; FINAL_DELAY_TMR0_20 : byte = 192;

var {**** vars for serial read routine ****} readchar, sy, se, pos, parity, blSy: byte;

comByte1, comByte2, comByte3: byte; {**** end vars ****} timer0, tmrPos, i: Byte; ipin: Byte; targetpos: array[N_SERVO] of byte; servopos: array[N_SERVO] of byte; servopos_fil: array[N_SERVO] of byte; {********************************************************************* ***} procedure verifySerial; begin {check again if the timer is ended } if timer0=0 then exit; {if serial read Ok, verify servo address and position, and a parity check byte on the two data bytes, then set them in arrays} if (blSy=1) then begin blSy := 0; {echoes characters received} { while testbit(TXSTA, TRMT) = 0 do begin end; TXREG := sy; while testbit(TXSTA, TRMT) = 0 do begin end; TXREG := se; while testbit(TXSTA, TRMT) = 0 do begin end; TXREG := pos; } if (sy=MAX_POS) and (se>=MIN_AD_CODE) and (se<=MAX_AD_CODE) and (parity = (se xor pos)) then begin se := se - MIN_AD_CODE; targetpos[se] := pos ; end; end; end; {********************************************************************* ***} procedure interrupt; begin // ************* interrupt on TMR0 if (INTCON and %00100100) = %00100100 then // | enabled interrupt on TMR0 // | TMR0 flag interrupt begin asm interr: end; clearbit(INTCON, T0IF); // clear interrupt flag on TMR0 // 1 = we are in the initial constant part of pulse if (timer0 = 1) then begin TMR0 := tmrPos;

timer0 := 2; // continue for the tmrPos timer cycles end else // 2 = we are in the variable part of pulse if (timer0 = 2) then begin asm turnoff: end; clearbit(PORTB, ipin); // turn OFF portb bit I timer0 := 0; clearbit(INTCON, T0IE); // disable TMR0 interrupts end else begin // 3 = we are in the low part of pulse, until the total of 2,5 ms timer0 := 0; clearbit(INTCON, T0IE); // disable TMR0 interrupts end; end {TMR0 interrupt} else begin // ************* interrupt on serial if testbit(PIR1, RCIF) = 1 then begin inc(readchar); // char read if readchar = 1 then begin comByte1 := RCREG; // verify first character, if it's not the sync character // reset read counter and annull preceding reads if (comByte1 <> MAX_POS) then readchar := 0; end else if readchar = 2 then begin comByte2 := RCREG; end else if readchar = 3 then begin comByte3 := RCREG; end else if (readchar = 4) then begin sy := comByte1; // store the bytes in variables for the main se := comByte2; pos := comByte3; parity := RCREG; setbit(blSy, 0); // read completed successfully readchar := 0; end; if testbit(RCSTA, OERR) = 1 then begin clearbit(RCSTA, CREN); // clear OERR bit setbit(RCSTA, CREN); // end; end; end; {serial interrupt} end; {interrupt proc}

{********************************************************************* ***} begin INTCON := $00; // disable all interrups PORTA := 0; CMCON := $07; // all RA as digital pins TRISA := %00100000; // all outputs, except RA5 always input // hardware UART settings clearbit(RCSTA, CREN); // disable serial receive setbit(PIE1, RCIE); // enable interrupts on serial receive TRISB := %00000010; // configure PORTB: pin 1 as serial input, pin 2 // as output for UART module, others as output SPBRG := KBITRATE_20; // set bit rate coefficient {8-bit transmit, transmitter enabled, asynchronous mode, high speed mode} TXSTA := %00100100; // |6: 8 bit data // |5: transmit enabled // |4: async mode // |2: high speed mode {8-bit receive, receiver enabled, serial port enabled} RCSTA := %10010000; // |7: serial port enabled // |6: 8 bit data // |4: continuous receive enabled // |2: high speed mode {clear the receive buffer FIFO reading 3 times, because it can contain 3 bytes} comByte1 := RCREG; comByte1 := RCREG; comByte1 := RCREG; OPTION_REG := OPTION_REG_20; for i:=0 to (N_SERVO-1) do // center every servo begin servopos[i] := NSTEP div 2 ; targetpos[i] := servopos[i]; servopos_fil[i] := NSTEP-servopos[i]; end; blSy := 0; setbit(INTCON, PEIE); // enable interrupts from peripherals setbit(INTCON, GIE); // enable all interrupts repeat // beginning of a repeat loop begin i := 0; while (i<N_SERVO) do begin {invia un impulso a ciascun servo, di durata pari a T_MIN + 8 * servopos[i] Per ogni servo impiega T_MAX us, per un totale di: N_SERVO * T_MAX }

if (i>0) then ipin := i + 2 else ipin := 0; // constant part of high pulse: TMIN // delay for interrupts and others at 4mhz: 50 us // TMIN - 40 = 360 // at 4mhz: about 45 cycles of 8 us // TMR0 = 256 - 45 = 211 // delay for interrupts and others at 20mhz: 10 us // TMIN - 10 = 390 // at 20mhz: about 61 cycles of 6,4 us // TMR0 = 256 - 61 = 195 timer0 := 1; // init high part of pulse // turn ON portb bit I asm turnon: end; setbit(PORTB,ipin); TMR0 := T_MIN_TMR0_20; setbit(INTCON, T0IE); // enable interrupts on TMR0 position // calculate new servo pos relative to target if (servopos[i] < targetpos[i]) then servopos[i] := servopos[i] + STEP_INCR else if (servopos[i] > targetpos[i]) then servopos[i] := servopos[i] - STEP_INCR; // duration for low part of pulse servopos_fil[i] := NSTEP-servopos[i]; // calculate variable part of high pulse: // is between 0 and 254 tmrPos := 256 - servopos[i] - 2; while (timer0>0) do // wait TMR0 interrupt begin verifySerial; end; // end high part of pulse. // init low part of pulse, until the T_MAX duration // delay servopos_fil[i] * 8 us TMR0 := 256 - servopos_fil[i] - 2; timer0 := 3; setbit(INTCON, T0IE); // enable interrupts on TMR0 while (timer0>0) do // wait TMR0 interrupt begin verifySerial; end; // end low part of pulse. High part + low part duration end; // would be: T_MAX inc(i);

servopos[i]

the

// // at 4mhz: // // // // // //

must delay around 4 ms; 256 * 8us = 2048 us is

maximum duration of the timer, then it needs 2 cycles (i=2) at the maximum duration (TMR0=0) at 20mhz: delay must be around 7,6 ms; 64 * 6,4us = 410 us is the duration of a quarter timer cycle, then it needs 19 cycles at the

quarter

end; until 0 = 1; satisfied) end.

// duration (TMR0=256-64=192) asm finaldelay: end; i := FINAL_DELAY_CYCLE_20; while (i>0) do begin timer0 := 3; TMR0 := FINAL_DELAY_TMR0_20; setbit(INTCON, T0IE); // enable interrupts on TMR0 while (timer0>0) do // wait TMR0 interrupt begin verifySerial; end; dec(i); end; // endless loop (as this condition is never