Hade inte funderat över det innan så fick tänka till lite. Men visst delar av koden släpper jag gärna. Versionen här är hela första utkastet till PD regulatorn. Dvs den saknar lite konstanter samt vissa av funktionerna nämnda ovan. Men det är den koden jag jobbat vidare med och det är inte helt svårt att lägga till delar själv.
Jag är medveten om att jag är långt ifrån att utnyttja processorn fullt ut och att mycket kan finslipas för att göras snabbare, jämnare mm. Men detta funkar
, se senaste filmen om ni inte tror mig. Garanterar inte att allt är 100% ok, men det borde vara det.
Förresten vad är praxis för kod på forumet? Att kopiera in den i inlägget eller att länka till den som med bilder? Lägger in det här så länge, för beständighetens skull om inte annat, så får ni rätta mig.
Men kommentera gärna koden. Även om mycket inte är optimalt så är det tillräckligt för mitt projekt. Men fler projekt kommer ju at följa detta så tips är välkomna. Nästa projekt kommer att bli någon slags inverterad pendel, vilken kanske kan kräva lite uppstramning av koden.
Förresten, har jag förstått det rätt att programmet i någon mening liknar hur en PLC fungerar?
Håll tillgodo.
Kod: Markera allt
;This is the fourth level of the Line follower. The robot is
;now able to follow a line of black duct tape somewhat fast and is nicely
;stable. In this v1.1 a PD regulator is implemented. Hardware is somewhat
;bad design, but software brings the total up.
;Port configuration. Port C is input from phototransistors: PC0 to left
;and PC1 to right. ;Port B is pwm output to motors ac to: PB0 left motor
;(reg OutPwmL) and PB1 to right motor (reg OutPwmR).
;For ATmega88
.include "m88def.inc"
.def Speed =r16 ; ;Contains the speed provided by P part, change in Scale.
.def OutPwmL = r17 ;Contains the time high for left motor.
.def OutPwmR = r21 ;Contains the time high for right motor.
.def I =r18 ;Loop (delay) variables.
.def J = r19
.def Temp = r20 ;Temporary registers, may be destroyed by subroutines.
.def Temp2 = r22
.def OutMotor = r23 ;The register to send out to motors.
.def Error = r24
RESET:
ser Temp
out DDRD,Temp ;
ldi Temp,3 ;Set ports.
out DDRB,Temp
ldi Temp, low(RAMEND)
out SPL, Temp
ldi Temp, high(RAMEND)
out SPH, Temp ;Stack pointer set.
ldi XH,0x01
ldi XL,0x00 ;Used for SRAM access.
cbi PRADC,0 ;Turn power saving device of.
ldi Temp,0b00100000 ;Uses ADLAR, and start to read left.
sts ADMUX,Temp ;tell ATmega.
ldi Temp,0b10000000
sts ADCSRA,Temp ;Enable ADC.
MAIN:
rcall Get_Analog
rcall Scale
rcall MAKE_CYCLE
rjmp MAIN
;**************************** Subroutines ****************************
;Destroyes Temp registers.
GET_ANALOG:
;Returns the analog value on PC0..1 to digital 8 bit
;values in reg OutPwmL and OutPwmR resp.
ldi Temp,0b00100000 ;This reads the PC0.
sts ADMUX,Temp ;Set mux to read left phototr, on PC0.
ldi Temp,0b11000000 ;Keep ADC enabled and start conversion.
sts ADCSRA,Temp
WAIT_L: ;Wait for conversion to finish.
sbic ADIF,4 ;"ADIF,4" and "ADSC,6" will work with short delay after.
rjmp WAIT_L
rcall ANALOG_DLY ;Needed.
lds OutPwmL,ADCH
;* Left value done, get right.
ldi Temp,0b00100001 ;Read PC1.
sts ADMUX,Temp ;Set mux to read right phototr, on PC1.
ldi Temp,0b11000000 ;Keep ADC enabled and start conversion on PC1.
sts ADCSRA,Temp
WAIT_R:
sbic ADIF,4 ;"ADIF,4" and "ADSC,6" will work with short delay after.
rjmp WAIT_R
rcall ANALOG_DLY ;Must have this.
lds OutPwmR,ADCH
ret
;*************************************************************************
Scale:
;This routine maps the values in reg OutPwmL/R into other values in
;the same registers to those better suited for control of the Line follower.
;The PD regulator is within this function.
ldi XL,00 ;SRAM-pointer.
ld Temp,X+ ;Last value from ADC, L.
ld Temp2,X+ ;Last value from ADC, R.
;*** First get D-part. Must not alter OutPwm registers, so push them.
push OutPwmL
push OutPwmR
sub Temp2,Temp ;= last (R-L)
mov Error,OutPwmR
sub Error,OutPwmL ;= present (R-L)
sub Error,Temp2 ;= present(R-L) - last(R-L)
brcs D_NEGATIVE
;*** Robot moves to right if gets here.
mov Temp2,Error ;:More speed to right.
inc Temp2
ldi Temp,0 ;Dont increase left motors speed
rjmp D_DONE
D_NEGATIVE:
neg Error
;*** The robot moves to the left if gets here, inc left.
mov Temp,Error
inc Temp
ldi Temp2,0 ;Dont boost right motor.
D_DONE:
;* First restore registers and update SRAM.
pop OutPwmR
pop OutPwmL
st -X,OutPwmR ;Store present value from ADC before change them.
st -X,OutPwmL
com Error
out PORTD,Error ;Debugging reason
com Error
;*** And now get P-part, the same as original way.
ldi Speed,50 ;Speeds 40-50 for small track, may be greater for larger.
cp OutPwmR,OutPwmL
brcs GO_MORE_RIGHT ;Branch if L > R
;* Go more left: OutPwmR > OutPwmL from ADC, so increase right hand side pw.
mov Error,OutPwmR
sub Error,OutPwmL ;=R-L > 0
dec Speed
cp Error,Speed
brcs LEFT_WITHIN ;Put upper bound on error since pw is bounded.
mov Error,Speed
LEFT_WITHIN:
inc Speed
mov OutPwmL,Speed
mov OutPwmR,Speed
add OutPwmR,Error
sub OutPwmL,Error
rjmp SCALE_RETURN
GO_MORE_RIGHT: ;Increase left hand side motors pw.
;*Know that OutPwmL > OutPwmR
mov Error,OutPwmL
sub Error,OutPwmR ;=L-R > 0
dec Speed
cp Error,Speed
brcs RIGHT_WITHIN ;Put upper bound on error since pwm is bounded.
mov Error,Speed
RIGHT_WITHIN:
inc Speed
mov OutPwmL,Speed
mov OutPwmR,Speed
add OutPwmL,Error
sub OutPwmR,Error
SCALE_RETURN:
add OutPwmL,Temp ;Adjust from D part last.
add OutPwmR,Temp2
ret
;******************************************************************
MAKE_CYCLE:
;Makes one duty cycle for both motors. Time high is in reg OutPwmL/R.
push J
ldi J,6 ;Dly for D part of regulator, etc.
NBR_CYCLES:
push I
clr I ;Full cycle
ldi OutMotor,0b00000011 ;Set high
mov Temp, OutPwmL
mov Temp2,OutPwmR
inc Temp ;To get zero right.
inc Temp2
MAKE_CYCLE_RET: ;This is the loop itself
dec Temp
breq C_L ;If zero, then clear LED
LAT1:
dec Temp2
breq C_R
LAT2:
rcall PWM_DLY ;Use to change pwm frequency.
out PORTB,OutMotor
dec I ;Sets the cycle
brne MAKE_CYCLE_RET
pop I
dec J
brne NBR_CYCLES
pop J
ret
;*************** TEMPORARY ROUTINES ***********
;Belongs to MAKE_CYCLE.
C_L:
rcall C_Lt
rjmp LAT1
C_Lt: ;Clear Left
andi OutMotor,0b00000010 ;Possibly keep right high.
ret
C_R:
rcall C_Rt
rjmp LAT2
C_Rt:
andi OutMotor,0b00000001 ;Possibly keep left high.
ret
;*************** DELAY ROUTINES ****************
ANALOG_DLY:
push J
ldi J,0x05
ANALOG_DLY_T:
dec J
brne ANALOG_DLY_T
pop J
ret
PWM_DLY:
push J
ldi J,0x05
PWM_DLY_T:
dec J
brne PWM_DLY_T
pop J
ret
mvh
. Men feedbacken här verkar ju god, så den kommer kanske ut där senare. 
. Med hårdvaran möjligen men nu jobbar jag med att få till mjukvaran bättre för att den ska gå snabbare. I filmen som ligger på diino gör den banan på ganska precis 20s. Nu är jag nere i 15 efter att ha laborerat vidare med P och D. Vad tror ni förresten om att förutom att ta hänsyn till reglerfelet och derivatan på detsamma även ta hänsyn till andraderivatan? Borde man kunna få den att reagera snabbare tror ni? Ska även försöka lägga in lite funktioner som gör att den utnyttjar banan lite bättre, så som jämnare fart i kurvorna och snabbare där det inte svänger så mycket. Ni får gärna komma med idéer också. Jag är mottaglig för tips nu när jag kommit så pass långt med den att jag ändå kan kalla den min ide, även om jag tar emot tips 
(det var därför jag valde att lägga ut allt först när det var färdigt). 
. 
