Inverted_Robot

Inverted_Robot.rb

#!mruby
#GR-CITRUS Version 2.15
#ここには全体で使用する変数を定義しています
@Usb = Serial.new(0,115200)
@Uart = Serial.new(4, 230400)#//TX/RX:pin12/pin11, baud rate: 230400
@Uart.println "START"
#################################
# 倒立倒立振り子変数
#################################
@recOmega =[0,0,0,0,0,0,0,0,0,0]
@omega = 0  
@theta = 0
@powerScale = 0
@power = 0
@sumPower  =  0  
@sumSumP  =  0 
@kAngle = 120 
@kOmega = 130 
@kSpeed  =  40
@kDistance  = 30
@R = 0
@vE5 = 0
@xE5 = 0
p =Array.new(2)
#################################
# I2C L3GD20
#################################
@L3GD20 = 0x6A				    # 0b01101011 ジャイロセンサのアドレス ADDRESS-> 0x6A or 0x6B
@L3GD20_dps = 0.00875	        # 00:0.00875, 10:0.0175, 30:0.07 ジャイロセンサの分解能(deg/s)
@Dev = I2c.new(1)				#I2C1 pin0,1を選択

#// Motor setup
def set_up()
    #//L Motor PinSET
    pinMode(14, 1)
    pinMode(15, 1)
    #//R Motor PinSET
    pinMode(16, 1)
    pinMode(17, 1)
end
def motor_stop()
    pwm(14, 0)
    pwm(15,0)
    pwm(16, 0)
    pwm(17, 0)
end
def l_motorf()
    pwm(14, 0)
    pwm(15, 255)
end
def ll_motorf(value)
    pwm(14, 0)
    pwm(15, value)
end
def l_motorb()
    pwm(14, 255)
    pwm(15, 0)
end
def ll_motorb(value)
    pwm(14, value)
    pwm(15, 0)
end
def r_motorf()
    pwm(16, 0)
    pwm(17, 255)
end
def rr_motorf(value)
    pwm(16, 0)
    pwm(17, value)
end
def r_motorb()
    pwm(16, 255)
    pwm(17, 0)
end
def rr_motorb(value)
    pwm(16, value)
    pwm(17, 0)
end
###################################
# I2CのWrite
###################################
def I2cWrite(id, add, dat)
    @Dev.begin(id)
    @Dev.lwrite(add)
    @Dev.lwrite(dat)
    @Dev.end(1)
end
###################################
# ジャイロセンサの初期化
###################################
def initGyro()
	L3GD20_CTRL_REG1 = 0x20		# Control register
	L3GD20_ENABLE = 0xCF            #  Output data rate 760Hz select 30Hz cut off
	L3GD20_CTRL_REG4 = 0x23		# Control register
	L3GD20_CTRL_REG3 = 0x22		# Control register
	L3GD20_RANGE = 0x00			# 00:+-250dps, 10:+-500dps, 30:+-2000dps
	# Turn on all axes, disable power down
	I2cWrite(@L3GD20, L3GD20_CTRL_REG1, L3GD20_ENABLE)
	delay(100)	#100ms待つ
	# +-250dps
	I2cWrite(@L3GD20, L3GD20_CTRL_REG4, L3GD20_RANGE)
	delay(100)	#100ms待つ
end
###################################
# I2CのREAD1
###################################
def I2cRead1(id, addL)
    @Dev.begin(id)
    @Dev.lwrite(addL)
    @Dev.end(1)
    @Dev.request(id, 1)
    dl = @Dev.lread()
    return dl
end
###################################
# I2CのREAD2
###################################
def I2cRead2(id, addL, addH)
    @Dev.begin(id)
    @Dev.lwrite(addL)
    @Dev.end(1)
    @Dev.request(id, 1)
    dl = @Dev.lread()
	@Dev.begin(id)
    @Dev.lwrite(addH)
    @Dev.end(1)
	@Dev.request(id, 1)
    dh = @Dev.lread()
    return dh*256 + dl
end
#// main start
@Usb.println("main")
set_up()
motor_stop()
initGyro()
sw = 1
while (true) do
#500.times do
    if (@Uart.available())then  #//uart receive some codes
    readc = @Uart.read() #//read received codes
    p = readc.split(" ")
        if(p[0] =="k1")then
        @kAngle = p[1].to_i
        @Usb.println @kAngle.to_s
        end
        if(p[0] =="k2")then
        @kOmega = p[1].to_i
        @Usb.println @kOmega.to_s
        end
        if(p[0] =="k3")then
        @kSpeed = p[1].to_i
        @Usb.println @kSpeed.to_s
        end
        if(p[0] =="k4")then
        @kDistance = p[1].to_i
        @Usb.println @kDistance.to_s
        end
    end            
###################################
# 倒立振り子処理
###################################
    @R=0
    for i in 0..19 do
	v1 = I2cRead2(@L3GD20, 0x2A, 0x2B)
	    if v1 > 32767
		v1 = v1 - 65536
	    end
        @R = @R + v1
    end
    @omega = @R * @L3GD20_dps / 20

#   @Usb.print "@omega = "    
#   @Usb.println @omega.to_s
    if  ( @omega.abs < 2) then 
        @omega = 0
    end
    @recOmega[0] = @omega
    @theta = @theta + @omega
    count  =  0 
    for i in 0..9 do
        if ( @recOmega[i].abs < 4) then 
             count = count + 1
        end     
    end
    if (count > 9) then  
        @theta = 0
        @sumPower = 0
        @sumSumP = 0
        @vE5 = 0
        @xE5 = 0
    end
    i = 9
    while i > 0  do
        @recOmega[ i ]  =  @recOmega[ i - 1 ]
        i = i - 1
    end    
    @powerScale =  ( @kAngle * @theta /100)  + (@kOmega * @omega / 100) + ( @kSpeed * @vE5 / 1000 ) + ( @kDistance * @xE5 / 1000 )
    cc = [ 95 * @powerScale / 100  ,  255].min
    cc = cc.to_i #//truncate 
    @power  =  [  cc ,  -255].max 
    @sumPower = @sumPower + @power  
    @sumSumP = @sumSumP + @sumPower
#    @Usb.print "@power = "
#    @Usb.println @power.to_s
    @vE5 = @sumPower 
    @xE5 = @sumSumP / 1000 
    if(@power >0) 
        ll_motorf(@power)
        rr_motorf(@power)
        else 
        ll_motorb(- @power)
        rr_motorb(- @power)
    end    
   
    #LEDを点滅させます
	led(sw)
	sw = 1 - sw
end