/* e-Tech Garage Rover Project Rover.c Global variables for controlling the motors The motors will be pulsed using Parallel Port A using two bits per motor. They will use a common period for the PWM with each motor havung its own on-time value. The ISR does not "know" what the interrupting interval is, all it needs to know is the number of interrupt intervals wanted for the operations. Motor_Status - two bits per motor, one per input TA291 H-Bridge Truth Table: Input Output IN1 IN2 OUT1 OUT2 0 0 open open 1 0 H L 0 1 L H 1 1 brake brake The routines that make use of the ISR will need to load Nbr_of_Motors after loading the other variables that control the period and on-time. Nbr_of_Motors - the number of active motors. Motor_Period - the number of interrupts in the period, this essentially controls the frequency of the PWM signal. Motor_Period_Remaining - the number of interrupts remaining in the current period This value is re-loaded from Motor_Period by the ISR whenever it counts down to 0. Motor_Width - the number of pulses that the ISR will keep the indicated output at its "1" level. Motor_Width[0] will be loaded into Motor_Width[1] at the beginning of each Motor_Period interval. Motor_Pulses - the number of pulses to generate. */ #define DEBUG_ISR nodebug enum { DIR_FREE, DIR_CW, DIR_CCW, DIR_BRAKE }; #define MOTOR_PERIOD 128 #define INTERRUPTS_PER_SECOND 2000L //1000L void Test_ISR(); void Motor_ISR(); void SetUpISR(); int Motor_Drive ( int MotorNbr, int Direction, int DutyCycle, int Pulses ); void fnMsDelay ( unsigned int iDelay ) { auto long StopTime; StopTime = (long)(MS_TIMER + iDelay); while ( (long)(MS_TIMER - StopTime) < 0L ); } main () { unsigned long CPUfreq; unsigned long Divisor; SetUpISR(); Motor_Drive ( 1, DIR_CW, 50, -10 ); Motor_Drive ( 1, DIR_CW, 90, -10 ); Motor_Drive ( 1, DIR_CW, 10, -10 ); Motor_Drive ( 1, DIR_CW, 50, 0 ); Motor_Drive ( 1, DIR_CCW, 50, -10 ); Motor_Drive ( 1, DIR_CCW, 90, -10 ); Motor_Drive ( 1, DIR_CCW, 10, -10 ); Motor_Drive ( 1, DIR_CCW, 50, 0 ); while (1); } char Motor_Status; // two bits per motor int Motor_Period; // nbr of interrupts for PWM period int Motor_Period_Remaining; // nbr of interrupts remaining in current period char Motor_Width[4][2]; // [0]=nbr of interrupts for "1" state // [1] = decremented in ISR int Motor_Pulses[4]; // number of pulses char Nbr_of_Motors; // nbr of active motors void SetUpISR() { unsigned long CPUfreq; unsigned long Divisor; int i; // initialize motor drive values Motor_Status = 0; // all motors off Motor_Period = MOTOR_PERIOD; // set period Motor_Period_Remaining = MOTOR_PERIOD; Nbr_of_Motors = 0; // no motors active for ( i=0; i<4; i++ ) { Motor_Pulses[i] = 0; } // set up parallel port A for output WrPortI ( PADR, &PADRShadow, 0 ); // init port A WrPortI ( SPCR, &SPCRShadow, 0x84 ); // port A = output // set up Timer A1 as prescaler for Timer B interrupt CPUfreq = get_cpu_frequency(); Divisor = CPUfreq/INTERRUPTS_PER_SECOND; // timer divisor Divisor /= 1024L; // Timer A1 prescale for Timer B Divisor /= 2; // adj for PerClk/2 into Timer A1 if ( Divisor > 255 ) { printf ( "\nInterrupt Frequency too low\n" ); exit(1); } WrPortI ( TAT1R, NULL, (int)Divisor ); WrPortI ( TACSR, &TACSRShadow, TACSRShadow|1 ); // enable Timer A clock WrPortI ( TBCR, NULL, 0x04 ); // IPL=0, usee Timer A1 as prescaler // set up Timer B interrupt SetVectIntern ( 0x0B, Motor_ISR ); // point to ISR WrPortI ( TBCSR, NULL, 3 ); // enable TmrB clk & B1 interrupt WrPortI ( TBM1R, NULL, 0 ); // set Timer B to interrupt WrPortI ( TBL1R, NULL, 0 ); // at count = 0 WrPortI ( TBCR, NULL, 0x05 ); // IPL=1, usee Timer A1 as prescaler } // SetUpISR /* START FUNCTION DESCRIPTION ******************************************** Motor_Drive SYNTAX: i = Motor_Drive ( int MotorNbr, int direction, int DutyCycle, int Pulses ); DESCRIPTION: drive the specified motors PARAMETER1: Motor Number: 1 <= N < =4 PARAMETER2: Direction: FREE, CW, CCW, BRAKE PARAMETER3: Duty Cycle: 1 <= n <= 99 PARAMETER4: Number of pulses: 1 <= n <= 32767 if n < 0 ==> continuous pulses at Duty RETURN VALUE: 0 = success -1 = invalid motor -2 = invalid direction -3 = invalid duty cycle END DESCRIPTION **********************************************************/ int Motor_Drive ( int MotorNbr, int Direction, int DutyCycle, int Pulses ) { float ftemp; int i; if ( MotorNbr < 1 || MotorNbr > 4 ) return -1; if ( Direction < DIR_FREE || Direction > DIR_BRAKE ) return -2; if ( DutyCycle < 1 || DutyCycle > 99 ) return -3; MotorNbr -= 1; // make 0 relative // set PADR value Motor_Pulses[MotorNbr] = 0; // stop current activity Motor_Status &= ~( 3 << (MotorNbr*2) ); // clear PADR bits for this motor Motor_Status |= ( Direction << (MotorNbr*2) ); // insert direction bits // calculate pulses needed for Duty Cycle ftemp = (float)DutyCycle/100.0 * (float)MOTOR_PERIOD; i = (int)ftemp; if ( i > 255 ) return -3; Motor_Width[MotorNbr][0] = i; Motor_Width[MotorNbr][1] = i; // do not change the order of these statements! WrPortI ( TBCR, NULL, 0x04 ); // IPL=0, usee Timer A1 as prescaler Nbr_of_Motors +=1 ; Motor_Pulses[MotorNbr] = Pulses; WrPortI ( TBCR, NULL, 0x05 ); // IPL=1, usee Timer A1 as prescaler ; // for breakpoint } #asm DEBUG_ISR Motor_ISR:: /* This ISR expects the following variables: Nbr_of_Motors - the number of active motors. This value is decremented by the ISR when each Motor_Pulses[x] is decremented to 0. IMPORTANT: - Nbr_of_Motors should be set LAST in any function which modifies any of the following values. All motors have completed operations when Nbr_of_Motors == 0. Motor_Status = two bits per motor - this is the value that will be written to PADR in order to drive the motors. Only those motors which have a value in Motor_Pulses will be pulsed. Other bits will not be affected. Motor_Period = number of interrupts in one PWM cycle - not modified in ISR Motor_Width[0] = the number of interrupts for the pulse width - not modified in ISR Motor_Pulses = number of pulses to generate. This value is decremented at the beginning of each pulse period. This value is used to determine motor activity: 0 = inactive This ISR modifies the following variables - both values MUST be loaded by the user: Motor_Period_Remaining = number of interrupts remaining in current period. Decremented with each interrupt. When it reaches 0 the ISR reloads it from Motor_Period. Motor_Width[1] = the number of interrupts remaining for the current pulse. Decremented with each interrupt. When it reaches 0 the output will be set to 0. The value will be reloaded from Motor_Width[0] at the beginning of each new period. 81: TOTAL: independant of operation - no motors active the following 3 times are per motor +140: motor not active +153: motor active - done current pulse +167: motor active - not done current pulse +83: not end of period - Total +123: for all following - (includes the 83 above) the following 4 times are per motor +32: motor done +56: continuous run +82: just completed pulses +78: still pulsing One example: all four motors running continuously at 50% duty cycle For 1/2 the interrupts - while the pulses are active [ 81 + (4*167) + 123 + (4*56) ] * .5 * Frequency of interrupts + [ 81 + (4*153) + 123 + (4*56) ] * .5 * PWM frequency + [ 81 + (4*140) + 123 + (4*32) ] * .5 * PWM frequency If Frequency of interrupts = 2000 and PWM Frequency = Frequency of interrupts/128 Then the total clocks in one secnd is about... [81 + 668 + 123 + 224 ] * .5 * 2000 + [81 + 612 + 123 + 224 ] * .5 * 2000/128 + [81 + 560 + 123 + 128 ] * .5 * 2000/128 = 1096000 + 8125 + 6968 = 1,111,093 clocks/second That is about a 1.5% load on a 74MHz Rabbit */ ; save registers and clear the interrupt push af ; 11 ioi ld a, (TBCSR) ; 11 clear the interrupt ; set up next Timer B interrupt xor a ; 2 a = 0 ioi ld (TBM1R), a ; 12 set up next interrupt ioi ld (TBL1R), a ; 12 ; ; check if done all motors ld a, (Nbr_of_Motors) ; 6 get nbr of motors remaining or a ; 2 any motors active? jp z, Motor_ISR_exitB ; 6 exit if no ; 62 ; save remaining registers push bc ; 11 push de ; 11 push HL ; 11 push ix ; 13 /******************************************************************************* Pulse the Active Motors *******************************************************************************/ ; set up ld ix, Motor_Pulses ; 6 point to nbr of pulses remaining ld bc, Motor_Width+1 ; 6 point to width values ld a, (Motor_Status) ; 6 PADR value ld e, a ; 2 save for processing ld d, ~3 ; 4 PADR mask for motor 0 ; ix = address of number of pulses remaining ; bc = address of Motor_Width[1] = working copy ; e = PADR value ; d = PADR mask Motor_ISR_1a: ;check if already done this motor ld HL, (ix) ; 6 get number of pulses test HL ; 2 already done this motor? jr z, Motor_ISR_1b ; 6 jump if yes ; 84 ; check if already done this pulse ld a, (bc) ; 5 nbr of interrupts remaining or a ; 2 done pulse for this period? jr z, Motor_ISR_1b ; 6 jump if yes ; 13 ; update pulse width dec a ; 2 update pulse width ld (bc), a ; 12 save new value jr Motor_ISR_1x ; 6 pulse it ; 20 Motor_ISR_1b: ; disable the motor for this interrupt ld a, e ; 2 get PADR value and d ; 2 disable motor for this interrupt ld e, a ; 2 save ; 6 Motor_ISR_1x: ; update pointers for next motor inc bc ; 2 point to inc bc ; 2 next motor width values inc ix ; 4 point to pulses inc ix ; 4 for next motor ; set up mask for next motor rlc d ; 4 create PADR rlc d ; 4 mask for next motor jr c, Motor_ISR_1a ; 6 jump if not done ; 26 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ; ; write PADR value ld a, e ; 2 get the PADR value ld (PADRShadow), a ; 10 update shadow register ioi ld (PADR), a ; 12 drive the motors! ; 24 ; /******************************************************************************* NO REGISTERS ARE SETUP ENTERING HERE !! *******************************************************************************/ ; check for end of period ld HL, (Motor_Period_Remaining) ; 11 dec HL ; 2 update interrupt count for this period ld (Motor_Period_Remaining), HL ; 13 save test HL ; 2 done this period? jr nz, Motor_ISR_exit ; 6 jump if no ; 34 ; here if start new period ; reload Motor_Period_Remaining from Motor_Period ld HL, (Motor_Period) ; 11 ld (Motor_Period_Remaining), HL ; 13 ; set up registers for processing ld ix, Motor_Pulses ; 6 point to Motor_Pulses list ld de, Motor_Width ; 6 point to width values ld b, 4 ; 4 nbr of motors ; 40 Motor_ISR_2a: ; ix = address of Motor_Pulses list ( int ) ; de = address of Pulse Width list ( char[2] ) ; b = nbr of motors remaining to check ; update Motor_Pulses for each motor ld HL, (ix) ; 6 get number of pulses test HL ; 2 already done this motor? jr z, Motor_ISR_2x ; 6 jump if yes ; 14 jp M, Motor_ISR_2b ; 7 jump if continuous run ; 7 ; update Motor_Pulses dec HL ; 2 update pulse count ld (ix), HL ; 12 save updated count test HL ; 2 motor still active? jr nz, Motor_ISR_2b ; 6 jump if yes ; 22 ; update nbr of active motors ld a, (Nbr_of_Motors) ; 6 get nbr of motors dec a ; 2 update ld (Nbr_of_Motors), a ; 7 save jr z, Motor_ISR_exit ; 6 exit if no more motors ; 21 Motor_ISR_2b: ; reload Motor_Width[1] from Motor_Width[0] ld a, (de) ; 6 get pulse width inc de ; 2 point to interrupts remaining count ld (de), a ; 7 save reloaded value dec bc ; 2 back up ; 17 ; set up to do next motor Motor_ISR_2x: inc de ; 2 point to inc de ; 2 next motor width values inc ix ; 4 point to pulses inc ix ; 4 for next motor djnz Motor_ISR_2a ; 6 jump if not done ; 18 Motor_ISR_exit: ; restore the registers and exit the ISR pop ix ; 9 pop HL ; 7 pop de ; 7 pop bc ; 7 ; 30 Motor_ISR_exitB: pop af ; 7 ipres ; 4 ret ; 8 ; 19 ; #endasm