nairin=[100,99,97,95,94,92,91,89,87,86,84,
83,81,80,79,77,75,74,73,72,70,
69,67,66,64,63,62,60,59,58,56,
55,54,52,50,49,48,46,45,43,42,
42,40,38,36,36]
class Keisan
def initialize(pgain,igain,dgain,limit)
@pg=pgain; @ig=igain; @dg=dgain; @lm=limit
end
def sousa(e)
$sek=$sek + e*0.01
bi= e - $eb
k=@pg*( e + $sek/@ig + @dg*bi )
k=@lm if k>=@lm
k=-@lm if k<=-@lm
$eb=e
return k
end
end
#--------motor control class--------------------------------
class Motor
def initialize(pt1,pt2,t)
pre=100000
@a= AnalogIO.new(pt1,OUTPUT,10,t,pre); @b= AnalogIO.new(pt2,OUTPUT,10,t,pre)
end
def go(speedg)
@a.write speedg; @b.write 0
end
def back(speedb)
@a.write 0; @b.write speedb
end
def stop
@a.write 0; @b.write 0
end
end
#-------sorvo-motor control class--------------------------------
class Servo_motor
def initialize(pt,t,pr)
@tt=t
@a= AnalogIO.new(pt,OUTPUT,10,t,pr)
end
def kakudo(kaku)
pwm= @tt/2 + kaku
@a.write pwm
end
end
#-----------------------------
pt=[D5,D6,D8,D9,D3]
pre=2000000;pwm=6000
servo_Motor3=Servo_motor.new(pt[4],pwm,pre) #3msec
klim=pwm/6
sk=Keisan.new(1.5,5000,0.8,klim)
#-------object-----------------
t=600
left_Motor=Motor.new(pt[0],pt[1],t)
right_Motor=Motor.new(pt[3],pt[2],t)
speed=250;lim=85
moku=400;offset=0
$eb=0; $sek=0
mk=Keisan.new(0.2,100000,0.01,lim)
#-----loop---------------------------------
loop do
a=analogRead(A0)
hensa=moku-a
speedR=speed;speedL=speed
sp=mk.sousa(hensa)
kk=sk.sousa(hensa)
servo_Motor3.kakudo(kk)
ka = 45*kk/klim
ka=-ka if ka<0
speedN=speed*nairin[ka.to_i]/100
speedR=speedN if hensa>offset
speedL=speedN if hensa<offset
right_Motor.go( speedR - sp )
left_Motor.go( speedL + sp )
delay(3)
end