/* e-Tech Garage Rover Project Rover.lib Version 1.02 Change History 10 Dec 2009 V1.02 JLC Modified Rover_Static_Turn remove Direction parameter changed Degrees limits to +/- 180 Modified Motor_Drive to accept 100% duty cycle 1 Dec 2009 V1.01 JLC Added high level functions: Rover_Go, Rover_Static_Turn, Rover_Stop changed names of functions from Motor_ to Rover__ 30 Nov 2009 V1.00 JLC Initial Release This library contains the low level driver and ISR which creates the PWM signals for driving up to four small motors. These are the functions in the library: Rover_SetUp ( void ) Rover_Go ( int Direction, int Pulses, int Speed ) Rover_Static_Turn ( int Direction, int Degrees, int Speed ) Rover_Stop ( void ) Motor_Drive ( int MotorNbr, int direction, int DutyCycle, int Pulses ) The motors will be pulsed using Parallel Port A, using two bits per motor, driving four TA7291S H-Bridges. They use a common period for the PWM with each motor havung its own duty cycle. The ISR does not "know" what the interrupting interval is, all it needs to know is the number of interrupt intervals wanted per period and per motor. 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. Along with the macro INTERRUPTS_PER_SECOND, this 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. */ /*** BeginHeader */ #ifndef __ROVER_LIB #define __ROVER_LIB #define ROVER_LIB_VERSION 102 #ifndef DEBUG_MOTOR_ISR #define DEBUG_MOTOR_ISR nodebug #endif #ifndef DEBUG_ROVER #define DEBUG_ROVER nodebug #endif /*** EndHeader */ /*** BeginHeader MsDelay */ void MsDelay ( unsigned int iDelay ); /*** EndHeader */ void MsDelay ( unsigned int iDelay ) { auto long StopTime; StopTime = (long)(MS_TIMER + iDelay); while ( (long)(MS_TIMER - StopTime) < 0L ); } /* START FUNCTION DESCRIPTION ******************************************** Rover_Go SYNTAX: (int) = Rover_Go ( int Direction, int Speed, int Pulses ); DESCRIPTION: Tell the Rover to move PARAMETER1: Direction: ROVER_FORWARD, ROVER_REVERSE PARAMETER2: Speed = Duty Cycle: 1 <= Speed <= 100 PARAMETER3: Distance = Number of pulses: 1 <= n <= 32767 if n < 0 ==> continuous pulses at specified Duty Cycle RETURN VALUE: 0 = success -1 = invalid Direction -2 = invalid Speed Nbr_of_Motors == 0 when all motors are done See also: Rover_Static_Turn, Rover_Stop END DESCRIPTION **********************************************************/ /*** BeginHeader Rover_Go*/ int Rover_Go ( int Direction, int Speed, int Pulses ); enum { ROVER_FORWARD, ROVER_REVERSE }; /*** EndHeader Rover_Go*/ DEBUG_ROVER int Rover_Go ( int Direction, int Speed, int Pulses ) { if ( Speed < 1 || Speed > 100 ) return -2; if ( Direction == ROVER_FORWARD ) { Motor_Drive ( 1, ROVER_CW, Speed, Pulses ); Motor_Drive ( 2, ROVER_CW, Speed, Pulses ); Motor_Drive ( 3, ROVER_CW, Speed, Pulses ); Motor_Drive ( 4, ROVER_CW, Speed, Pulses ); } else if ( Direction == ROVER_REVERSE ) { Motor_Drive ( 1, ROVER_CCW, Speed, Pulses ); Motor_Drive ( 2, ROVER_CCW, Speed, Pulses ); Motor_Drive ( 3, ROVER_CCW, Speed, Pulses ); Motor_Drive ( 4, ROVER_CCW, Speed, Pulses ); } else return -1; return 0; } // Rover_Go /* START FUNCTION DESCRIPTION ******************************************** Rover_Static_Turn SYNTAX: (void) = Rover_Static_Turn ( int int Degrees, int Speed ); DESCRIPTION: Tell the Rover to turn PARAMETER1: Degrees of Rotation: -180 <= Degrees <= 180 PARAMETER2: Speed: 80 <= Speed <= 99 RETURN VALUE: none NOTES: 1) invalid parameters are changed to valid parameters 2) Nbr_of_Motors == 0 when all motors are done See also: Rover_Go, Rover_Stop END DESCRIPTION **********************************************************/ /*** BeginHeader Rover_Static_Turn*/ void Rover_Static_Turn ( int Degrees, int Speed ); enum { ROVER_RIGHT, ROVER_LEFT }; #define DEGREES_PER_PULSE 5.8 #define SPEED_2_DUTYCYCLE 1.0 /*** EndHeader Rover_Static_Turn*/ DEBUG_ROVER void Rover_Static_Turn ( int Degrees, int Speed ) { float fSpeed; int Pulses; if ( Degrees < -180 ) Degrees = -180; if ( Degrees > 180 ) Degrees = 180; if ( Speed > 99 ) Speed = 99; if ( Speed < 80 ) Speed = 80; Rover_Stop(1); // make sure Rover is stopped Pulses = (int)((float)Degrees/DEGREES_PER_PULSE); // convert Degrees to Pulses fSpeed = Speed; // get fp value fSpeed *= SPEED_2_DUTYCYCLE; // adjust to get duty cycle Speed = (int)fSpeed; // back to integer if ( Pulses > 0 ) { Motor_Drive ( 1, ROVER_CW, Speed, Pulses ); Motor_Drive ( 3, ROVER_CW, Speed, Pulses ); Motor_Drive ( 2, ROVER_CCW, Speed, Pulses ); Motor_Drive ( 4, ROVER_CCW, Speed, Pulses ); } else { Pulses *= -1; Motor_Drive ( 1, ROVER_CCW, Speed, Pulses ); Motor_Drive ( 3, ROVER_CCW, Speed, Pulses ); Motor_Drive ( 2, ROVER_CW, Speed, Pulses ); Motor_Drive ( 4, ROVER_CW, Speed, Pulses ); } Rover_Stop (1); // wait for turn to complete return; } // Rover_Static_Turn /* START FUNCTION DESCRIPTION ******************************************** Rover_Stop SYNTAX: (void) = Rover_Stop ( int Blocking ); DESCRIPTION: Tell the Rover to stop PARAMETER1: 1 = wait for motors to stop pulsing 0 = stop them immediately RETURN VALUE: none See also: Rover_Go, Rover_Static_Turn END DESCRIPTION **********************************************************/ /*** BeginHeader Rover_Stop*/ void Rover_Stop ( int Blocking ); /*** EndHeader Rover_Stop*/ DEBUG_ROVER void Rover_Stop ( int Blocking ) { int i; if ( !Motor_Status ) return; // return if already stopped if ( Blocking ) while ( Motor_Status ); // wait for all motor pulses to stop for ( i=1; i <= 4; i++ ) { Motor_Drive ( i, ROVER_BOTH-LastDirection[i-1], 100, 2 ); // apply brakes } } // Rover_Stop /* START FUNCTION DESCRIPTION ******************************************** Motor_Drive SYNTAX: (int) = Motor_Drive ( int MotorNbr, int direction, int DutyCycle, int Pulses ); DESCRIPTION: drive the specified motor PARAMETER1: Motor Number: 1 <= N < =4 PARAMETER2: Direction: ROVER_CW, ROVER_CCW PARAMETER3: Duty Cycle: 1 <= n <= 99 PARAMETER4: Number of pulses: 1 <= n <= 32767 if n < 0 ==> continuous pulses at specified Duty Cycle RETURN VALUE: 0 = success -1 = invalid motor -2 = invalid direction -3 = invalid duty cycle END DESCRIPTION **********************************************************/ /*** BeginHeader Motor_Drive*/ int Motor_Drive ( int MotorNbr, int Direction, int DutyCycle, int Pulses ); #define ROVER_CCW 1 #define ROVER_CW 2 #define ROVER_BOTH (ROVER_CCW+ROVER_CW) /*** EndHeader */ DEBUG_ROVER int Motor_Drive ( int MotorNbr, int Direction, int DutyCycle, int Pulses ) { float ftemp; int i; if ( MotorNbr < 1 || MotorNbr > 4 ) return -1; if ( Direction < 1 || Direction > 2 ) return -2; if ( DutyCycle < 1 || DutyCycle > 101 ) return -3; MotorNbr -= 1; // make 0 relative LastDirection[MotorNbr] = Direction; // save direction // set PADR value WrPortI ( TBCR, NULL, 0x04 ); // IPL=0, use Timer A1 as prescaler 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! Nbr_of_Motors +=1 ; Motor_Pulses[MotorNbr] = Pulses; WrPortI ( TBCR, NULL, 0x05 ); // IPL=1, usee Timer A1 as prescaler ; // for breakpoint } // Motor_Drive /* START FUNCTION DESCRIPTION ******************************************** Rover_SetUp SYNTAX: void Rover_SetUp ( void ); DESCRIPTION: Initialize parameters required for Motor_ISR and Motor_Drive. Enable Motor_ISR, IPL=0, via Timer B0 driven by Timer A1. Use the CPU frequency and INTERRUPTS_PER_SECOND to calculate Timer A1 divisor based on Timer B always dividing by 1024. The macros MOTOR_PERIOD and INTERRUPTS_PER_SECOND determine the PWM frequency. MOTOR_PERIOD is the number of interrupts per period. PWM frequency = INTERRUPTS_PER_SECOND/MOTOR_PERIOD Both of these macros can be defined in the main program. Their default values are: MOTOR_PERIOD = 128 INTERRUPTS_PER_SECOND = 2000L PARAMETER: none RETURN VALUE: none END DESCRIPTION **********************************************************/ /*** BeginHeader Rover_SetUp, Motor_ISR, SRF05_ISR */ void Rover_SetUp ( void ); void Motor_ISR ( void ); void SRF05_ISR ( void ); 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 LastDirection[4]; char Nbr_of_Motors; // nbr of active motors #ifndef MOTOR_PERIOD #define MOTOR_PERIOD 128 // interrupts per period 15Hz @2000ints/sec #endif #ifndef INTERRUPTS_PER_SECOND #define INTERRUPTS_PER_SECOND 2000L //1000L #endif #define GetDistance ( (float)SRF05Echo/SRF05ScaleFactor ); unsigned int SRF05Echo; float SRF05ScaleFactor; /*** EndHeader Motor_SetUp, Motor_ISR */ DEBUG_ROVER void Rover_SetUp() { unsigned long CPUfreq; unsigned long Divisor; int i; float ftemp, ActualFreq; int TA8divisor, TimerCdivisor; //*************************** Motor Setup ************************************** // 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 //*********************** End Motor Setup ************************************** /* //*************************** SRF05 Setup ************************************** // set up PE1 as the Echo input for the SRF05 BitWrPortI ( PEDDR, &PEDDRShadow, 0, 1 ); // PE1 = input // set up input capture 1 to measure the pulse width on PE0 // determine TA8 value to get close to 1us resolution ftemp = (float)CPUfreq * 1e-6; // CPU clocks/usec TA8divisor = (int)(ftemp/2.0); // adjust for pclk/2 WrPortI ( TAT8R, NULL, TA8divisor-1 ); ActualFreq = (float)CPUfreq/2.0/(float)TA8divisor; // Input Capture clock frequency ActualFreq /= 1e6; // make MHz SRF05ScaleFactor = 148.0*ActualFreq; // set up IC registers WrPortI ( ICCR, NULL, 0 ); // interrupts disabled WrPortI ( ICT1R, NULL, 0x56 ); // run continuously, latch on stop // start on rising, stop on falling WrPortI ( ICS1R, NULL, 0x88 ); // use PE0 for start & stop SetVectIntern ( 0x1A, SRF05_ISR ); // point to ISR WrPortI ( ICCSR, NULL, 0x10 ); // enable interrupt on IC1 stop WrPortI ( ICCR, NULL, 1 ); // interrupts enabled at IPL = 1 // set up Timer C0 on PE0 as the trigger for the SRF05 WrPortI ( PEALR, &PEALRShadow, PEALRShadow & ~0x03 ); // set PE0 to be WrPortI ( PEALR, &PEALRShadow, PEALRShadow | 0x02 ); // Timer C0 (alt out 2) // set up the clock for Timer C - need <= 20Hz WrPortI ( TAT1R, NULL, 255 ); // we want a very low frequency TimerCdivisor = (int)(CPUfreq/2/256/20); WrPortI ( TCDLR, NULL, TimerCdivisor ); // low byte WrPortI ( TCDHR, NULL, TimerCdivisor>>8 ); // high byte // set the pulse start and stop counts WrPortI ( TCS0LR, NULL, 0 ); // start count = 0 WrPortI ( TCS0HR, NULL, 0 ); // start count = 0 WrPortI ( TCR0LR, NULL, 10 ); // stop count = 10 WrPortI ( TCR0HR, NULL, 0 ); // stop count = 10 // start the clock WrPortI ( TCCR, NULL, 4 ); // Timer C clocked by TA1 WrPortI ( TCCSR, NULL, 1 ); // enable the clock // there should now be narrow pulses comming out of PE0 @ about 20Hz */ //*********************** End SRF05 Setup ************************************** } // Rover_SetUp #asm SRF05_ISR:: ; input capture ISR push af ; save the registers ioi ld a, (ICCSR) ; clear the interrupt ioi ld a, (ICL1R) ; get LSByte ld (SRF05Echo), a ; save it ioi ld a, (ICM1R) ; get MSByte ld (SRF05Echo+1), a ; save it ld a, 0x14 ; set up to ioi ld (ICCSR), a ; clear the IC1 counter pop af ; restore register ipres ; and IPL ret ; SRF05_ISR #endasm /* #asm debug _Motor_ISR:: push HL ld HL, lxpc push HL lcall _Motor_ISR pop HL ld lxpc, HL pop HL ret #endasm */ #asm DEBUG_MOTOR_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 ld c, ~0x03 ; 4 mask for motor 0 ; 44 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 ; c = mask for PADR ; 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, (Motor_Status) ; 6 PADR value and c ; 2 remove current motor ld (Motor_Status), a ; 6 save ioi ld (PADR), a ; 12 disable the motor 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 ; 47 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 de ; 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 rlc c ; 4 create PADR rlc c ; 4 mask 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 ; Motor_ISR ; 19 #endasm /*** BeginHeader */ #endif // __ROVER_LIB /*** EndHeader */