#---------sensa ------------------------------
class Sensa
def initialize(aa)
@ana=AnalogIO.new(aa,INPUT)
end
def average(k)
sum=0
k.times do
[email protected]
sum=sum+an
delay_us(200)
end
avg=sum/k
return avg
end
def hensa(shoki,limit)
[email protected]
hensa=( aan - shoki )
if (-limit < hensa) && (hensa < limit)
hensa=0
end
return hensa
end
end
#------------keisan-------------------------------------
class Keisan
def initialize(kag,kagsp,kpwsm,kpw)
@kag=kag; @kagsp=kagsp; @kpwsm=kpwsm; @kpw=kpw
end
def angle_pwm(ags,ac)
tsmp=0.002; coff=1.25
$ang = $ang + ags*tsmp - $ang*coff*tsmp + ac*0.9*coff*tsmp
$pw = $pw + @kag*$ang + @kagsp*ags + @kpwsm*$psum + @kpw*$pw
$pw = 100 if $pw>100
$pw = -100 if $pw<-100
if $ang<-60 || 60<$ang
$pw=0
end
$psum = $pw*tsmp + $psum
return $pw
end
end
#--------motor control class--------------------------------
class Motor
def initialize(pw1,in1,in2,t)
pre=300000; @tm=t
@a= AnalogIO.new(pw1,OUTPUT,10,t,pre);
@b= DigitalIO.new(in1,OUTPUT)
@c= DigitalIO.new(in2,OUTPUT)
end
def go(pw)
if pw>=0
@a.write pw*@tm/100; @b.high; @c.low
else
mpw=-pw
@a.write mpw*@tm/100; @b.low; @c.high
end
end
end
#-------------PWM=1msec----------------------------------
migiMotor=Motor.new(D8,D9,D10,300)
hidariMotor=Motor.new(D6,D5,D7,300)
Kang=0.018; KangSpd=0.009; KpwmSum=0.0005; Kpwm=0.0004
#Kang=0.08; KangSpd=0.03; KpwmSum=0.0012; Kpwm=0.0015
#Kang=0.018; KangSpd=0.009; KpwmSum=0.0042; Kpwm=0.0038
kk=Keisan.new(Kang, KangSpd, KpwmSum, Kpwm)
ag=Sensa.new(A0)
ac=Sensa.new(A1)
delay(800)
Acofs=ac.average(100)*1.0 #2batt=0.92
Agofs=ag.average(100)*1.0
$ang=0; $psum=0; $pw=0
addPWMm=0; addPWMh=-10
#---------controll loop--------------------------------
loop do
ach=ac.hensa(Acofs,2)*0.322 #+30
agh=ag.hensa(Agofs,3)*0.481 -10.0
# print "agh=#{agh} ach=#{ach} ang=#{$ang}\n"
kpwm = kk.angle_pwm(agh,ach) #PWM keisan
migiMotor.go(kpwm + addPWMm)
hidariMotor.go(kpwm + addPWMh)
end