Q-learning-PID/CTQL.py

#!/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
import paho.mqtt.client as mqtt
import json

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=320    #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.1', 'kd+0','kd-0.1']     # 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
x=0
jsonmsg=""
pwm_1=1600
S_=""
p_prev_error=0
p_integral=0
def on_connect(client, userdata, flags, rc):
    print("Connected with result code " + str(rc))
    client.subscribe("b8:27:eb:eb:21:13/Log", qos=2)

    # 當接收到從伺服器發送的訊息時要進行的動作


def on_message(client, userdata, msg):
    # 轉換編碼utf-8才看得懂中文
    global  jsonmsg
    global  x
    msg.payload = msg.payload.decode('utf-8')
    jsonmsg = json.loads(msg.payload)
    x=int(jsonmsg['x'])
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
    
   
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(controller,system,num_iterations):
    global  jsonmsg
    global x
    global S_,R
    errors=[]
    raise_time=0
    cont=0
    for _ in range(num_iterations):
        #error = system.get_error()
        raise_time+=1
        current=x
        print(raise_time,current)
        error=system.target-current # 真實訊號
        output=controller.control(error)
        current+= output
        pwm_3=0.6156*current+1396
        if(pwm_3<1500):
               pwm_3=1500
        elif(pwm_3>1750):
               pwm_3=1750        
        pwm.setServoPulse(14,pwm_3) #底部馬達置中  
        time.sleep(0.5)
        #control_signal=controller.control(error)
        #current=system.update(control_signal)
        if ((current-system.target)>0):
            cont=raise_time
            break
        else:
            cont=50
    if cont<=10:
                S_= N_STATES[0]
                R=5
                print('raise_time success')
    elif (10<cont) and (cont<=20):
                S_= N_STATES[1]
                R=2
    elif (20< cont) and (cont <= 40):
                S_= N_STATES[2]
                R=1
    else:
                S_= N_STATES[3]
                R=1.5-(error/100)
    return  S_,R

def train2(controller,system,num_iterations):
    global x
    global S_,R
    errors=[]
    current_arr=[]
    ot=0
    over_time=0
    over_shoot=0
    for _ in range(num_iterations):
        #error = system.get_error()
        #current = 23.5 - (board.getDistance() / 10)
        current = x
        value=current
        error = system.target - current  # 真實訊號
        output=controller.control(error)
        current+= output
        pwm_3=0.6156*current+1396
        if(pwm_3<1500):
               pwm_3=1500
        elif(pwm_3>1750):
               pwm_3=1750        
        pwm.setServoPulse(14,pwm_3) #底部馬達置中  
        time.sleep(0.5)
        over_time+=1
        if(value>ot):
           ot=value
        print(over_time,ot)
    over_shoot=float(abs(ot-system.target))/320
    print("overshoot",str(over_shoot))
    if over_shoot>=0 and over_shoot < 0.0625:
        print('overshoot success')
        S_ = N_STATES[4]
        R = 5
    elif (0.0625 <= over_shoot) and (over_shoot <1):
        S_ = N_STATES[5]
        R = -1*over_shoot
    else: 
        S_ = N_STATES[0]
        R =  0
    return S_, R

def train3(controller,system,num_iterations):
    global x
    global S_,R
    errors=[]
    cont=0
    setingtime=0
    con=0
    print("3")
    for _ in range(num_iterations):
        cont=cont+1
        current = x
        error = system.target - current  # 真實訊號
        output=controller.control(error)
        current+= output
        pwm_3=0.6156*current+1396
        if(pwm_3<1500):
               pwm_3=1500
        elif(pwm_3>1750):
               pwm_3=1750        
        pwm.setServoPulse(14,pwm_3) #底部馬達置中  
        time.sleep(0.5)
        print(cont,error)
        
        if ((-1*error)>=0) and ((-1*error)<=5) and (con==0):
            setingtime =cont
            con=1
        elif(con==0):
            setingtime=40
    print(setingtime)
    if setingtime>=0 and setingtime < 10:
        S_ = N_STATES[6]
        R = 10
        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 (10 <= setingtime) and (setingtime <= 40):
        S_ = N_STATES[7]
        R = 1.5 -1*(error/100)
    else:
        S_ = N_STATES[4]
        R = -1
    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, :]
    print("3")
    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.1', 'ki-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
    print(action_name)    
    return action_name

def pid(kp):
    global  goal,count
    global S_
    R=0
    print("raisetime")
    pid_controller = PIDController(kp,0.0,0.0)
    any_system = Any_System(goal)
    S_,R = train(pid_controller, any_system,count)
    print('kp:',kp)
    return  S_,R

def pid1(kp):
    global S_
    R=0
    print("overshoot")
    pid_controller = PIDController(kp, 0.0, 0.0)
    any_system = Any_System(goal)
    S_, R = train2(pid_controller, any_system, count)
    print('kp:', kp)
    return  S_,R

def pid2(kp,ki,kd):
    global S_
    R=0
    print("setingtime")
    pid_controller = PIDController(kp,ki,kd)
    any_system = Any_System(goal)
    S_, R = train3(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,S_
    R=0
    if A == 'kp+1':    # move right
          kp+=1
          S_,R=pid(kp)
    elif A == 'kp+0.1':    # move right
          kp+=0.1
          S_,R=pid(kp)
    elif A == 'kp+0.01':    # move right
          kp+=0.01
          S_,R=pid(kp)
    elif A=='kp+0':
          kp=kp+0
          S_,R= pid(kp)
    elif A == 'kp-0.01':    # move right
          kp-=0.01
          S_,R=pid(kp)
    elif A == 'kp-0.1':    # move right
          kp-=0.1
          S_,R=pid(kp)
    elif A == 'kp-1':
          kp-=1
          S_,R= pid(kp)
          
    return S_, R

def get_env_feedback1(S, A):
    # This is how agent will interact with the environment
    global kp,S_
    R=0
    if A == 'kp+1':    # move right
          kp+=1
          S_,R=pid1(kp)
    elif A == 'kp+0.1':    # move right
          kp+=0.1
          S_,R=pid1(kp)
    elif A == 'kp+0.01':    # move right
          kp+=0.01
          S_,R=pid1(kp)
    elif A=='kp+0':
          kp=kp+0
          S_,R= pid1(kp)
    elif A == 'kp-0.01':    # move right
          kp-=0.01
          S_,R=pid1(kp)
    elif A == 'kp-0.1':    # move right
          kp-=0.1
          S_,R=pid1(kp)
    elif A == 'kp-1':    # move right
          kp-=1
          S_,R=pid1(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,S_
    R=0
    if A == 'ki+0.1':    # move right
          ki+=0.1
          S_,R=pid2(kp,ki,kd)
    elif A == 'ki+0.01':    # move right
          ki+=0.01
          S_,R=pid2(kp,ki,kd)
    elif A=='ki+0':
          ki=ki+0
          S_,R= pid2(kp,ki,kd)
    elif A == 'ki-0.1':    # move right
          ki-=0.1
          S_,R=pid2(kp,ki,kd)
    elif A == 'ki-0.01':    # move right
          ki-=0.01
          S_,R=pid2(kp,ki,kd)
    elif A == 'kd+0.1':    # move right
          kd+=0.1
          S_,R=pid2(kp,ki,kd)
    elif A=='kd+0':
          kd=kd+0
          S_,R= pid2(kp,ki,kd)
    elif A == 'kd-0.1':    # move right
          kd-=0.1
          S_,R=pid2(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():
    global  x,y,z
    # main part of RL loop
    q_table = build_q_table(N_STATES, ACTIONS)
    for episode in range(MAX_EPISODES):
        S = N_STATES[3]
        is_terminated = False
        pwm.setServoPulse(14, 1600)  # 底部馬達置中
        time.sleep(5)
        while not is_terminated:
           #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]:
              pwm.setServoPulse(14, 1600)  # 底部馬達置中
              time.sleep(3)
              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]:
               pwm.setServoPulse(14, 1600)  # 底部馬達置中
               time.sleep(3)
               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]
               print(q_table)
               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] :
               pwm.setServoPulse(14, 1600)  # 底部馬達置中
               print("ok")
               time.sleep(3)
               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]
               print(q_table)
               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


def coordinate_pwm_pid():
  global p_prev_error,p_integral,x
  # ----------------------------------------
  #計算各關節角度
  while True:
    current= x
    p_error=320-current
    p_output=2.0599999999999987*p_error+ 0.1*p_integral+0.00*(p_error - p_prev_error)
    p_integral +=p_error
    p_prev_error=p_error 
    current+= p_output 
    pwm_1=0.6156*current+1396
    print(pwm_1)
    if(pwm_1<1500):
        pwm_1=1500
    elif(pwm_1>1750):
        pwm_1=1750    
    pwm.setServoPulse(14,pwm_1)
    time.sleep(0.1)
    
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)
    client = mqtt.Client()
    client.username_pw_set(username='aisky-client', password='aiskyc')
    client.connect("60.250.156.234", 1883, 60)
    client.on_connect = on_connect
    client.on_message = on_message
    client.loop_start()
    #a = raw_input("input:")
    a="t"
    if (a=="a"):
      q_table = rl()
      print('\r\nQ-table:\n')
      print(q_table)
      q_table.to_csv("/home/pi/pid.csv")
    else:
      coordinate_pwm_pid()