.Include "m32def.inc" ;My Very First AVR Projedt .def ir = r18 ;Infarad input .def n1 = r19 ;variable used for a counter .def n2 = r20 ;^ .def n3 = r21 ;^ .def d1 = r22 ;delay amount 1 - 1 of 3 in total (ie 250 x 250 x 4 ~ 1sec) .def d2 = r23 ;^ .def d3 = r24 ;^ .def retaddr = r25 ;used for return address storage .org 0x0000 ;Plades the following dode from address 0x0000 rjmp RESET ;Take a Relative Jump to the RESET Label RESET: ;Reset Label ;Sets all variables to 0 ;n# are just used as temporary number holders ;They are used in the time routine, but there values are ;stored the in the memory beforehand, and then reloaded ;after the delay routine is done. ; ;IE, use them at will ; ;SRAM lookup table for n1-3 ;n1 is stored in 0x60 ;n2 is stored in 0x61 ;n3 is stored in 0x62 ldi n1, 0x00 ;Generic temp # holder ldi n2, 0x00 ;Generic Temp # holder ldi n3, 0x00 ;Generic Temp # holder ;Used for the time delay subroutine. Don't touch ;unless your sure you won't bee needing a timed delay ldi d1, 0x00 ;Time Delay Value 1 ldi d2, 0x00 ;Time Delay Value 2 ldi d3, 0x00 ;Time Delay Value 3 sbi ddrd, pd0 sbi ddrd, pd1 sbi ddrd, pd2 sbi ddrd, pd3 sbi ddrd, pd4 sbi ddrd, pd5 ;These lines set up the subroutine return function ldi retaddr,high(RAMEND) ; Main program start out SPH,retaddr ; Set Stack Pointer to top of RAM ldi retaddr,low(RAMEND) out SPL,retaddr ;End of subroutine return function ;Main program loop starts here ;Main program loop starts here ;Main program loop starts here Loop: ;Loop Label ;To change a servos position, simply put the ;servo number (0-5) in n1 Call ServoSetMemory ;Sets up multi Servo Variables ;^ is REQUIRED ldi n1, 0x00 ;Select Servo to change position Call ServoSet0 ;Change Servo Angle.. ldi n1, 0x01 call servoset20 ldi n1, 0x02 call servoset45 ldi n1, 0x03 call servoset90 ldi n1, 0x04 call servoset135 ldi n1, 0x05 call servodisable ;Modify the time in the ServoCycle loop ;Currently it's set for 400 cycles, which leaves 161 cycles for ;input handleing & similar ; ;Note: This is only meant for a clock frequency of 4MHz asdfloop: call ServoCycle ;Total Dealy of 232 cycles per run (static) jmp asdfloop ;plus this 3 = 235 Call Servo0 Call Servo0 Call Servo0 Call Servo0 call servocycle call servocycle call servocycle call Servo20 call Servo20 call Servo20 call Servo20 call servocycle call servocycle call servocycle call Servo45 call Servo45 call Servo45 call Servo45 call servocycle call servocycle call servocycle call Servo90 call Servo90 call Servo90 call Servo90 call Delay_1ms call Servo135 call Servo135 call Servo135 call Servo135 call Delay_1ms call Servo160 call Servo160 call Servo160 call Servo160 call Delay_1ms call servo180 call servo180 call servo180 call servo180 call Delay_1ms jmp loop ;Servo Controlling ;Set the bit that the servo is on to output, ;and the others to input ;Then, output 0xff to all it's ports ;All the ones set to output will send the siginal cbi ddrd, pd1 cbi ddrd, pd3 ldi n1, 0xff out portd, n1 ;End sample ;Uber cool flashing led sequence :/ cbi portd, pd5 ; turn off output pin 5 (pd5) sbi portd, pd0 ; turn on output pin 0 (pd0) call Delay_1sec ; Delay for 1 second cbi portd, pd0 ; Turn off output pin 0 (pd0) sbi portd, pd1 ; Turn On output pin 1(pd1) call Delay_1sec cbi portd, pd1 sbi portd, pd2 call Delay_1sec cbi portd, pd2 sbi portd, pd3 call Delay_1sec cbi portd, pd3 sbi portd, pd4 call Delay_1sec cbi portd, pd4 sbi portd, pd5 call Delay_1sec ;End of flashing leds. Ah ;( jmp Loop ;Jumps back to the top, and re runs all of the code ; ;----------------------------------------------------------- ; ;Single ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ; ;----------------------------------------------------------- ; Servo0: ;servo position <- ;Requires a pulse of 1 ms ldi n1, 0x00 ;loop counter set to zero ldi n2, 0xFF ;number of times to loop (255 here) Servo0Loop: ldi n3, 0xFF ;register that stores output data... out portd, n3 ;Turn on all ports meant for output call Delay_1ms ;Delay for required time ldi n3, 0x00 ;register that stores output data out portd, n3 ;Now, turn them off call Delay_2ms ;delay for requried time inc n1 ;increase the loop counter cpse n1, n2 ;see it it meets the loop # if so, skip the jmp jmp Servo0Loop ;jump back to the loop :) ret ;Return to original position ; ;----------------------------------------------------------- ; Servo20: ;Between angle 0 & 45 - Used for standing position ;Requires a pulse of 1.11 ms ldi n1, 0x00 ;loop counter set to zero ldi n2, 0xFF ;number of times to loop (255 here) Servo20Loop: ldi n3, 0xFF ;register that stores output data... out portd, n3 ;Turn on all ports meant for output call Delay_011ms ;Delay for required time ldi n3, 0x00 ;register that stores output data out portd, n3 ;Now, turn them off call Delay_2ms ;delay for requried time inc n1 ;increase the loop counter cpse n1, n2 ;see it it meets the loop # if so, skip the jmp jmp Servo20Loop ;jump back to the loop :) ret ;Return to original position ; ;----------------------------------------------------------- ; Servo45: ;Servo Position \ ;Requires a pulse of 1.25 ms ldi n1, 0x00 ;loop counter set to zero ldi n2, 0xFF ;number of times to loop (255 here) Servo45Loop: ldi n3, 0xFF ;register that stores output data... out portd, n3 ;Turn on all ports meant for output call Delay_0125ms ;Delay for required time ldi n3, 0x00 ;register that stores output data out portd, n3 ;Now, turn them off call Delay_2ms ;delay for requried time inc n1 ;increase the loop counter cpse n1, n2 ;see it it meets the loop # if so, skip the jmp jmp Servo45Loop ;jump back to the loop :) ret ;Return to original position ; ;----------------------------------------------------------- ; Servo90: ;Servo Position | ;Requires a pulse of 1.5 ms ldi n1, 0x00 ;loop counter set to zero ldi n2, 0xFF ;number of times to loop (255 here) Servo90Loop: ldi n3, 0xFF ;register that stores output data... out portd, n3 ;Turn on all ports meant for output call Delay_015ms ;Delay for required time ldi n3, 0x00 ;register that stores output data out portd, n3 ;Now, turn them off call Delay_2ms ;delay for requried time inc n1 ;increase the loop counter cpse n1, n2 ;see it it meets the loop # if so, skip the jmp jmp Servo90Loop ;jump back to the loop :) ret ;Return to original position ; ;----------------------------------------------------------- ; Servo135:; Servo Position / ;Requires a pulse of 1.75 ms ldi n1, 0x00 ;loop counter set to zero ldi n2, 0xFF ;number of times to loop (255 here) Servo135Loop: ldi n3, 0xFF ;register that stores output data... out portd, n3 ;Turn on all ports meant for output call Delay_0175ms ;Delay for required time ldi n3, 0x00 ;register that stores output data out portd, n3 ;Now, turn them off call Delay_2ms ;delay for requried time inc n1 ;increase the loop counter cpse n1, n2 ;see it it meets the loop # if so, skip the jmp jmp Servo135Loop ;jump back to the loop :) ret ;Return to original position ; ;----------------------------------------------------------- ; Servo160: ;Between angle 0 & 45 - Used for standing position ;Requires a pulse of 1.89 ms ldi n1, 0x00 ;loop counter set to zero ldi n2, 0xFF ;number of times to loop (255 here) Servo160Loop: ldi n3, 0xFF ;register that stores output data... out portd, n3 ;Turn on all ports meant for output call Delay_019ms ;Delay for required time ldi n3, 0x00 ;register that stores output data out portd, n3 ;Now, turn them off call Delay_2ms ;delay for requried time inc n1 ;increase the loop counter cpse n1, n2 ;see it it meets the loop # if so, skip the jmp jmp Servo160Loop ;jump back to the loop :) ret ;Return to original position ; ;----------------------------------------------------------- ; Servo180: ;Servo Position -> ;Requires a pulse of 2 ms ldi n1, 0x00 ;loop counter set to zero ldi n2, 0xFF ;number of times to loop (255 here) Servo180Loop: ldi n3, 0xFF ;register that stores output data... out portd, n3 ;Turn on all ports meant for output call Delay_2ms ;Delay for required time ldi n3, 0x00 ;register that stores output data out portd, n3 ;Now, turn them off call Delay_2ms ;delay for requried time inc n1 ;increase the loop counter cpse n1, n2 ;see it it meets the loop # if so, skip the jmp jmp Servo180Loop ;jump back to the loop :) ret ;Return to original position ; ;----------------------------------------------------------- ; ; ;----------------------------------------------------------- ; ServoHold: ;Servo Position -> ;Requires a pulse of 2 ms ldi ir, 0x00 ldi r17, 0x0F ldi n1, 0x00 ;loop counter set to zero ldi n2, 0xFF ;number of times to loop (255 here) ServoHoldLoop: ldi n3, 0xFF ;register that stores output data... out portd, n3 ;Turn on all ports meant for output ServoHoldLoop2: call Delay_005ms ;Delay for required time inc ir cpse ir, r17 jmp ServoHoldLoop2 ldi n3, 0x00 ;register that stores output data out portd, n3 ;Now, turn them off call Delay_1ms ;delay for requried time inc n1 ;increase the loop counter cpse n1, n2 ;see it it meets the loop # if so, skip the jmp jmp ServoHoldLoop ;jump back to the loop :) ret ;Return to original position ; ;----------------------------------------------------------- ; ;single ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ; ;----------------------------------------------------------- ; ; ;----------------------------------------------------------- ; ;Multi ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ;Servo Position Control Subroutine - Start ; ;----------------------------------------------------------- ; ;Servo Variable list - between 0x63 - 0x80 ;Counter1 - n1 ;Counter2 - n2 ;Time Dealy 1 - n3 ;Time Delay 2 - d1 ;Current Servo State - d2 ;Note for d1 & d2 variable. ;The time dealy subroutine is not used, so these variables will ;not be altered during the servo control loop ServoSetMemory: ;Sets up the X variable, and disables all ;servos by default ;This is just here to help stop complications ldi XL, Low(0x63) ldi XH, High(0x7A) ;DO NOT CHANGE THIS VALUE OUTSIDE ;OF THIS SUBROUTINE. IT WILL REMOVE ;THE MEMORY THAT THE X VARIABLE ;CAN ACCESS ;Servo 0 ldi n1, 0x00 st X+, n1 ;Clear Counter Variable ldi n1, 250 st X+, n1 ;Store Default Delay Value ldi n1, 2 st X+, n1 ;Servo is disabled... ;Servo 1 ldi n1, 0x00 st X+, n1 ldi n1, 250 st X+, n1 ldi n1, 2 st X+, n1 ;Servo 2 ldi n1, 0x00 st X+, n1 ldi n1, 250 st X+, n1 ldi n1, 2 st X+, n1 ;Servo 3 ldi n1, 0x00 st X+, n1 ldi n1, 250 st X+, n1 ldi n1, 2 st X+, n1 ;Servo 4 ldi n1, 0x00 st X+, n1 ldi n1, 250 st X+, n1 ldi n1, 2 st X+, n1 ;Servo 5 ldi n1, 0x00 st X+, n1 ldi n1, 250 st X+, n1 ldi n1, 2 st X+, n1 ;Offsets below this line, are used to store the angle ;when the time is switched to 2ms for the off ;period of the pulse ;Servo 0 st X+, n1 ;Servo 1 st X+, n1 ;Servo 2 st X+, n1 ;Servo 3 st X+, n1 ;Servo 4 st X+, n1 ;Servo 5 st X, n1 ret ServoSet0: ;Find proper offset for variable, by ;taking the start address (0x63) and then ;adding # of variables (5) * Servo # ldi n2, 3 ;Number of servo variables mul n2, n1 ;# of servo variables X Servo # mov n2, r0 ;Saves results to n2 ldi XL, 0x63 ;Changes index variable to 0x63 add XL, n2 ;Adds n2 to 0x63 ;Store the proper variables into the memory ldi n2, 0x00 ;Set counter 1 to zero st X+, n2 ;Save new counter value ldi n2, 10 ;Set Delay to proper value st X+, n2 ;Save new Delay Value ldi n2, 0 ;Set new Servo State (0 - off, 1 - on, 2 -disabled) st X+, n2 ;Save servo state ldi n2, 1 ;Number of servo variables mul n2, n1 ;# of servo variables X Servo # mov n2, r0 ;Saves results to n2 ldi XL, 0x75 ;Changes index variable to 0x75 add XL, n2 ;Adds n2 to 0x81 ;Store the proper variables into the memory ldi n2, 10 ;Time delay value, requried for angle st X+, n2 ;Save delay value ret ServoSet20: ldi n3, 11 ;Dealy amount - tweak timing here... ldi n2, 3 mul n2, n1 mov n2, r0 ldi XL, 0x63 add XL, n2 ldi n2, 0x00 st X+, n2 mov n2, n3 st X+, n2 ldi n2, 0 st X+, n2 ldi n2, 1 mul n2, n1 mov n2, r0 ldi XL, 0x75 add XL, n2 mov n2, n3 st X+, n2 ret servoSet45: ldi n3, 13 ;Dealy amount - tweak timing here... ldi n2, 3 mul n2, n1 mov n2, r0 ldi XL, 0x63 add XL, n2 ldi n2, 0x00 st X+, n2 mov n2, n3 st X+, n2 ldi n2, 0 st X+, n2 ldi n2, 1 mul n2, n1 mov n2, r0 ldi XL, 0x75 add XL, n2 mov n2, n3 st X+, n2 ret ServoSet90: ldi n3, 15 ;Dealy amount - tweak timing here... ldi n2, 3 mul n2, n1 mov n2, r0 ldi XL, 0x63 add XL, n2 ldi n2, 0x00 st X+, n2 mov n2, n3 st X+, n2 ldi n2, 0 st X+, n2 ldi n2, 1 mul n2, n1 mov n2, r0 ldi XL, 0x75 add XL, n2 mov n2, n3 st X+, n2 ret ServoSet135: ldi n3, 18 ;Dealy amount - tweak timing here... ldi n2, 3 mul n2, n1 mov n2, r0 ldi XL, 0x63 add XL, n2 ldi n2, 0x00 st X+, n2 mov n2, n3 st X+, n2 ldi n2, 0 st X+, n2 ldi n2, 1 mul n2, n1 mov n2, r0 ldi XL, 0x75 add XL, n2 mov n2, n3 st X+, n2 ret ServoSet160: ldi n3, 19 ;Dealy amount - tweak timing here... ldi n2, 3 mul n2, n1 mov n2, r0 ldi XL, 0x63 add XL, n2 ldi n2, 0x00 st X+, n2 mov n2, n3 st X+, n2 ldi n2, 0 st X+, n2 ldi n2, 1 mul n2, n1 mov n2, r0 ldi XL, 0x75 add XL, n2 mov n2, n3 st X+, n2 ret ServoSet180: ldi n3, 20 ;Dealy amount - tweak timing here... ldi n2, 3 mul n2, n1 mov n2, r0 ldi XL, 0x63 add XL, n2 ldi n2, 0x00 st X+, n2 mov n2, n3 st X+, n2 ldi n2, 0 st X+, n2 ldi n2, 1 mul n2, n1 mov n2, r0 ldi XL, 0x75 add XL, n2 mov n2, n3 st X+, n2 ret ServoDisable: ldi n3, 250 ;Dealy amount - tweak timing here... ldi n2, 3 mul n2, n1 mov n2, r0 ldi XL, 0x63 add XL, n2 ldi n2, 0x00 st X+, n2 mov n2, n3 st X+, n2 ldi n2, 2 st X+, n2 ldi n2, 1 mul n2, n1 mov n2, r0 ldi XL, 0x75 add XL, n2 mov n2, n3 st X+, n2 ret ServoCycle: ;Servo Position -> ;Notes on Timeing ; ;Currently, it takes 37 cycles to process each servo ;I've made this value static, so it won't change when ;you increment the counter, turn the port on, turn ;the port off, or just reset the disabled servo variables ; ;So, since We currently have 6 servos (0-5), it takes 222 ;cycles to update all of their values ; ;Now, add in the two lines below ldi d2, 20 ;off delay value ldi XL, Low(0x63) ;ldi only takes one cycle, so that pushes us up to 224 ; ;of course, wear not done yet. We still have to factor in ;the time it takes to jump to this subroutine (through call) ;as well as to return to it's original position ;(through ret) ; ;both of these take 4 cycles, so our total is 232 cycles for this ;subroutine ; ;To better adjust the time, we simply need to add nop below this ;statement. Each one will increase the dealy by 1 cycle. ;For now, we should increase this value to 400 ; ;Why? Well the internal oscillator for my Atmega 32 is set to ;4MHz, or 4,000,000 Hz. In other words, at 4MHz, the ATMega 32 ;can handle 4 million cycles a second. ; ;So, while it's running at 4MHz, it takes 4000 cycles for one ;milisecond (4,000,000 / 1,000, since there are 1000 ms in a sec) ; ;That tells me that I need to loop this subroutine 10 times for 1 ms ;or 20 time for 2 ms. ; ;So, how does this help me out? Well, since I know how many times ;this subroutine needs to loop for a specified time, I can easily ;calibrate the timeing of the pulses. ; ;Quick table, based on calculated value ;Dealy - 10 - 1.0 ms - 0 degrees ;Dealy - 11 - 1.1 ms - 18 degrees ;Dealy - 12 - 1.2 ms - 36 degrees ;Dealy - 13 - 1.3 ms - 54 degrees ;Dealy - 14 - 1.4 ms - 72 degrees ;Dealy - 15 - 1.5 ms - 90 degrees ;Dealy - 16 - 1.6 ms - 108 degrees ;Dealy - 17 - 1.7 ms - 126 degrees ;Dealy - 18 - 1.8 ms - 114 degrees ;Dealy - 19 - 1.9 ms - 162 degrees ;Dealy - 20 - 2.0 ms - 180 degrees ; ; ;nop x168 -3 for cycles used from calling this subroutine ;of course, having 165 nop would be a pain, so just loop it and ;subtract the proper values. nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop ;Load Servo 0 variables ld n1, X+ ;Load Counter ld n2, X+ ;Load Dealy amount ld n3, X+ ;Load Servo State inc n1 ;Increase the counter cpse n1, n2 ;Check to see if n1 = n2 jmp Servo0ExtraDelay ;If not, go to dealy ldi d3, 0x00 ;Servo Off State value cpse n3, d3 ;Check to see if the servo is off jmp Servo0OffOrDis ;If it's not off, go to <- sbi PortD, PD0 ;Turn on the port ldi XL, Low(0x75) ;Load reserved delay location ld n1, X ;Load reserved delay amount Mov n2, n1 ;Copy value to register (n2) ldi n1, 0x00 ;Reset counter ldi n3, 0x01 ;Update servo state nop ;Delay, used to even out processing time nop ;^ nop ;^ nop ;^ nop ;^ jmp Servo1Cycle ;Go to the next servo Servo0OffOrDis: ldi d3, 0x01 ;Servo On State cpse n3, d3 ;Check to see if the servo is on jmp servo0Disabled ;If not, it's disabled, so go here cbi PortD, PD0 ;Turn off the port ldi n1, 0x00 ;Reset Counter mov n2, d2 ;load 2ms dealy amount ldi n3, 0x00 ;load new servo state (off) nop ;Delay used to even out the timeing jmp Servo1Cycle ;Go to next servo Servo0Disabled: ldi n1, 0x00 ;Reset Counter ldi n2, 250 ;Load new dealy amount ldi n3, 0x02 ;Load servo state jmp Servo1Cycle ;Go to the next servo Servo0ExtraDelay: nop ;Dealy used to even out the number nop ;of used cycles. nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ nop ;^ Servo1Cycle: ldi XL, Low(0x63) ;Load proper offset st x+, n1 ;Save servo variables st x+, n2 st x+, n3 ;Load Servo 1 variables ld n1, X+ ld n2, X+ ld n3, X+ inc n1 cpse n1, n2 jmp Servo1ExtraDelay ldi d3, 0x00 cpse n3, d3 jmp Servo1OffOrDis sbi PortD, PD1 ldi XL, Low(0x76) ld n1, X Mov n2, n1 ldi n1, 0x00 ldi n3, 0x01 nop nop nop nop nop jmp Servo2Cycle Servo1OffOrDis: ldi d3, 0x01 cpse n3, d3 jmp servo1Disabled cbi PortD, PD1 ldi n1, 0x00 mov n2, d2 ldi n3, 0x00 nop jmp Servo2Cycle Servo1Disabled: ldi n1, 0x00 ldi n2, 250 ldi n3, 0x02 jmp Servo2Cycle Servo1ExtraDelay: nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop Servo2Cycle: ldi XL, Low(0x66) st x+, n1 st x+, n2 st x+, n3 ;Load Servo 2 variables ld n1, X+ ld n2, X+ ld n3, X+ inc n1 cpse n1, n2 jmp Servo2ExtraDelay ldi d3, 0x00 cpse n3, d3 jmp Servo2OffOrDis sbi PortD, PD2 ldi XL, Low(0x77) ld n1, X Mov n2, n1 ldi n1, 0x00 ldi n3, 0x01 nop nop nop nop nop jmp Servo3Cycle Servo2OffOrDis: ldi d3, 0x01 cpse n3, d3 jmp servo2Disabled cbi PortD, PD2 ldi n1, 0x00 mov n2, d2 ldi n3, 0x00 nop jmp Servo3Cycle Servo2Disabled: ldi n1, 0x00 ldi n2, 250 ldi n3, 0x02 jmp Servo3Cycle Servo2ExtraDelay: nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop Servo3Cycle: ldi XL, Low(0x69) st x+, n1 st x+, n2 st x+, n3 ;Load Servo 3 variables ld n1, X+ ld n2, X+ ld n3, X+ inc n1 cpse n1, n2 jmp Servo3ExtraDelay ldi d3, 0x00 cpse n3, d3 jmp Servo3OffOrDis sbi PortD, PD3 ldi XL, Low(0x78) ld n1, X Mov n2, n1 ldi n1, 0x00 ldi n3, 0x01 nop nop nop nop nop jmp Servo4Cycle Servo3OffOrDis: ldi d3, 0x01 cpse n3, d3 jmp servo3Disabled cbi PortD, PD3 ldi n1, 0x00 mov n2, d2 ldi n3, 0x00 nop jmp Servo4Cycle Servo3Disabled: ldi n1, 0x00 ldi n2, 250 ldi n3, 0x02 jmp Servo4Cycle Servo3ExtraDelay: nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop Servo4Cycle: ldi XL, Low(0x6C) st x+, n1 st x+, n2 st x+, n3 ;Load Servo 4 variables ld n1, X+ ld n2, X+ ld n3, X+ inc n1 cpse n1, n2 jmp Servo4ExtraDelay ldi d3, 0x00 cpse n3, d3 jmp Servo4OffOrDis sbi PortD, PD4 ldi XL, Low(0x79) ld n1, X Mov n2, n1 ldi n1, 0x00 ldi n3, 0x01 nop nop nop nop nop jmp Servo5Cycle Servo4OffOrDis: ldi d3, 0x01 cpse n3, d3 jmp servo4Disabled cbi PortD, PD4 ldi n1, 0x00 mov n2, d2 ldi n3, 0x00 nop jmp Servo5Cycle Servo4Disabled: ldi n1, 0x00 ldi n2, 250 ldi n3, 0x02 jmp Servo5Cycle Servo4ExtraDelay: nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop Servo5Cycle: ldi XL, Low(0x6F) st x+, n1 st x+, n2 st x+, n3 ;Load Servo 5 variables ld n1, X+ ld n2, X+ ld n3, X+ inc n1 cpse n1, n2 jmp Servo5ExtraDelay ldi d3, 0x00 cpse n3, d3 jmp Servo5OffOrDis sbi PortD, PD5 ldi XL, Low(0x7A) ld n1, X Mov n2, n1 ldi n1, 0x00 ldi n3, 0x01 nop nop nop nop nop jmp Servo6Cycle Servo5OffOrDis: ldi d3, 0x01 cpse n3, d3 jmp servo5Disabled cbi PortD, PD5 ldi n1, 0x00 mov n2, d2 ldi n3, 0x00 nop jmp Servo6Cycle Servo5Disabled: ldi n1, 0x00 ldi n2, 250 ldi n3, 0x02 jmp Servo6Cycle Servo5ExtraDelay: nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop Servo6Cycle: ldi XL, Low(0x72) st x+, n1 st x+, n2 st x+, n3 ret ;Return to original position ; ;----------------------------------------------------------- ; ;Multi ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ;Servo Position Control Subroutine - End ; ;----------------------------------------------------------- ; ; ;----------------------------------------------------------- ; ; Time Delay Sub Routines Start ; Time Delay Sub Routines Start ; Time Delay Sub Routines Start ; Time Delay Sub Routines Start ; Time Delay Sub Routines Start ; Time Delay Sub Routines Start ; Time Delay Sub Routines Start ; Time Delay Sub Routines Start ; Time Delay Sub Routines Start ; Time Delay Sub Routines Start ; ;----------------------------------------------------------- ; .org 0x2000 ;required for ret ;Sets all variables, based on the requested delay time Delay_1sec: ;Delay of 250000 ldi d1, 250 ;1 - clocks used ldi d2, 245 ;1 ldi d3, 3 ;1 jmp Time_Loop Delay_3qsec: ;Delay of 187500 ldi d1, 187 ldi d2, 184 ldi d3, 4 jmp Time_Loop Delay_2qsec: ;Delay of 125000 ldi d1, 166 ldi d2, 138 ldi d3, 4 jmp Time_Loop Delay_1qsec: ;Delay of 62500 ldi d1, 200 ldi d2, 77 ldi d3, 3 jmp Time_Loop Delay_125ms: ;Delay of 31250 ldi d1, 203 ldi d2, 113 ldi d3, 1 jmp Time_Loop Delay_100ms: ;Delay 25000 ldi d1, 131 ldi d2, 70 ldi d3, 2 jmp Time_Loop Delay_2ms: ;delays 2 ms - 90 degrees ldi d1, 30 ldi d2, 15 ldi d3, 1 jmp Time_Loop Delay_019ms: ;delays 1.9 ms ldi d1, 26 ldi d2, 15 ldi d3, 1 jmp Time_Loop Delay_0175ms: ;delays 1.75 ms - 45 degrees ldi d1, 184 ldi d2, 2 ldi d3, 1 jmp Time_Loop Delay_017ms: ;delays 1.7 ms ldi d1, 21 ldi d2, 13 ldi d3, 1 jmp Time_Loop Delay_015ms: ;delays 1.5 ms - 0 degrees ldi d1, 140 ldi d2, 2 ldi d3, 1 jmp Time_Loop Delay_013ms: ;delays 1.3 ms ldi d1, 18 ldi d2, 13 ldi d3, 1 jmp Time_Loop Delay_0125ms: ;delays 1.25 ms - -45 degrees ldi d1, 170 ldi d2, 1 ldi d3, 1 jmp Time_Loop Delay_011ms: ;delays 1.1 ms ldi d1, 135 ldi d2, 1 ldi d3, 1 jmp Time_Loop Delay_1ms: ;delays 1 ms - -90 degrees ldi d1, 100 ldi d2, 1 ldi d3, 1 jmp Time_Loop Delay_005ms: ;delays .05 ms ldi d1, 4 ldi d2, 1 ldi d3, 1 jmp Time_Loop Time_Loop: ;Time Delay Loop ; ;----------------------------------------------------------- ; ;Stores current value of n1-3, so there data won't be lost sts 0x60, n1 sts 0x61, n2 sts 0x62, n3 ;And then clears their values for the delay routine ldi n1, 0x00 ldi n2, 0x00 ldi n3, 0x00 ; ;----------------------------------------------------------- ; TDelay1: TDelay2: TDelay3: inc n1 ;1 - clocks used CPSE n1, d1 ;1 jmp TDelay3 ;.5 ldi n1, 0x00 ;1 inc n2 ;1 cpse n2, d2 ;1 jmp TDelay2 ;.5 ldi n2, 0x00 ;1 inc n3 ;1 cpse n3, d3 ;1 jmp TDelay1 ;.5 ldi n3, 0x00 ;1 ; ;----------------------------------------------------------- ; ;Loads the values from the memory back into the registers lds n1, 0x60 lds n2, 0x61 lds n3, 0x62 ; ;----------------------------------------------------------- ; ret ;returns to origin ; Time Delay Sub Routines End ; Time Delay Sub Routines End ; Time Delay Sub Routines End ; Time Delay Sub Routines End ; Time Delay Sub Routines End ; Time Delay Sub Routines End ; Time Delay Sub Routines End ; Time Delay Sub Routines End ; Time Delay Sub Routines End ; Time Delay Sub Routines End