RP_Jalousiecontrol/mc_control/mc_statemachine.py

'''
Created on 24.06.2014

@author: volkma_s

implementation for motor control state machine

list of states:

idle      - idle state  
Move_UP   - move up
UP        - in up position
Move_DOWN - move down
DOWN      - in down position 

'''

from queue import Queue
import threading
from mc_control.mc_timer import mc_timer
#from pyface_simulation import pifacedigitalio



class Position():
    def __init__(self, min_value=0, max_value=100, step=1):
        self.step=step
        self.min=min_value
        self.max=max_value
        self.position=self.min
    
    def increase(self):
        if self.position+self.step > self.max:
            self.position = self.max
        else:
            self.position = self.position+self.step
        
    def decrease(self):
        if self.position-self.step < self.min:
            self.position = self.min
        else:
            self.position = self.position-self.step
            
    def reset(self):
        self.position=self.min
                
    def SetPosition(self, position):
        self.position = position
        
class mc_event():
    Up=0
    Down=1
    Stop=2
    Timeout=3
    UpdatePosition=4
    
class mc_states():
    idle=0
    move_up=1
    move_down=2
    state_up=3
    state_down=4  
    
  
class mc_state_machine(threading.Thread):
    
    def __init__(self, Port_Up, Port_Down, timeout_value=60):
        threading.Thread.__init__(self)
        self.MailBox = Queue()
        self.__running = True 
        self.Port_Up = Port_Up
        self.Port_Down = Port_Down
        #self.pfd = pifacedigitalio.PiFaceDigital() # creates a PiFace Digtal object
        
        self.timeout_value = timeout_value # sec.
        self.PositionUpdateTimerPeriode = timeout_value/10 # sec.
        self.MotorTimeout = mc_timer()
        self.PositionUpdate = mc_timer()
             
        self.states = []        
        self.states.append( mc_state_idle(self))
        self.states.append( State_Move_UP(self))
        self.states.append( State_Move_DOWN(self))
        self.states.append( State_UP(self))
        self.states.append( State_DOWN(self))
        
        self.current_state = mc_states.idle
        
        self.Start()    
        self.position=Position(0, 100, 10);    
        pass
        
    def EventHandler(self, event):
        
        print("event: {} position: {}".format(event, self.position.position))
        
        if(event == mc_event.Up):
            self.states[self.current_state].Up()
            
        elif(event == mc_event.Down):
            self.states[self.current_state].Down()
            
        elif(event == mc_event.Stop):
            self.states[self.current_state].Stop()
            
        elif(event == mc_event.Timeout):
            self.states[self.current_state].Timeout()
            
        elif(event == mc_event.UpdatePosition):
            self.states[self.current_state].UpdatePosition()

    
    def set_state(self, state):
        self.current_state = state
        self.states[self.current_state].entry()
    
    # actions
    def MotorUp(self):
        print('MotorUp')
        print('Switch Port={} on'.format(self.Port_Up))
        print('Switch Port={} off'.format(self.Port_Down))
        self.Port_Up.on()
        self.Port_Down.off()
                
    def MotorDown(self):
        print('MotorDown')
        print('Switch Port={} off'.format(self.Port_Up))
        print('Switch Port={} on'.format(self.Port_Down))
        self.Port_Up.off()
        self.Port_Down.on()
        
    def MotorStop(self): 
        print('MotorStop')
        print('Switch Port={} off'.format(self.Port_Up))
        print('Switch Port={} off'.format(self.Port_Down))
        self.Port_Up.off()
        self.Port_Down.off()
        
                        
    def run(self):
        print("leave StateMachine")
        while self.__running==True:
            #try:
                event = self.MailBox.get()#timeout=1)
                if(event != None):
                    self.EventHandler(event)
                
            #except:
            #    pass
        print("leave StateMachine")
        
    def Start(self):
        self.__running=True
        self.start()
        
    def Stop(self):
        self.__running=False
        self.MailBox.put(mc_event.Stop)
        self.join()

class State():

    # event handlers
    def Up(self):
        self.mc_sm.set_state(mc_states.move_up) 
        
    def Down(self):
        self.mc_sm.set_state(mc_states.move_down) 
    
    def Stop(self):
        self.mc_sm.set_state(mc_states.idle) 
    
    def Timeout(self):
        pass  
    
    def UpdatePosition(self):
        pass
    
    def entry(self):
        pass
    
##########################################################################    
class mc_state_idle(State):
    def __init__(self, mc_sm):
        self.mc_sm = mc_sm
    
    def entry(self):
        self.mc_sm.MotorStop()
        self.mc_sm.MotorTimeout.StopTimer()
        self.mc_sm.PositionUpdate.StopTimer() 
    
    def UpdatePosition(self):
        self.mc_sm.PositionUpdate.StopTimer()

##########################################################################        
class State_Move_UP(State):
    def __init__(self, mc_sm):
        self.mc_sm=mc_sm
        
    def entry(self):    
        self.mc_sm.MotorUp()
        self.mc_sm.MotorTimeout.StartTimer(self.mc_sm.timeout_value, self.mc_sm.MailBox, mc_event.Timeout)
        self.mc_sm.PositionUpdate.StartTimer(self.mc_sm.PositionUpdateTimerPeriode, self.mc_sm.MailBox, mc_event.UpdatePosition)
    
    def Timeout(self):
        self.mc_sm.set_state(mc_states.state_up) 

    def UpdatePosition(self):
        self.mc_sm.PositionUpdate.StartTimer(self.mc_sm.PositionUpdateTimerPeriode, self.mc_sm.MailBox, mc_event.UpdatePosition)
        self.mc_sm.position.increase()
            
##########################################################################            
class State_UP(State):
    def __init__(self, mc_sm):
        self.mc_sm=mc_sm
        
    def entry(self):
        self.mc_sm.MotorStop()
        #self.mc_sm.Position = 100
        self.mc_sm.position.SetPosition(self.mc_sm.position.max)
    
    def UpdatePosition(self):
        self.mc_sm.PositionUpdate.StopTimer()
    
##########################################################################    
class State_Move_DOWN(State):
    def __init__(self, mc_sm):
        self.mc_sm=mc_sm
        
    def entry(self):
        self.mc_sm.MotorDown()
        self.mc_sm.MotorTimeout.StartTimer(self.mc_sm.timeout_value, self.mc_sm.MailBox, mc_event.Timeout)
        self.mc_sm.PositionUpdate.StartTimer(self.mc_sm.PositionUpdateTimerPeriode, self.mc_sm.MailBox, mc_event.UpdatePosition)
    
    def Timeout(self):
        self.mc_sm.set_state(mc_states.state_down) 
    
    def UpdatePosition(self):
        self.mc_sm.PositionUpdate.StartTimer(self.mc_sm.PositionUpdateTimerPeriode, self.mc_sm.MailBox, mc_event.UpdatePosition)
        self.mc_sm.position.decrease()
 
##########################################################################        
class State_DOWN(State):
    def __init__(self, mc_sm):
        self.mc_sm=mc_sm
        
    def entry(self):
        self.mc_sm.MotorStop()
        #self.mc_sm.Position = 0 
        self.mc_sm.position.SetPosition(self.mc_sm.position.min) 
    
    def UpdatePosition(self):
        self.mc_sm.PositionUpdate.StopTimer()