* Filename : PWM.ASM * Programmer : Michael Hattermann * Date : February 22, 2002 * Version : 1.0 * Description : This file contains the pulse width * modulation functions for generating * an output waveform. The following * functions are available * * INITPWM - inits PWM system * KILLPWM - shut down PWM system * LEFTMOTOR - sets spd,dir for left motor * RIGHTMOTOR - sets spd,dir for right motor * CHNGSPEED - sets new speed for motors * STEER - sets motors to perform known manuevers * PULLOUT - move to begin chase * * *#define __DEBUGPWM_ 1 #include "hc12.asm" * ************************************************************ * PWM Equates ************************************************************ * PWM0 EQU 0 ; left motor PWM1 EQU 1 ; right motor PWM2 EQU 2 PWM3 EQU 3 PWM4 EQU 4 ; left direction PWM5 EQU 5 ; left brake PWM6 EQU 6 ; right direction PWM7 EQU 7 ; right brake ACCELCONST EQU 6 ; acceleration constant FULLSPEED EQU 200 _7_8_SPEED EQU 175 _3_4_SPEED EQU 150 _5_8_SPEED EQU 125 HALFSPEED EQU 100 _3_8_SPEED EQU 75 _1_4_SPEED EQU 50 _1_8_SPEED EQU 25 STOPSPEED EQU 0 LEFTDIR EQU BIT6 LEFTBRK EQU BIT7 RIGHTDIR EQU BIT4 RIGHTBRK EQU BIT5 GOFOWARD EQU 0 GOBACK EQU 1 HARDLEFT EQU 2 SOFTLEFT EQU 3 HARDRIGHT EQU 4 SOFTRIGHT EQU 5 BACKLEFT EQU 6 BACKRIGHT EQU 7 STOP EQU 8 * ************************************************************ * PWM Debug Code ************************************************************ * #ifdef __DEBUGPWM_ ORG USERPROG_PVECT JMP TEST ORG $B000 TEST JSR INITPWM ; initialize the PWM system LDD #HALFSPEED ; load speed JSR CHNGSPEED ; set max speed of motors TEST1 LDX #GOFOWARD ; load manuever MOVB #GOFOWARD,SEG7PORT ; write direction to port JSR STEER ; move the robot LDX #5000 ; wait 1 sec JSR WAIT ; LDX #GOBACK ; load manuever MOVB #GOBACK,SEG7PORT ; write direction to port JSR STEER ; move the robot LDX #5000 ; wait 1 sec JSR WAIT ; LDX #HARDLEFT ; load manuever MOVB #HARDLEFT,SEG7PORT ; write direction to port JSR STEER ; move the robot LDX #5000 ; wait 10 sec JSR WAIT ; LDX #SOFTLEFT ; load manuever MOVB #SOFTLEFT,SEG7PORT ; write direction to port JSR STEER ; move the robot LDX #5000 ; wait 1 sec JSR WAIT ; LDX #HARDRIGHT ; load manuever MOVB #HARDRIGHT,SEG7PORT ; write direction to port JSR STEER ; move the robot LDX #5000 ; wait 10 sec JSR WAIT ; LDX #SOFTRIGHT ; load manuever MOVB #SOFTRIGHT,SEG7PORT ; write direction to port JSR STEER ; move the robot LDX #5000 ; wait 1 sec JSR WAIT ; LDX #STOP ; load manuever MOVB #STOP,SEG7PORT ; write direction to port JSR STEER ; move the robot LDX #5000 ; wait 1 sec JSR WAIT ; BRA TEST1 SWI OLEFTSPD DC.W $0000 ; current speed of left motor NLEFTSPD DC.W $0000 ; next speed of left motor ORIGHTSPD DC.W $0000 ; current speed of right motor NRIGHTSPD DC.W $0000 ; next speed of right motor MAXFOWARDSPD DC.W $0000 ; maximum foward speed for motors MAXBACKSPD DC.W $0000 ; maximum reverse speed for motors CURRMAN DC.W $0000 ; current manuever being performed #include "wait.asm" #endif * ******************************************************************************* * SUBROUTINE - INITPWM * Description: Initializes the pulse width modulation system * Input : None. * Output : None. * Destroys : None. * Calls : None. ******************************************************************************* * INITPWM MOVB #$08,PWCLK ; set prescaler bits, concatenate PWM channels MOVB #$33,PWPOL ; set clock source (S0,S1), polarity (high first) MOVB #99,PWSCAL0 ; set S0 clock prescaler MOVB #$00,PWSCAL1 ; init S1 clock prescaler MOVB #200,PWPER0 ; set period of PWM0 to about 20ms MOVB #200,PWPER1 ; set period of PWM1 to about 20ms MOVB #$FF,PWPER2 ; init period of PWM2 MOVB #$FF,PWPER3 ; init period of PWM3 MOVB #$00,PWDTY0 ; init duty cycle to 0% MOVB #$00,PWDTY1 ; init duty cycle to 0% MOVB #$00,PWDTY2 ; init duty cycle to 0% MOVB #$00,PWDTY3 ; init duty cycle to 0% MOVB #$00,PWCTL ; init PWM to run normally, left aligned MOVB #$50,DDRP ; configure unused bits to be outputs (motor controls) MOVB #$50,PORTP ; set direction to foward, brake to off MOVB #$03,PWEN ; enable the PWM channels RTS ; return to caller * ******************************************************************************* * SUBROUTINE - KILLPWM * Description: Shuts down the pulse width modulation system * Input : None. * Output : None. * Destroys : None. * Calls : None. ******************************************************************************* * KILLPWM MOVB #$00,PWEN ; turn off PWM channels RTS ; return to caller * ******************************************************************************* * SUBROUTINE - LEFTMOTOR * Description: Sets the left motor to the speed specified. Valid * speed values range from -100 to 100, where 0 to -100 is back- * wards speed and 0 to 100 is foward speed. * * Input : None. * Output : None. * Destroys : None. * Calls : None. ******************************************************************************* * LEFTMOTOR PSHX ; save reg X PSHY ; save reg Y PSHD ; save reg D LDD OLEFTSPD ; load the old speed LDY #ACCELCONST ; get the acceleration constant EMULS ; multiply old speed by acceleration constant ADDD NLEFTSPD ; add new speed to result LDX #(ACCELCONST+1) ; load divisor IDIVS ; calc new speed XGDX ; put new speed in reg D STD OLEFTSPD ; save new speed as old speed BLT LEFTMOTOR1 ; if new speed is negative, branch BSET PORTP,LEFTDIR ; set direction to foward BRA LEFTMOTOR2 ; continue LEFTMOTOR1 BCLR PORTP,LEFTDIR ; set direction to reverse NEGB ; take abs value of speed LEFTMOTOR2 STAB PWDTY1 ; set the new speed for the left motor PULD ; restore reg D PULY ; restore reg Y PULX ; restore reg X LEFTMOTORX RTS ; return to caller * ******************************************************************************* * SUBROUTINE - RIGHTMOTOR * Description: Sets the right motor to the speed specified. Valid * speed values range from -100 to 100, where 0 to -100 is back- * wards speed and 0 to 100 is foward speed. * * Input : None. * Output : None. * Destroys : None. * Calls : None. ******************************************************************************* * RIGHTMOTOR PSHX ; save reg X PSHY ; save reg Y PSHD ; save reg D LDD ORIGHTSPD ; load the old speed LDY #ACCELCONST ; get the acceleration constant EMULS ; multiply old speed by acceleration constant ADDD NRIGHTSPD ; add new speed to result LDX #(ACCELCONST+1) ; load divisor IDIVS ; calc new speed XGDX ; put new speed in reg D STD ORIGHTSPD ; save new speed as old speed BLT RIGHTMOTOR1 ; if new speed is negative, branch BSET PORTP,RIGHTDIR ; set direction to foward BRA RIGHTMOTOR2 ; continue RIGHTMOTOR1 BCLR PORTP,RIGHTDIR ; set direction to reverse NEGB ; take abs value of speed RIGHTMOTOR2 STAB PWDTY0 ; set the new speed for the right motor PULD ; restore reg D PULY ; restore reg Y PULX ; restore reg X RIGHTMOTORX RTS ; return to caller * ******************************************************************************* * SUBROUTINE - CHNGSPEED * Description: Changes the maximum speed for either motor to the speed passed * in register D. * Input : Speed(reg D). * Output : None. * Destroys : None. * Calls : None. ******************************************************************************* * CHNGSPEED STD MAXFOWARDSPD ; save max speed STD MAXBACKSPD ; save max speed to backward COM MAXBACKSPD ; convert to negative speed NEG MAXBACKSPD+1 ; RTS ; return to caller * ******************************************************************************* * SUBROUTINE - STEER * Description: Sets up the left and right motors to perform the specified * manuever (reg X). The value of manuever must be one of the * following: * * GOFOWARD * GOBACK * HARDLEFT * SOFTLEFT * HARDRIGHT * SOFTRIGHT * BACKLEFT * BACKRIGHT * STOP * * Input : Manuever(reg X). * Output : None. * Destroys : None. * Calls : LEFTMOTOR,RIGHTMOTOR. ******************************************************************************* * STEER PSHD ; save register D PSHX ; save register X STX CURRMAN ; save manuever TBNE X,STEER1 ; if not FOWARD, continue LDD MAXFOWARDSPD ; load maximum foward speed STD NRIGHTSPD ; right motor full foward STD NLEFTSPD ; left motor foward full BRA STEERX ; get out STEER1 DBNE X,STEER2 ; if not BACKWARD, continue LDD MAXBACKSPD ; load maximum back speed STD NRIGHTSPD ; right motor back full STD NLEFTSPD ; left motor back full BRA STEERX ; get out STEER2 DBNE X,STEER3 ; if not HARD LEFT, continue LDD MAXFOWARDSPD ; load maximum foward speed STD NRIGHTSPD ; right motor foward full LDD MAXBACKSPD ; load maximum back speed STD NLEFTSPD ; left motor backward full BRA STEERX ; get out STEER3 DBNE X,STEER4 ; if not SOFT LEFT, continue LDD MAXFOWARDSPD ; load maximum foward speed STD NRIGHTSPD ; right motor foward full LSRD ; set left motor speed STD NLEFTSPD ; left motor foward half BRA STEERX ; get out STEER4 DBNE X,STEER5 ; if not HARD RIGHT, continue LDD MAXFOWARDSPD ; load maximum foward speed STD NLEFTSPD ; left motor foward full LDD MAXBACKSPD ; load maximum back speed STD NRIGHTSPD ; right motor back full BRA STEERX ; get out STEER5 DBNE X,STEER6 ; if not SOFT RIGHT, continue LDD MAXFOWARDSPD ; load maximum foward speed STD NLEFTSPD ; left motor foward full LSRD ; set right motor speed STD NRIGHTSPD ; right motor foward half BRA STEERX ; get out STEER6 DBNE X,STEER7 ; if not BACK LEFT, continue LDD MAXBACKSPD ; load maximum foward speed STD NRIGHTSPD ; right motor back full LSRD ; set left motor speed STD NLEFTSPD ; left motor foward half BRA STEERX ; get out STEER7 DBNE X,STEER8 ; if not BACK RIGHT, continue LDD MAXBACKSPD ; load maximum foward speed STD NLEFTSPD ; left motor back full LSRD ; set left motor speed STD NRIGHTSPD ; right motor foward half BRA STEERX ; get out STEER8 DBNE X,STEERX ; if not STOP, get out MOVW #$0000,NLEFTSPD ; stop left motor MOVW #$0000,NRIGHTSPD ; stop right motor STEERX PULX ; restore register X PULD ; restore register D RTS ; return to caller * ******************************************************************************* * SUBROUTINE - PULLOUT * Description: Performs pre-programmed manuever to pull out onto the road and * turn toward moving objext to begin chasing * Input : None. * Output : None. * Destroys : None. * Calls : None. ******************************************************************************* * PULLOUT PSHD ; save reg D PSHX ; save reg X LDD #FULLSPEED ; set motor speed JSR CHNGSPEED ; LDX #GOFOWARD ; pull foward JSR STEER ; LDX #60 ; for a little bit JSR WAIT ; LDAA SPDCAUGHT ; get direction of speeder CMPA #LSPDFLAG ; was the speeder going left? BEQ PULLOUTL ; yes, so go pull out left PULLOUTR LDX #SOFTRIGHT ; no, then pull out right JSR STEER ; LDX #625 ; wait for manuever JSR WAIT ; BRA PULLOUTX ; get out PULLOUTL LDX #SOFTLEFT ; turn left JSR STEER ; LDX #600 ; wait for manuever JSR WAIT ; PULLOUTX PULX ; restore reg X PULD ; restore reg D RTS ; return to caller * *******************************************************************************