|
|
@@ -0,0 +1,583 @@
|
|
|
+#!/usr/bin/env python
|
|
|
+# -*- coding: UTF-8 -*-
|
|
|
+"""
|
|
|
+A simple example for Reinforcement Learning using table lookup Q-learning method.
|
|
|
+An agent "o" is on the left of a 1 dimensional world, the treasure is on the rightmost location.
|
|
|
+Run this program and to see how the agent will improve its strategy of finding the treasure.
|
|
|
+
|
|
|
+View more on my tutorial page: https://morvanzhou.github.io/tutorials/
|
|
|
+"""
|
|
|
+import numpy as np
|
|
|
+import pandas as pd
|
|
|
+import time
|
|
|
+import smbus
|
|
|
+import math
|
|
|
+import sympy
|
|
|
+from sympy import asin, cos, sin, acos,tan ,atan
|
|
|
+from DFRobot_RaspberryPi_A02YYUW import DFRobot_A02_Distance as Board
|
|
|
+
|
|
|
+np.random.seed(2) # reproducible
|
|
|
+
|
|
|
+
|
|
|
+N_STATES = ['raise_time<1','1<=raise_time<2','2<=raise_time<4','raise_time>4','0<=overshoot<0.33','0.33<overshoot<1','10<=setingtime<20','20<=setingtime<30'] ## 1:time<5 2:5.05<time<5.25 3:5.25<time<5.5 4:time>5.5
|
|
|
+goal=16 #goal
|
|
|
+ACTIONS = ['kp+1','kp+0.1','kp+0.01', 'kp+0','kp-0.01','kp-0.1','kp-1','ki+0.1','ki+0.01', 'ki+0','ki-0.01','ki-0.1','kd+0.01', 'kd+0','kd-0.01'] # available actions
|
|
|
+EPSILON = 0.9 # greedy police
|
|
|
+ALPHA = 0.1 # learning rate
|
|
|
+GAMMA = 0.9 # discount factor
|
|
|
+MAX_EPISODES =1 # maximum episodes
|
|
|
+FRESH_TIME = 0.1 # fresh time for one move
|
|
|
+
|
|
|
+kp=0.0
|
|
|
+ki=0.0
|
|
|
+kd=0.0
|
|
|
+count=50
|
|
|
+y=23.5
|
|
|
+class PCA9685:
|
|
|
+
|
|
|
+ # Registers/etc.
|
|
|
+ __SUBADR1 = 0x02
|
|
|
+ __SUBADR2 = 0x03
|
|
|
+ __SUBADR3 = 0x04
|
|
|
+ __MODE1 = 0x00
|
|
|
+ __PRESCALE = 0xFE
|
|
|
+ __LED0_ON_L = 0x06
|
|
|
+ __LED0_ON_H = 0x07
|
|
|
+ __LED0_OFF_L = 0x08
|
|
|
+ __LED0_OFF_H = 0x09
|
|
|
+ __ALLLED_ON_L = 0xFA
|
|
|
+ __ALLLED_ON_H = 0xFB
|
|
|
+ __ALLLED_OFF_L = 0xFC
|
|
|
+ __ALLLED_OFF_H = 0xFD
|
|
|
+
|
|
|
+ def __init__(self, address=0x60, debug=False):
|
|
|
+ self.bus = smbus.SMBus(1)
|
|
|
+ self.address = address
|
|
|
+ self.debug = debug
|
|
|
+ if (self.debug):
|
|
|
+ print("Reseting PCA9685")
|
|
|
+ self.write(self.__MODE1, 0x00)
|
|
|
+
|
|
|
+ def write(self, reg, value):
|
|
|
+ "Writes an 8-bit value to the specified register/address"
|
|
|
+ self.bus.write_byte_data(self.address, reg, value)
|
|
|
+ if (self.debug):
|
|
|
+ print("I2C: Write 0x%02X to register 0x%02X" % (value, reg))
|
|
|
+
|
|
|
+ def read(self, reg):
|
|
|
+ "Read an unsigned byte from the I2C device"
|
|
|
+ result = self.bus.read_byte_data(self.address, reg)
|
|
|
+ if (self.debug):
|
|
|
+ print("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % (self.address, result & 0xFF, reg))
|
|
|
+ return result
|
|
|
+
|
|
|
+ def setPWMFreq(self, freq):
|
|
|
+ "Sets the PWM frequency"
|
|
|
+ prescaleval = 25000000.0 # 25MHz
|
|
|
+ prescaleval /= 4096.0 # 12-bit
|
|
|
+ prescaleval /= float(freq)
|
|
|
+ prescaleval -= 1.0
|
|
|
+ if (self.debug):
|
|
|
+ print("Setting PWM frequency to %d Hz" % freq)
|
|
|
+ print("Estimated pre-scale: %d" % prescaleval)
|
|
|
+ prescale = math.floor(prescaleval + 0.5)
|
|
|
+ if (self.debug):
|
|
|
+ print("Final pre-scale: %d" % prescale)
|
|
|
+
|
|
|
+ oldmode = self.read(self.__MODE1);
|
|
|
+ newmode = (oldmode & 0x7F) | 0x10 # sleep
|
|
|
+ self.write(self.__MODE1, newmode) # go to sleep
|
|
|
+ self.write(self.__PRESCALE, int(math.floor(prescale)))
|
|
|
+ self.write(self.__MODE1, oldmode)
|
|
|
+ time.sleep(0.005)
|
|
|
+ self.write(self.__MODE1, oldmode | 0x80)
|
|
|
+
|
|
|
+ def setPWM(self, channel, on, off):
|
|
|
+ "Sets a single PWM channel"
|
|
|
+ self.write(self.__LED0_ON_L + 4 * channel, on & 0xFF)
|
|
|
+ self.write(self.__LED0_ON_H + 4 * channel, on >> 8)
|
|
|
+ self.write(self.__LED0_OFF_L + 4 * channel, off & 0xFF)
|
|
|
+ self.write(self.__LED0_OFF_H + 4 * channel, off >> 8)
|
|
|
+ if (self.debug):
|
|
|
+ print("channel: %d LED_ON: %d LED_OFF: %d" % (channel, on, off))
|
|
|
+
|
|
|
+ def setServoPulse(self, channel, pulse):
|
|
|
+ "Sets the Servo Pulse,The PWM frequency must be 50HZ"
|
|
|
+ pulse = pulse * 4096 / 20000 # PWM frequency is 50HZ,the period is 20000us
|
|
|
+ self.setPWM(channel, 0, int(pulse))
|
|
|
+
|
|
|
+class PIDController:
|
|
|
+ def __init__(self, Kp, Ki, Kd):
|
|
|
+ self.Kp = Kp
|
|
|
+ self.Ki = Ki
|
|
|
+ self.Kd = Kd
|
|
|
+ self.last_error = 0
|
|
|
+ self.integral = 0
|
|
|
+
|
|
|
+ def control(self, error):
|
|
|
+ output = self.Kp * error + self.Ki * self.integral + self.Kd * (error - self.last_error)
|
|
|
+ self.integral += error
|
|
|
+ self.last_error = error
|
|
|
+ return output
|
|
|
+
|
|
|
+
|
|
|
+ def ctr_pwm(self,x,y,z):
|
|
|
+ pwm_1=1750
|
|
|
+ i = 0
|
|
|
+ j = 0
|
|
|
+ # ----------------------------------------
|
|
|
+ r1 = 8
|
|
|
+ r2 = 8
|
|
|
+ # 計算各關節角度
|
|
|
+ x = (x - 320) / 100
|
|
|
+ y = float(y)
|
|
|
+ z = ((240 - z) / 100 )+ 5.3
|
|
|
+ enable = 1
|
|
|
+ print("x=", x)
|
|
|
+ if (x < -19 or x > 19): # 超出範圍離開
|
|
|
+ print("x over range")
|
|
|
+ enable = 0
|
|
|
+ # break
|
|
|
+ # y= input("input y (0~19):")
|
|
|
+ print("y=", y)
|
|
|
+ if (y < 0 or y > 19): # 超出範圍離開
|
|
|
+ print("y over range")
|
|
|
+ enable = 0
|
|
|
+ # break
|
|
|
+ # z= input("input z (3~11):")
|
|
|
+ print("z=", z)
|
|
|
+ z2 = z - 3
|
|
|
+ if (z2 < 0 or z2 > 8): # 超出範圍離開
|
|
|
+ print("z over range")
|
|
|
+ enable = 0
|
|
|
+
|
|
|
+ L = math.sqrt(x * x + y * y) - 8 # 實際夾子前端馬達6.5右邊馬達為1.5公分
|
|
|
+ print("L=", L)
|
|
|
+ if (L < 2): # 超出範圍離開
|
|
|
+ print("L over range")
|
|
|
+ enable = 0
|
|
|
+
|
|
|
+ h1 = L * L + z2 * z2 + 2 * r1 * L + r1 * r1 - r2 * r2
|
|
|
+ # print("h1=",h1)
|
|
|
+ h2 = -4 * r1 * z2
|
|
|
+ # print("h2=",h2)
|
|
|
+ h3 = L * L + z2 * z2 - 2 * r1 * L + r1 * r1 - r2 * r2
|
|
|
+ # print("h3=",h3)
|
|
|
+ try:
|
|
|
+ Theta = 2 * atan((-h2 + math.sqrt(h2 * h2 - 4 * h1 * h3)) / (2 * h1)) # 無法得出角度離開
|
|
|
+ # print("Theta=",Theta,math.degrees(Theta))
|
|
|
+ except:
|
|
|
+ print("error")
|
|
|
+ enable = 0
|
|
|
+ try:
|
|
|
+ Gamma = asin((z2 - r1 * sin(Theta)) / r2) # 無法得出角度離開
|
|
|
+ # print("Gamma=",Gamma,math.degrees(Gamma))
|
|
|
+ except:
|
|
|
+ print("error")
|
|
|
+ enable = 0
|
|
|
+ try:
|
|
|
+ tt = x / (r1 * cos(Theta) + r2 * cos(Gamma) + 8)
|
|
|
+ print(tt)
|
|
|
+ if (tt >= 1):
|
|
|
+ tt = 1
|
|
|
+ if (tt <= -1):
|
|
|
+ tt = -1
|
|
|
+ Beta = acos(tt) # 無法得出角度離開
|
|
|
+ # print("Beta=",Beta,math.degrees(Beta))
|
|
|
+ except:
|
|
|
+ print("error")
|
|
|
+ enable = 0
|
|
|
+ if (enable == 1):
|
|
|
+ # Gamma角度轉換為PWM(右邊馬達)
|
|
|
+ pwm_3 = -12.222 * (-math.degrees(Gamma)) + 2300
|
|
|
+ print("pwm_3", pwm_3)
|
|
|
+ error=pwm_3-pwm_1
|
|
|
+ print("error",error)
|
|
|
+ output = self.Kp * error + self.Ki * self.integral + self.Kd * (error - self.last_error)
|
|
|
+ self.integral += error
|
|
|
+ self.last_error = error
|
|
|
+ pwm_1+=output*10
|
|
|
+ if pwm_1>1700 and pwm_1<2300:
|
|
|
+ pwm_1=pwm_1
|
|
|
+ elif pwm_1>2300:
|
|
|
+ pwm_1=2300
|
|
|
+ print("output kp ki kd",pwm_1,self.Kp,self.Ki,self.Kd)
|
|
|
+ pwm.setServoPulse(0,2300) #前面馬達開夾子
|
|
|
+ pwm.setServoPulse(1,1750) #右邊馬達45度
|
|
|
+ pwm.setServoPulse(14,1600) #底部馬達置中
|
|
|
+ pwm.setServoPulse(15, pwm_1)
|
|
|
+
|
|
|
+class Any_System:
|
|
|
+ def __init__(self, goal):
|
|
|
+ self.target = goal
|
|
|
+ self.current = 0
|
|
|
+
|
|
|
+ def update(self, control_singal):
|
|
|
+ self.current += control_singal
|
|
|
+ return self.current
|
|
|
+
|
|
|
+ def get_error(self):
|
|
|
+ return self.target - self.current
|
|
|
+
|
|
|
+def train(S,controller,system,num_iterations):
|
|
|
+ errors=[]
|
|
|
+ raise_time=0
|
|
|
+ for _ in range(num_iterations):
|
|
|
+ #error = system.get_error()
|
|
|
+ current =23.5-(board.getDistance()/10)
|
|
|
+ with open('dis1.txt', 'a') as f:
|
|
|
+ f.write(str(current))
|
|
|
+ f.write('\r\n')
|
|
|
+ error=system.target-current # 真實訊號
|
|
|
+ controller.ctr_pwm(320,current,240)
|
|
|
+ #control_signal=controller.control(error)
|
|
|
+ #current=system.update(control_signal)
|
|
|
+ errors.append(error)
|
|
|
+ #time.sleep(0.1)
|
|
|
+ raise_time+=1
|
|
|
+ S_ = N_STATES[3]
|
|
|
+ if error >= 0 and error <= system.target:
|
|
|
+ R = 0
|
|
|
+ elif error > system.target:
|
|
|
+ R = -1
|
|
|
+ elif error < 0:
|
|
|
+ R = -1
|
|
|
+ print(raise_time,current)
|
|
|
+ if current>system.target:
|
|
|
+ print('time',raise_time)
|
|
|
+ if raise_time<10:
|
|
|
+ S_= N_STATES[0]
|
|
|
+ R=5
|
|
|
+ elif (10<=raise_time) and (raise_time<20):
|
|
|
+ S_= N_STATES[1]
|
|
|
+ R=3
|
|
|
+ elif (20<= raise_time) and (raise_time < 40):
|
|
|
+ S_= N_STATES[2]
|
|
|
+ R=2
|
|
|
+ else:
|
|
|
+ S_= N_STATES[3]
|
|
|
+ if error>0 and error < system.target:
|
|
|
+ R = 0
|
|
|
+ elif error >system.target:
|
|
|
+ R = -1
|
|
|
+ elif error <0:
|
|
|
+ R=-1
|
|
|
+ return S_, R
|
|
|
+ return S_,R
|
|
|
+
|
|
|
+def train2(S,controller,system,num_iterations):
|
|
|
+ errors=[]
|
|
|
+ current_arr=[]
|
|
|
+ overshoot_value=[]
|
|
|
+ for _ in range(num_iterations):
|
|
|
+ #error = system.get_error()
|
|
|
+ current = 23.5 - (board.getDistance() / 10)
|
|
|
+ with open('dis2.txt', 'a') as f:
|
|
|
+ f.write(str(current))
|
|
|
+ f.write('\r\n')
|
|
|
+ error = system.target - current # 真實訊號
|
|
|
+ controller.ctr_pwm(320,current,240)
|
|
|
+ #control_signal=controller.control(error)
|
|
|
+ #current=system.update(control_signal)
|
|
|
+ errors.append(error)
|
|
|
+ #time.sleep(0.1)
|
|
|
+ current_arr.append(current)
|
|
|
+ for i in range(num_iterations):
|
|
|
+ if (current_arr[i]-system.target>=0):
|
|
|
+ overshoot_value.append((current_arr[i] - system.target) / system.target)
|
|
|
+ print(i,current_arr[i])
|
|
|
+ #min(temp_arr[9:19])
|
|
|
+ #print(min(temp_arr[9:19]))
|
|
|
+ #overshoot=abs((min(temp_arr[9:19])-30)/30)
|
|
|
+ try:
|
|
|
+ overshoot=max(overshoot_value)
|
|
|
+ except:
|
|
|
+ overshoot =1
|
|
|
+ print(overshoot)
|
|
|
+ if overshoot>=0 and overshoot < 0.0625:
|
|
|
+ print('overshoot success')
|
|
|
+ S_ = N_STATES[4]
|
|
|
+ R = 2
|
|
|
+ elif (0.0625 <= overshoot) and (overshoot < 1):
|
|
|
+ S_ = N_STATES[5]
|
|
|
+ R = 1
|
|
|
+ else:
|
|
|
+ S_ = N_STATES[0]
|
|
|
+ R = 0
|
|
|
+ return S_, R
|
|
|
+
|
|
|
+def train3(S,controller,system,num_iterations):
|
|
|
+ errors=[]
|
|
|
+ current_arr=[]
|
|
|
+ for _ in range(num_iterations):
|
|
|
+ #error = system.get_error()
|
|
|
+ current = 23.5 - (board.getDistance() / 10)
|
|
|
+ with open('dis3.txt', 'a') as f:
|
|
|
+ f.write(str(current))
|
|
|
+ f.write('\r\n')
|
|
|
+ error = system.target - current # 真實訊號
|
|
|
+ controller.ctr_pwm(320,current,240)
|
|
|
+ #control_signal=controller.control(error)
|
|
|
+ #current=system.update(control_signal)
|
|
|
+ errors.append(error)
|
|
|
+ #time.sleep(0.1)
|
|
|
+ current_arr.append(current)
|
|
|
+ if (abs(current_arr[10]-system.target)) < 5:
|
|
|
+ setingtime =10
|
|
|
+ elif (abs(current_arr[20]-system.target)) < 5:
|
|
|
+ setingtime =20
|
|
|
+ elif (abs(current_arr[30]-system.target)) < 5:
|
|
|
+ setingtime =30
|
|
|
+ else:
|
|
|
+ setingtime=31
|
|
|
+ for i in range(9,49):
|
|
|
+ if (abs(current_arr[i] - system.target))>5:
|
|
|
+ setingtime=31
|
|
|
+
|
|
|
+ print(setingtime)
|
|
|
+ if setingtime>=10 and setingtime < 20:
|
|
|
+ S_ = N_STATES[6]
|
|
|
+ R = 2
|
|
|
+ print('setingtime success')
|
|
|
+ with open('pid.txt', 'a') as f:
|
|
|
+ f.write('kp:')
|
|
|
+ f.write(str(controller.Kp))
|
|
|
+ f.write('ki:')
|
|
|
+ f.write(str(controller.Ki))
|
|
|
+ f.write('kd:')
|
|
|
+ f.write(str(controller.Kd))
|
|
|
+ f.write('\r\n')
|
|
|
+ elif (20 <= setingtime) and (setingtime < 30):
|
|
|
+ S_ = N_STATES[7]
|
|
|
+ R = 1
|
|
|
+ else:
|
|
|
+ S_ = N_STATES[4]
|
|
|
+ R = 0
|
|
|
+ return S_, R
|
|
|
+
|
|
|
+def build_q_table(n_states, actions):
|
|
|
+ try:
|
|
|
+ table = pd.read_csv("/home/pi/pid.csv",index_col=0)
|
|
|
+ except:
|
|
|
+ table = pd.DataFrame(
|
|
|
+ np.zeros((len(n_states), len(actions))), # q_table initial values
|
|
|
+ columns=actions, index=n_states, # actions's name
|
|
|
+ )
|
|
|
+ print(table) # show table
|
|
|
+ return table
|
|
|
+
|
|
|
+
|
|
|
+def choose_action(state, q_table):
|
|
|
+ # This is how to choose an action
|
|
|
+ state_actions = q_table.loc[state, :]
|
|
|
+ if (np.random.uniform() > EPSILON) or ((state_actions == 0).all()): # act non-greedy or state-action have no value
|
|
|
+ ACT = ['kp+1', 'kp+0.1', 'kp+0.01', 'kp+0', 'kp-0.01', 'kp-0.1', 'kp-1']
|
|
|
+ action_name = np.random.choice(ACT)
|
|
|
+ else: # act greedy
|
|
|
+ action_name = state_actions.idxmax() # replace argmax to idxmax as argmax means a different function in newer version of pandas
|
|
|
+ return action_name
|
|
|
+
|
|
|
+def choose_action1(state, q_table):
|
|
|
+ # This is how to choose an action
|
|
|
+ state_actions = q_table.loc[state, :]
|
|
|
+ if (np.random.uniform() > EPSILON) or ((state_actions == 0).all()): # act non-greedy or state-action have no value
|
|
|
+ ACT = ['kp+1', 'kp+0.1', 'kp+0.01', 'kp+0', 'kp-0.01', 'kp-0.1', 'kp-1']
|
|
|
+ action_name = np.random.choice(ACT)
|
|
|
+ else: # act greedy
|
|
|
+ action_name = state_actions.idxmax() # replace argmax to idxmax as argmax means a different function in newer version of pandas
|
|
|
+ return action_name
|
|
|
+
|
|
|
+def choose_action2(state, q_table):
|
|
|
+ # This is how to choose an action
|
|
|
+ state_actions = q_table.loc[state, :]
|
|
|
+ if (np.random.uniform() > EPSILON) or ((state_actions == 0).all()): # act non-greedy or state-action have no value
|
|
|
+ ACT = [ 'ki+0.1', 'ki+0.01', 'ki+0', 'ki-0.01', 'ki-0.1','kd+0.01', 'kd+0','kd-0.01']
|
|
|
+ action_name = np.random.choice(ACT)
|
|
|
+ else: # act greedy
|
|
|
+ action_name = state_actions.idxmax() # replace argmax to idxmax as argmax means a different function in newer version of pandas
|
|
|
+ return action_name
|
|
|
+
|
|
|
+def pid(S,kp):
|
|
|
+ global goal,count
|
|
|
+ print('kp:',kp)
|
|
|
+ pid_controller = PIDController(kp,0.0,0.0)
|
|
|
+ any_system = Any_System(goal)
|
|
|
+ S_,R = train(S,pid_controller, any_system,count)
|
|
|
+ return S_,R
|
|
|
+
|
|
|
+def pid1(S,kp):
|
|
|
+ print("overshoot")
|
|
|
+ pid_controller = PIDController(kp, 0.0, 0.0)
|
|
|
+ any_system = Any_System(goal)
|
|
|
+ S_, R = train2(S, pid_controller, any_system, count)
|
|
|
+ print('kp:', kp)
|
|
|
+ return S_,R
|
|
|
+
|
|
|
+def pid2(S,kp,ki,kd):
|
|
|
+ print("setingtime")
|
|
|
+ pid_controller = PIDController(kp,ki,kd)
|
|
|
+ any_system = Any_System(goal)
|
|
|
+ S_, R = train3(S, pid_controller, any_system, count)
|
|
|
+ print('kp:', kp,'ki',ki,'kd',kd)
|
|
|
+ return S_,R
|
|
|
+
|
|
|
+def get_env_feedback(S, A):
|
|
|
+ # This is how agent will interact with the environment
|
|
|
+ global kp
|
|
|
+ if A == 'kp+1': # move right
|
|
|
+ kp+=1
|
|
|
+ S_,R=pid(S,kp)
|
|
|
+ elif A == 'kp+0.1': # move right
|
|
|
+ kp+=0.1
|
|
|
+ S_,R=pid(S,kp)
|
|
|
+ elif A == 'kp+0.01': # move right
|
|
|
+ kp+=0.01
|
|
|
+ S_,R=pid(S,kp)
|
|
|
+ elif A=='kp+0':
|
|
|
+ kp=kp+0
|
|
|
+ S_,R= pid(S,kp)
|
|
|
+ elif A == 'kp-0.01': # move right
|
|
|
+ kp-=0.01
|
|
|
+ S_,R=pid(S,kp)
|
|
|
+ elif A == 'kp-0.1': # move right
|
|
|
+ kp-=0.1
|
|
|
+ S_,R=pid(S,kp)
|
|
|
+ elif A == 'kp-1':
|
|
|
+ kp-=1
|
|
|
+ S_,R= pid(S,kp)
|
|
|
+ return S_, R
|
|
|
+
|
|
|
+def get_env_feedback1(S, A):
|
|
|
+ # This is how agent will interact with the environment
|
|
|
+ global kp
|
|
|
+ if A == 'kp+1': # move right
|
|
|
+ kp+=1
|
|
|
+ S_,R=pid1(S,kp)
|
|
|
+ elif A == 'kp+0.1': # move right
|
|
|
+ kp+=0.1
|
|
|
+ S_,R=pid1(S,kp)
|
|
|
+ elif A == 'kp+0.01': # move right
|
|
|
+ kp+=0.01
|
|
|
+ S_,R=pid1(S,kp)
|
|
|
+ elif A=='kp+0':
|
|
|
+ kp=kp+0
|
|
|
+ S_,R= pid1(S,kp)
|
|
|
+ elif A == 'kp-0.01': # move right
|
|
|
+ kp-=0.01
|
|
|
+ S_,R=pid1(S,kp)
|
|
|
+ elif A == 'kp-0.1': # move right
|
|
|
+ kp-=0.1
|
|
|
+ S_,R=pid1(S,kp)
|
|
|
+ elif A == 'kp-1':
|
|
|
+ kp-=1
|
|
|
+ S_,R= pid1(S,kp)
|
|
|
+ return S_, R
|
|
|
+
|
|
|
+def get_env_feedback2(S, A):
|
|
|
+ # This is how agent will interact with the environment
|
|
|
+ global ki
|
|
|
+ global kp
|
|
|
+ global kd
|
|
|
+ if A == 'ki+0.1': # move right
|
|
|
+ ki+=0.1
|
|
|
+ S_,R=pid2(S,kp,ki,kd)
|
|
|
+ elif A == 'ki+0.01': # move right
|
|
|
+ ki+=0.01
|
|
|
+ S_,R=pid2(S,kp,ki,kd)
|
|
|
+ elif A=='ki+0':
|
|
|
+ ki=ki+0
|
|
|
+ S_,R= pid2(S,kp,ki,kd)
|
|
|
+ elif A == 'ki-0.01': # move right
|
|
|
+ ki-=0.01
|
|
|
+ S_,R=pid2(S,kp,ki,kd)
|
|
|
+ elif A == 'ki-0.1': # move right
|
|
|
+ ki-=0.1
|
|
|
+ S_,R=pid2(S,kp,ki,kd)
|
|
|
+ elif A == 'kd+0.01': # move right
|
|
|
+ kd+=0.01
|
|
|
+ S_,R=pid2(S,kp,ki,kd)
|
|
|
+ elif A=='kd+0':
|
|
|
+ kd=kd+0
|
|
|
+ S_,R= pid2(S,kp,ki,kd)
|
|
|
+ elif A == 'kd-0.01': # move right
|
|
|
+ kd-=0.01
|
|
|
+ S_,R=pid2(S,kp,ki,kd)
|
|
|
+ return S_, R
|
|
|
+
|
|
|
+def update_env(S, episode, step_counter):
|
|
|
+ # This is how environment be updated
|
|
|
+ interaction = 'Episode %s: raise_time= %s' % (episode + 1,S)
|
|
|
+ #print('\r{}'.format(interaction), end='')
|
|
|
+ #print('Episode %s: raise_time= %s\r\n' % (episode + 1,S))
|
|
|
+
|
|
|
+
|
|
|
+def rl():
|
|
|
+ # main part of RL loop
|
|
|
+ global x, y, z, w
|
|
|
+ q_table = build_q_table(N_STATES, ACTIONS)
|
|
|
+ for episode in range(MAX_EPISODES):
|
|
|
+ S = N_STATES[3]
|
|
|
+ is_terminated = False
|
|
|
+ while not is_terminated:
|
|
|
+ x = 320
|
|
|
+ z = 240
|
|
|
+ y = 15.5
|
|
|
+ pwm.setServoPulse(0, 1100) # 前面馬達開夾子
|
|
|
+ pwm.setServoPulse(1, 1750) # 右邊馬達45度
|
|
|
+ pwm.setServoPulse(14, 1600) # 底部馬達置中
|
|
|
+ pwm.setServoPulse(15, 1700) # 左邊馬達垂直
|
|
|
+ time.sleep(1)
|
|
|
+ pwm.setServoPulse(0, 0)
|
|
|
+ pwm.setServoPulse(1, 0)
|
|
|
+ pwm.setServoPulse(14, 0)
|
|
|
+ pwm.setServoPulse(15, 0)
|
|
|
+ #update_env(S, episode, step_counter)
|
|
|
+ if S==N_STATES[3] or S==N_STATES[2] or S==N_STATES[1] or S==N_STATES[0]:
|
|
|
+ A = choose_action(S, q_table)
|
|
|
+ S_, R = get_env_feedback(S, A) # take action & get next state and reward
|
|
|
+ q_predict = q_table.loc[S, A]
|
|
|
+ q_target = R + GAMMA * q_table.loc[S_, :].max() # next state is not terminal
|
|
|
+ q_table.loc[S, A] += ALPHA * (q_target - q_predict) # update
|
|
|
+ print(q_table)
|
|
|
+ S = S_ # move to next state
|
|
|
+ #update_env(S, episode, step_counter)
|
|
|
+ #step_counter += 1
|
|
|
+ if S==N_STATES[0]:
|
|
|
+ S=N_STATES[5]
|
|
|
+ elif S == N_STATES[4] or S == N_STATES[5]:
|
|
|
+ print("raise_time success")
|
|
|
+ A = choose_action1(S, q_table)
|
|
|
+ S_, R = get_env_feedback1(S, A) # take action & get next state and reward
|
|
|
+ q_predict = q_table.loc[S, A]
|
|
|
+ q_target = R + GAMMA * q_table.loc[S_, :].max() # next state is not terminal
|
|
|
+ q_table.loc[S, A] += ALPHA * (q_target - q_predict) # update
|
|
|
+ S = S_ # move to next state
|
|
|
+ #update_env(S, episode, step_counter)
|
|
|
+ #step_counter += 1
|
|
|
+ if S==N_STATES[4]:
|
|
|
+ S=N_STATES[7]
|
|
|
+ elif S == N_STATES[6] or S == N_STATES[7] :
|
|
|
+ A = choose_action2(S, q_table)
|
|
|
+ S_, R = get_env_feedback2(S, A) # take action & get next state and reward
|
|
|
+ q_predict = q_table.loc[S, A]
|
|
|
+ q_target = R + GAMMA * q_table.loc[S_, :].max() # next state is not terminal
|
|
|
+ q_table.loc[S, A] += ALPHA * (q_target - q_predict) # update
|
|
|
+ S = S_ # move to next state
|
|
|
+ if S == N_STATES[6]:
|
|
|
+ is_terminated = True
|
|
|
+ #update_env(S, episode, step_counter )
|
|
|
+ return q_table
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+if __name__ == "__main__":
|
|
|
+ pwm = PCA9685(0x60, debug=False)
|
|
|
+ pwm.setPWMFreq(50)
|
|
|
+ board = Board()
|
|
|
+ dis_min = 0 #Minimum ranging threshold: 0mm
|
|
|
+ dis_max = 4500 #Highest ranging threshold: 4500mm
|
|
|
+ board.set_dis_range(dis_min, dis_max)
|
|
|
+ q_table = rl()
|
|
|
+ print('\r\nQ-table:\n')
|
|
|
+ print(q_table)
|
|
|
+ q_table.to_csv("/home/pi/pid.csv")
|
