/* Copyright ©1998, 1999 Larry Barello Author: Larry Barello lbarello@accessone.com You are allowed to use this file for personal use only. You may use, embed, modify or distribute the contents of this file in any way you see fit as long as you include this copyright notice in the resulting file. Any commercial or industrial use including publishing or distributing in any form requires the written authorization of the copyright holder(s). No guarantees are made about fitness, functionality, correctness or performance of this file. The file is supplied on a "AS IS" basis, with no responsibility assumed by the author(s) for bug fixes, support, or damages of any sort arising from the use of this file. Contact the author with problem reports or fixes, feature requests or additions and questions. They are welcomed and will be incorporated as time and energy permits. */ #include "registers.inc" #include "hardware.inc" #include "avrx.inc" MODULE TARGETING EXTERN TICKRATE CPUCLK EXTERN AvrXReadEEProm EXTERN AvrXDelay AvrXStartTimer AvrXWaitTimer EXTERN AvrXWaitSemaphore AvrXTestSemaphore AvrXIntSetSemaphore EXTERN TargetTable TargetTableEnd TargetTableEntry EXTERN TargetFar TargetNear NoTarget Servo_Latency PUBLIC TargetingTcb TargetingPid ; TASK Targeting, 4, 0, 0, 6 ; RSEG SSEG:DATA(0) ;------------------------------------------- ; Stack = Task Usage + one context + any non-avrx interrupt ; stack usage (4 + 10 + 2 = 16) ; DS 17 TargetingStack: DS 1 ; Give 18 bytes RSEG AVRXDATA:DATA(0) TargetingPid: DS PidSz TargetingTimer: DS TcbSz ; RSEG CODE ;+ ;---------------------------------------------- ; TargetingTcb: dw TargetingStack ; Stack dw Targeting>>1 ; Start Address db TargetingPid, 0 ; PID and context size dw 0>>1 ; Context save routine db 3 ; Priority ; ; NOTE: Highest priority in system due to time critical ; nature of the code. ;- EVEN RSEG CODE ;+ ;------------------------------------------------------- ; ; Targeting - TASK, Standard context switch ; ; Targeting uses a standard RC servo to scan the field of view ; with a Sharp GP2D02 proximity detector. After the servo settles ; into each position a reading is taken. After a complete cycle ; From right to left or left to right, the position with the ; strongest return signal (e.g. closest) is used to update the ; drive values (LeftMotor, RightMotor). Dead ahead is full speed ; off to the right or left biases the drive to turn our sumo towards ; the opponent. ; ; NOTE: Initialize timer0 and sets servo output pulses ; to the middle position ; ;- #define Distance R0 #define Tmp Xh Targeting: ldi Xl, TargetingTimer ldi Yl, low(TICKRATE/50) ; 20ms delay for Sharp sensor to initalize ldi Yh, high(TICKRATE/50) rcall AvrXDelay ldi Zl, low(TargetTable) ; ; The big loop starts here ; ReadProximitySensor: cbi MOTOR, Vin ; Initiate Proximity Detection ldi Xl, TargetingTimer ldi Yl, low(TICKRATE/500) ; 2ms loop ldi Yh, high(TICKRATE/500) dw0: rcall AvrXDelay sbis SENSOR, Vout ; Delay until sensor ready (should be rjmp dw0 ; an interrupt routine) clr Distance ; Clear accumulator ReadNextBit: clc sbic SENSOR, Vout ; Read input sec rol Distance ; Shift in data brcs ShutDownSensor ; done when first bit read in shifts out sbi MOTOR, Vin ldi Xh, (2*CPUCLK/3000000) ; 2 uS delay dec Xh brne $-2 cbi MOTOR, Vin ldi Xh, (16*CPUCLK/3000000) ; 16 uS delay dec Xh brne $-2 rjmp ReadNextBit ShutDownSensor: sbi MOTOR, Vin rcall AvrXDelay ; Give sensor time to recover ; ; See if a target was sighted ; cp NearestTarget, Distance brsh NotNearer ; Unsigned values from sensor mov NearestTarget, Distance mov TargetDirection, Zl ; Pointer to current position in table NotNearer: ; ; Ratched through the table, up or down count depending ; upon the T bit (Initially Zero, or CountUp) ; NextPosition: brtc CountDown CountUp: cpi Zl, TargetTableEnd brne IncPointer clt rjmp DirectionChanged IncPointer: subi Zl, -TargetTableEntry rjmp SetNewPosition CountDown: cpi Zl, TargetTable brne DecPointer set rjmp DirectionChanged DecPointer: subi Zl, TargetTableEntry SetNewPosition: rcall AvrXReadEEProm mov Servo1, Yl mov Xh, Zl ldi Zl, Servo_Latency rcall AvrXReadEEProm mov Zl, Xh mov Xh, Yl ldi Yl, low(TICKRATE/50) ldi Yh, high(TICKRATE/50) ldi Xl, TargetingTimer Loop: inc SERVO_COUNT sbi MOTOR, Servo1Bit ; Pulse the servo a bit rcall AvrXDelay dec Xh brne Loop rjmp ReadProximitySensor ; ; Analyse the results of the last sweep. The strongest return sets ; the spin direction and the duration of the spin. After the spin ; start over at the position we are already in. ; DirectionChanged: mov Xh, Zl ldi Zl, NoTarget rcall AvrXReadEEProm cp NearestTarget, Yl mov NearestTarget, Yl ; Reset nearest target breq NoTargetDetected mov Zl, TargetDirection ; Point to table entry of nearest target inc Zl rcall AvrXReadEEProm ; Read Right/Left drive info mov tLeftDrive, Yl inc Zl rcall AvrXReadEEProm ; Read Right/Left drive info mov tRightDrive, Yl inc Zl rcall AvrXReadEEProm ; Read Turn Delay mov Yh, Yl inc Zl rcall AvrXReadEEProm ; Read Turn Delay ldi Xl, TargetingTimer rcall AvrXDelay ; Delay so bot can turn ldi Zl, 0x7f mov tRightDrive, Zl ; Full tilt mov tLeftDrive, Zl rjmp EndDirectionChanged NoTargetDetected: ldi Zl, 0x3f mov tRightDrive, Zl mov tLeftDrive, Zl ; Idle EndDirectionChanged: mov Zl, Xh rjmp ReadProximitySensor ENDMOD MODULE BOUNDRY EXTERN TICKRATE CPUCLK EXTERN AvrXDelay AvrXStartTimer AvrXWaitTimer EXTERN AvrXReadEEProm EXTERN Spin_135_Degrees Reverse_Duration PUBLIC BoundryTcb BoundryPid ;+ ;------------------------------------------------------- ; ; Boundry ; ; TASK: Runs a simple loop and logic looking for the edge and ; causing the bot to back up and spin a bit when it hits it. ; ; This process also updates the motor drive from the targeting ; subsystem when there is no avoidance being done ; ;- ; TASK Boundry, 2, 0, 0, 6 RSEG SSEG:DATA(0) ;------------------------------------------- ; Stack = Task Usage + one context + any non-avrx interrupt ; stack usage (4 + 10 + 2 = 16) ; DS 17 BoundryStack: DS 1 ; Give 18 RSEG AVRXDATA:DATA(0) BoundryPid: DS PidSz BoundryTimer: DS TcbSz RSEG CODE ;+ ;---------------------------------------------- ; BoundryTcb: dw BoundryStack ; Stack dw Boundry>>1 ; Start Address db BoundryPid, 0 ; PID and context size dw 0 ; Context save routine db 2 ; Priority ; ; NOTE: Highest priority in system due to time critical ; nature of the code. ;- EVEN RSEG CODE Boundry: ldi Xl, BoundryTimer ; Initial 5 second delay ldi Yl, low(TICKRATE*5) ldi Yh, high(TICKRATE*5) rcall AvrXDelay While: ldi Yl, low(TICKRATE/50) ; Sample every 20 ms ldi Yh, high(TICKRATE/50) rcall AvrXDelay sbic SENSOR, LeftBoundry rjmp WaitRightBoundry sbic SENSOR, RightBoundry rjmp WaitLeftBoundry ; ; If no boundry, then look to targeting and proximity ; values. ; mov LeftMotor, tLeftDrive mov RightMotor, tRightDrive mov R0, pRightDrive ; Override Targeting if Proximity or R0, pLeftDrive ; non-zero breq Continue mov LeftMotor, pLeftDrive mov RightMotor, pRightDrive Continue: rjmp While WaitLeftBoundry: ldi RightMotor, 0x00 ldi LeftMotor, 0x7f ldi Xh, 25 ; 1/2 second timeout wlb00: sbic SENSOR, LeftBoundry rjmp StartReverse rcall AvrXDelay dec Xh breq StartReverse rjmp wlb00 WaitRightBoundry: ldi LeftMotor, 0x00 ldi RightMotor, 0x7f ldi Xh, 25 wrb00: sbic SENSOR, RightBoundry rjmp StartReverse rcall AvrXDelay dec Xh breq StartReverse rjmp wrb00 StartReverse: ldi LeftMotor, 0x80 ldi RightMotor, 0x80 ldi Zl, Reverse_Duration rcall AvrXReadEEProm inc Zl mov Yh, Yl rcall AvrXReadEEProm rcall AvrXDelay DoSpin: ldi LeftMotor, 0x7F ldi RightMotor, 0x80 ldi Zl, low(Spin_135_Degrees) rcall AvrXReadEEProm mov Yh, Yl inc Zl rcall AvrXReadEEProm rcall AvrXDelay rjmp While ENDMOD MODULE NEARTARGETING EXTERN TICKRATE EXTERN AvrXDelay AvrXReadEEProm EXTERN Spin_135_Degrees Spin_180_Degrees PUBLIC NearTargetingPid NearTargetingTcb ;+ ;------------------------------------------------------- ; ; NearTargeting ; ; TASK: Runs a simple loop and logic looking for the edge and ; causing the bot to back up and spin a bit when it hits it. ; ; This process also updates the motor drive from the targeting ; subsystem when there is no avoidance being done ; ;- ; TASK NearTargeting, 4, 0, 0, 4 RSEG SSEG:DATA(0) ;------------------------------------------- ; Stack = Task Usage + one context + any non-avrx interrupt ; stack usage (4 + 10 + 2 = 16) ; DS 17 NearTargetingStack: DS 1 ; Give 18 RSEG AVRXDATA:DATA(0) NearTargetingPid: DS PidSz NearTargetingTimer: DS TcbSz RSEG CODE ;+ ;---------------------------------------------- ; NearTargetingTcb: dw NearTargetingStack ; Stack dw NearTargeting>>1 ; Start Address db NearTargetingPid, 0 ; PID and context size dw 0 ; Context save routine db 2 ; Priority ; ; NOTE: Highest priority in system due to time critical ; nature of the code. ;- EVEN adi MACRO dest, val subi dest, -val endm RSEG CODE #define Left Zl #define Right Zh #define Other Xh #define LoopVar Yh #define FrontCnt 0x01 #define RearCnt 0x10 #define LEDPULSES 50 ; 25/41k = cycles EnableRight = low(~(1< PWM_COUNT) SetLeftMotorForward else SetLeftMotorReverse if (RightMotor > PWM_COUNT) SetRightMotorForward else SetRightMotorReverse if (PWM_COUNT & 0x01 && SERVO_COUNT) ; Divide by two & { ; Stop on rollover if (++SERVO_COUNT > Servo) ResetServoBit } } -*/ DoMotorDrive: BeginInterrupt ldi MotorDriveTmp, TCNT0_INIT out TCNT0, MotorDriveTmp ; Reload timer in MotorDriveTmp, SREG ; Save SREG inc PWM_COUNT ; Free running 8 bit counter DoLeftMotor: cp LeftMotor, PWM_COUNT ; -128 to +127 brlt ReverseLeftMotor sbi MOTOR, LeftForward cbi MOTOR, LeftReverse rjmp DoRightMotor ReverseLeftMotor: cbi MOTOR, LeftForward sbi MOTOR, LeftReverse DoRightMotor: cp RightMotor, PWM_COUNT brlt ReverseRightMotor sbi MOTOR, RightForward cbi MOTOR, RightReverse rjmp DoServoStuff ReverseRightMotor: cbi MOTOR, RightForward sbi MOTOR, RightReverse DoServoStuff: sbrc PWM_COUNT, 0 ; Run servo stuff at 1/2 the rjmp ExitDoMotorDrive ; interrupt rate (75khz) DoServo: ; Set SERVO_COUNT & Servo Output tst SERVO_COUNT ; Bits to 1 to start output pulse breq DoLedDrive ; if (SERVO_COUNT) inc SERVO_COUNT ; SERVO_COUNT++ cp SERVO_COUNT, Servo1 ; if (SERVO_COUNT > Servo1) brlo $+4 ; Clear Servo1Bit cbi MOTOR, Servo1Bit DoLedDrive: ; ; I/R detect subsystem LED driver. ; sbrc LedCount, 7 ; Idle = 3 cycles; Active = 9 cycles rjmp LedDone ; LedCount of -1 is flag that we are done in IR, MOTOR sbr IR, ((1<