上傳檔案到 ''

TQL.py

@@ -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")