def main(argv=None):
    comm = RepRapSerialComm(port=COMM_PORT, baudrate=COMM_BAUDRATE)

    print "Sleeping for 1 second for the serial port and firmware to settle..."
    time.sleep(1)

    print "Flushing communicaton channel..."
    comm.reset()

    print "Querying for Heater 1 temperature (Command 91)..."
    p = SimplePacket()
    p.add_8(0)
    p.add_8(91)
    comm.send(p)

    print "Reading back the response..."
    p = comm.readback()
    while p == None:
        p = comm.readback()

    print "Readback result code (1 for success, anything else - failure): " + str(
        p.rc)
    if p.rc == SimplePacket.RC_OK:
        print "The current temperature is: " + str(p.get_16(1))
    print str(p.get_8(0))
    print str(p.get_8(1))
    print str(p.get_8(2))
    print str(p.get_8(3))
def main(argv=None):
    comm = RepRapSerialComm(port = COMM_PORT, baudrate = COMM_BAUDRATE)

    print "Sleeping for 1 second for the serial port and firmware to settle..."
    time.sleep(1)

    print "Flushing communicaton channel..."
    comm.reset()
        
    print "Querying for Heater 1 temperature (Command 91)..."
    p = SimplePacket()
    p.add_8(0)
    p.add_8(91)
    comm.send(p)
    
    print "Reading back the response..."
    p = comm.readback()
    while p == None:
        p = comm.readback()
        
    print "Readback result code (1 for success, anything else - failure): " + str(p.rc)
    if p.rc == SimplePacket.RC_OK: print "The current temperature is: " + str(p.get_16(1))
    print str(p.get_8(0))
    print str(p.get_8(1))
    print str(p.get_8(2))
    print str(p.get_8(3))
Exemple #3
0
def sendPacket(p, CallBack):
    RetryCounter = 0
    sendFlag = True

    comm = RepRapSerialComm(port=COMM_PORT, baudrate=COMM_BAUDRATE)
    comm.reset()

    while RetryCounter < 10:
        # Send the Packet to slave.
        if sendFlag:
            comm.send(p)
            sendFlag = False

        # Read answer from slave.
        SlaveAnswer_Packet = comm.readback()
        if SlaveAnswer_Packet != None:
            if not (SlaveAnswer_Packet.rc != SimplePacket.RC_OK) and not (
                    SlaveAnswer_Packet.get_8(0) !=
                    1) and not (SlaveAnswer_Packet.id_received !=
                                SlaveAnswer_Packet.id()):
                # Execute the call back if there was no CRC mismatch on slave...
                (CallBack)(SlaveAnswer_Packet)
                return True

            if SlaveAnswer_Packet.rc != SimplePacket.RC_OK:
                print >> sys.stderr, datetime.now()
                print >> sys.stderr, "Slave -> Master communication error: RC: %d" % (
                    SlaveAnswer_Packet.rc)
                print >> sys.stderr, " "

            if SlaveAnswer_Packet.get_8(0) != 1:
                print >> sys.stderr, datetime.now()
                print >> sys.stderr, "Master -> Slave communication error: response_code: %d" % (
                    SlaveAnswer_Packet.get_8(0))
                print >> sys.stderr, " "

            if SlaveAnswer_Packet.id_received != SlaveAnswer_Packet.id():
                print >> sys.stderr, datetime.now()
                print >> sys.stderr, "Packet id error. id = %d; id_received = %d" % (
                    SlaveAnswer_Packet.id(), SlaveAnswer_Packet.id_received)
                print >> sys.stderr, " "

            sendFlag = True
            RetryCounter += 1

    # There was to much errors...
    # Shut down system
    comm.reset()
    return None
def sendPacket(p, CallBack):
    RetryCounter = 0
    sendFlag = True
    
    comm = RepRapSerialComm(port = COMM_PORT, baudrate = COMM_BAUDRATE)
    comm.reset()

    while RetryCounter < 10:
        # Send the Packet to slave.
        if sendFlag:
            comm.send(p)
            sendFlag = False

        # Read answer from slave.
        SlaveAnswer_Packet = comm.readback()
        if SlaveAnswer_Packet != None:
            if not(SlaveAnswer_Packet.rc != SimplePacket.RC_OK) and not(SlaveAnswer_Packet.get_8(0) != 1) and not(SlaveAnswer_Packet.id_received != SlaveAnswer_Packet.id()):
                # Execute the call back if there was no CRC mismatch on slave...
                (CallBack)(SlaveAnswer_Packet)
                return True

            if SlaveAnswer_Packet.rc != SimplePacket.RC_OK:
                print >> sys.stderr, datetime.now()
                print >> sys.stderr, "Slave -> Master communication error: RC: %d" %(SlaveAnswer_Packet.rc)
                print >> sys.stderr, " "

            if SlaveAnswer_Packet.get_8(0) != 1:
                print >> sys.stderr, datetime.now()
                print >> sys.stderr, "Master -> Slave communication error: response_code: %d" %(SlaveAnswer_Packet.get_8(0))
                print >> sys.stderr, " "

            if SlaveAnswer_Packet.id_received != SlaveAnswer_Packet.id():
                print >> sys.stderr, datetime.now()
                print >> sys.stderr, "Packet id error. id = %d; id_received = %d" % (SlaveAnswer_Packet.id(), SlaveAnswer_Packet.id_received)
                print >> sys.stderr, " "

            sendFlag = True
            RetryCounter += 1

    # There was to much errors...
    # Shut down system
    comm.reset()
    return None
Exemple #5
0
class Extruder:
    def __init__(self, hal_component):
        self.c = hal_component
        self._trigger_dict = {
            'heater1.set-sv': self._trigger_heater1_sv,
            'heater2.set-sv': self._trigger_heater2_sv,
            'motor1.rel-pos.trigger': self._trigger_motor1_rel_pos,
            'motor1.speed.trigger': self._trigger_motor1_speed,
            'motor1.spindle.on': self._trigger_motor1_spindle,
            'motor1.mmcube.trigger': self._trigger_motor1_mmcube,
            'motor1.pwm.r-fast': self._trigger_motor1_pwm,
            'motor1.pwm.r-slow': self._trigger_motor1_pwm,
            'motor1.pwm.f-slow': self._trigger_motor1_pwm,
            'motor1.pwm.f-fast': self._trigger_motor1_pwm,
            'motor1.tuning.trigger': self._trigger_motor1_tuning,
            'mapp.seqid': self._trigger_mapp,
            'running': self._trigger_running
        }
        self._trigger_state = {}

        self.comm = None
        self.readback_queue = []

        self.estop_state = 0
        self.enable_state = 0

        self.extruder_state = 0
        self.extruder_ready_check = 0
        self.mcode_heater1_sv = 0
        self.mcode_motor1_speed = 0

    def execute(self):
        """
        Start the main process loop.
        This will return only when error (Communication, Exception, etc) is encountered.
        """
        next_status_read = datetime.now()
        next_temp_read = datetime.now()
        next_motor_read = datetime.now()
        self.readback_queue = []
        self._init_trigger_state()

        self.comm = None
        try:
            self.comm = RepRapSerialComm(port=COMM_PORT,
                                         baudrate=COMM_BAUDRATE)
            self.comm.reset()
            p = self.comm.readback()

            while True:
                time.sleep(0.005)

                # Process any packets
                if len(self.readback_queue) > 0:
                    p = self.comm.readback()
                    if p != None:
                        if p.rc != SimplePacket.RC_OK:
                            print >> sys.stderr, "Extruder communication error: RC: %d" % (
                                p.rc)
                            self.c['fault.communication'] = 1
                            self.c['connection'] = 0
                            self.c['estop'] = 1
                            self.c['online'] = 0
                            self.extruder_ready_check = 0
                            self.extruder_state = 0
                            self.c['mapp.done'] = self.c['mapp.seqid']

                            self.comm.reset()
                            self.readback_queue = []

                            # Turn Off
                            p = SimplePacket()
                            p.add_8(0)
                            p.add_8(82)
                            self.comm.send(p)
                            self.readback_queue.append(self._rb_dummy)
                        else:
                            self.c['connection'] = 1
                            (self.readback_queue[0])(p)
                            del self.readback_queue[0]

                if len(self.readback_queue) > 20:
                    raise SystemExit(
                        "The readback queue is too long. Suggesting microcontroller overflow or other bus problem"
                    )

                # Enable
                if self.enable_state != self.c['enable']:
                    self.enable_state = self.c['enable']
                    p = SimplePacket()
                    p.add_8(0)
                    if self.enable_state:
                        p.add_8(81)
                    else:
                        self.extruder_ready_check = 0
                        self.extruder_state = 0
                        self.c['mapp.done'] = self.c['mapp.mcode']
                        p.add_8(82)
                    self.comm.send(p)
                    self.readback_queue.append(self._rb_enable)

                # Check button trigger
                self._check_trigger()

                # Read Status
                if datetime.now() > next_status_read:
                    next_status_read = datetime.now() + timedelta(
                        milliseconds=50)
                    p = SimplePacket()
                    p.add_8(0)
                    p.add_8(80)
                    self.comm.send(p)
                    self.readback_queue.append(self._rb_status)

                # Read Heater PV/SV
                if datetime.now() > next_temp_read:
                    next_temp_read = datetime.now() + timedelta(
                        milliseconds=250)
                    p = SimplePacket()
                    p.add_8(0)
                    p.add_8(91)
                    self.comm.send(p)
                    self.readback_queue.append(self._rb_heater1_pvsv)
                    p = SimplePacket()
                    p.add_8(0)
                    p.add_8(93)
                    self.comm.send(p)
                    self.readback_queue.append(self._rb_heater2_pvsv)

                # Read Motor PV/SV
                if datetime.now() > next_motor_read:
                    next_motor_read = datetime.now() + timedelta(
                        milliseconds=50)
                    p = SimplePacket()
                    p.add_8(0)
                    p.add_8(95)
                    self.comm.send(p)
                    self.readback_queue.append(self._rb_motor1_pvsv)

        except KeyboardInterrupt:
            if self.comm != None:
                p = SimplePacket()
                p.add_8(0)
                p.add_8(82)
                self.comm.send(p)
                self.comm.readback()
                raise SystemExit
        finally:
            if self.comm != None:
                self.comm.close()
                self.comm = None

    def _init_trigger_state(self):
        """
        Setup the trigger dictionary
        """
        for key in self._trigger_dict.keys():
            self._trigger_state[key] = 0

    def _check_trigger(self):
        """
        Look for any pin changes we are interested in and trigger the handlers
        """
        for key in self._trigger_dict.keys():
            if self._trigger_state[key] != self.c[key]:
                self._trigger_state[key] = self.c[key]
                self._trigger_dict[key](name=key,
                                        value=self._trigger_state[key])

    def __del__(self):
        if self.comm != None:
            self.comm.close()
            self.comm = None

    def _extruder_ready_poll(self):
        """
        Check if the temperature reached the set value, and signal the motor movement accordingly.
        """
        if self.extruder_ready_check > 0 and self.c[
                'heater1.pv'] >= self.mcode_heater1_sv - 5:
            if self.extruder_ready_check != 150:
                p = SimplePacket()
                p.add_8(0)
                p.add_8(97)
                if self.extruder_ready_check == 101:
                    p.add_16(self.mcode_motor1_speed)
                else:
                    p.add_16(-self.mcode_motor1_speed)
                self.comm.send(p)
                self.readback_queue.append(self._rb_dummy)

                self.extruder_state = self.extruder_ready_check
            self.c['mapp.done'] = self.c['mapp.seqid']
            self.extruder_ready_check = 0

    def _trigger_heater1_sv(self, name, value):
        p = SimplePacket()
        p.add_8(0)
        p.add_8(92)
        p.add_16(value)
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_heater2_sv(self, name, value):
        p = SimplePacket()
        p.add_8(0)
        p.add_8(94)
        p.add_16(value)
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_rel_pos(self, name, value):
        if not value:
            return
        p = SimplePacket()
        p.add_8(0)
        p.add_8(96)
        p.add_16(self.c['motor1.rel-pos'])
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_speed(self, name, value):
        if not value:
            return
        p = SimplePacket()
        p.add_8(0)
        p.add_8(97)
        p.add_16(self.c['motor1.speed'])
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_spindle(self, name, value):
        if not value:
            self.mcode_motor1_speed = 0
        else:
            self.mcode_motor1_speed = int(self.c['motor1.spindle'] *
                                          self.c['steps_per_mm_cube'] * 2**8)
        p = SimplePacket()
        p.add_8(0)
        p.add_8(97)
        p.add_16(self.mcode_motor1_speed)
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_mmcube(self, name, value):
        if not value:
            return
        p = SimplePacket()
        p.add_8(0)
        p.add_8(97)
        p.add_16(
            int(self.c['motor1.mmcube'] * self.c['steps_per_mm_cube'] * 2**8))
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_pwm(self, name, value):
        p = SimplePacket()
        p.add_8(0)
        p.add_8(98)
        p.add_8(name.find('f-') >= 0)
        if not value:
            p.add_8(0)
        else:
            if name.find('fast') >= 0:
                p.add_8(192)
            else:
                p.add_8(128)
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_tuning(self, name, value):
        if not value:
            return
        p = SimplePacket()
        p.add_8(0)
        p.add_8(100)
        if self.c['motor1.tuning.p'] > 0:
            p.add_16(int(2**abs(self.c['motor1.tuning.p'])))
            pass
        elif self.c['motor1.tuning.p'] < 0:
            p.add_16(-int(2**abs(self.c['motor1.tuning.p'])))
        else:
            p.add_16(0)

        if self.c['motor1.tuning.i'] > 0:
            p.add_16(int(2**abs(self.c['motor1.tuning.i'])))
        elif self.c['motor1.tuning.i'] < 0:
            p.add_16(-int(2**abs(self.c['motor1.tuning.i'])))
        else:
            p.add_16(0)

        if self.c['motor1.tuning.d'] > 0:
            p.add_16(int(2**abs(self.c['motor1.tuning.d'])))
        elif self.c['motor1.tuning.d'] < 0:
            p.add_16(-int(2**abs(self.c['motor1.tuning.d'])))
        else:
            p.add_16(0)

        if self.c['motor1.tuning.iLimit'] > 0:
            p.add_16(int(2**abs(self.c['motor1.tuning.iLimit'])))
        elif self.c['motor1.tuning.iLimit'] < 0:
            p.add_16(-int(2**abs(self.c['motor1.tuning.iLimit'])))
        else:
            p.add_16(0)

        p.add_8(self.c['motor1.tuning.deadband'])
        p.add_8(self.c['motor1.tuning.minOutput'])

        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _mapp_heater1_set_sv(self):
        p = SimplePacket()
        p.add_8(0)
        p.add_8(92)
        p.add_16(self.mcode_heater1_sv)
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_mapp(self, name, value):
        seqid = value
        mcode = self.c['mapp.mcode']
        if mcode == 101:
            # Extruder Heatup + Forward
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()
        elif mcode == 102:
            # Extruder Heatup + Reverse
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()
        elif mcode == 103:
            # Extruder Heatup + Motor Off
            self._mapp_heater1_set_sv()
            p = SimplePacket()
            p.add_8(0)
            p.add_8(97)
            p.add_8(0)
            p.add_8(0)
            self.comm.send(p)
            self.readback_queue.append(self._rb_dummy)
            self.extruder_state = 0

            self.c['mapp.done'] = seqid
        elif mcode == 104:
            # Set extruder temp
            self.mcode_heater1_sv = int(self.c['mapp.p'])
            self._mapp_heater1_set_sv()
            self.c['mapp.done'] = seqid

        # 105: Get temperature: Do nothing
        # 106: TODO FAN ON
        # 107: TODO FAN OFF

        elif mcode == 108:
            # Set future extruder speed
            # Won't take effect until next M101/M102
            self.mcode_motor1_speed = int(self.c['mapp.p'] *
                                          self.c['steps_per_mm_cube'] * 2**8)
            self.c['mapp.done'] = seqid

        elif mcode == 150:
            # Wait for temperature to reach the set value
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()

        else:
            # Release all unknown MCode
            self.c['mapp.done'] = seqid

        # self.readback_queue.append(lambda p: _rb_mcode(seqid))

    def _trigger_running(self, name, value):
        if not value:
            p = SimplePacket()
            p.add_8(0)
            p.add_8(
                98
            )  # Use PWM instead of SPEED. PWM=0 frees the motor1. SPEED=0 keeps motor locked at position
            p.add_8(0)
            p.add_8(0)
            self.comm.send(p)
            self.readback_queue.append(self._rb_dummy)
        elif self.extruder_state and self.mcode_motor1_speed != 0:
            self._mapp_heater1_set_sv()
            p = SimplePacket()
            p.add_8(0)
            p.add_8(97)
            if self.extruder_state == 101:
                p.add_16(self.mcode_motor1_speed)
            else:
                p.add_16(-self.mcode_motor1_speed)
            self.comm.send(p)
            self.readback_queue.append(self._rb_dummy)

    def _rb_mcode(self, p, seqid):
        # Not used yet
        self.c['mapp.done'] = seqid

    def _rb_dummy(self, p):
        pass

    def _rb_status(self, p):
        new_estop_state = p.get_8(1) & 1
        if new_estop_state and not self.estop_state:
            self.c['estop'] = 1
        else:
            self.c['estop'] = 0
        self.estop_state = new_estop_state

        self.c['online'] = p.get_8(1) & 2
        self.c['fault.thermistor-disc'] = p.get_8(2) != 0
        self.c['fault.heater-response'] = p.get_8(3) != 0
        self.c['fault.motor-jammed'] = p.get_8(4) != 0
        self.c['fault.no-plastic'] = p.get_8(5) != 0

        self.c['heater1.on'] = (p.get_8(6) & 1) != 0
        self.c['heater2.on'] = (p.get_8(6) & 2) != 0

    def _rb_enable(self, p):
        self.c['fault.communication'] = 0
        self.estop_state = 0

    def _rb_heater1_pvsv(self, p):
        self.c['heater1.pv'] = p.get_16(1)
        self.c['heater1.sv'] = p.get_16(3)
        self._extruder_ready_poll()

    def _rb_heater2_pvsv(self, p):
        self.c['heater2.pv'] = p.get_16(1)
        self.c['heater2.sv'] = p.get_16(3)
        self._extruder_ready_poll()

    def _rb_motor1_pvsv(self, p):
        self.c['motor1.pv'] = p.get_16(1)
        self.c['motor1.sv'] = p.get_16(3)
Exemple #6
0
class Extruder:
    def __init__(self, hal_component):
        self.c = hal_component
        self._trigger_dict = {
            'heater1.set-sv': self._trigger_heater1_sv,
            'motor1.speed.trigger': self._trigger_motor1_speed,
            'motor1.spindle.on': self._trigger_motor1_spindle,
            'mapp.seqid': self._trigger_mapp
        }
        self._trigger_state = {}

        self.comm = None

        self.estop_state = 0
        self.enable_state = 0

        self.extruder_state = 0
        self.extruder_ready_check = 0
        self.mcode_heater1_sv = 0
        self.mcode_motor1_speed = 0

    def execute(self):
        """
        Start the main process loop.
        This will return only when error (Communication, Exception, etc) is encountered.
        """
        next_status_read = datetime.now()
        next_temp_read = datetime.now()
        next_motor_read = datetime.now()
        self._init_trigger_state()

        self.comm = None
        try:
            self.comm = RepRapSerialComm(port=COMM_PORT,
                                         baudrate=COMM_BAUDRATE)
            self.comm.reset()
            p = self.comm.readback()

            while True:
                time.sleep(0.005)

                # Enable the system if it should be enable
                if self.enable_state != self.c['enable']:
                    self.enable_state = self.c['enable']
                    p = SimplePacket()
                    p.add_8(SLAVE_ADDRESS)
                    if self.enable_state:
                        p.add_8(SLAVE_CMD_TURN_ON)
                    else:
                        self.extruder_ready_check = 0
                        self.extruder_state = 0
                        self.c['mapp.done'] = self.c['mapp.mcode']
                        p.add_8(SLAVE_CMD_TURN_OFF)
                    self.sendPacket(p, self._rb_enable)

                # Check button trigger
                self._check_trigger()

                # Read Status
                if datetime.now() > next_status_read:
                    next_status_read = datetime.now() + timedelta(
                        milliseconds=50)
                    p = SimplePacket()
                    p.add_8(SLAVE_ADDRESS)
                    p.add_8(SLAVE_CMD_STATUS)
                    self.sendPacket(p, self._rb_status)

                # Read Heater PV/SV
                if datetime.now() > next_temp_read:
                    next_temp_read = datetime.now() + timedelta(
                        milliseconds=50)
                    p = SimplePacket()
                    p.add_8(SLAVE_ADDRESS)
                    p.add_8(SLAVE_CMD_GET_HEATER1_PVSV)
                    self.sendPacket(p, self._rb_heater1_pvsv)

                # Read Motor PV/SV
                if datetime.now() > next_motor_read:
                    next_motor_read = datetime.now() + timedelta(
                        milliseconds=50)
                    p = SimplePacket()
                    p.add_8(SLAVE_ADDRESS)
                    p.add_8(SLAVE_CMD_GET_MOTOR1_PVSV)
                    self.sendPacket(p, self._rb_motor1_pvsv)

        except KeyboardInterrupt:
            # Disable system if we hit keyboard interrupt
            if self.comm != None:
                p = SimplePacket()
                p.add_8(SLAVE_ADDRESS)
                p.add_8(SLAVE_CMD_TURN_OFF)
                self.sendPacket(p, self._rb_dummy)
                raise SystemExit
        finally:
            if self.comm != None:
                self.comm.close()
                self.comm = None

    def _init_trigger_state(self):
        """
        Setup the trigger dictionary
        """
        for key in self._trigger_dict.keys():
            self._trigger_state[key] = 0

    def _check_trigger(self):
        """
        Look for any pin changes we are interested in and trigger the handlers
        """
        for key in self._trigger_dict.keys():
            if self._trigger_state[key] != self.c[key]:
                self._trigger_state[key] = self.c[key]
                self._trigger_dict[key](name=key,
                                        value=self._trigger_state[key])

    def __del__(self):
        if self.comm != None:
            self.comm.close()
            self.comm = None

    def _extruder_ready_poll(self):
        """
        Check if the temperature reached the set value, and signal the motor movement accordingly.
        """
        if self.extruder_ready_check > 0 and self.c[
                'heater1.pv'] >= self.mcode_heater1_sv - 5:
            if self.extruder_ready_check != 150:
                p = SimplePacket()
                p.add_8(SLAVE_ADDRESS)
                p.add_8(SLAVE_CMD_SET_MOTOR1_SPEED)
                if self.extruder_ready_check == 101:
                    if self.mcode_motor1_speed == 0:
                        self.mcode_motor1_speed = 1
                    elif self.mcode_motor1_speed < 0:
                        self.mcode_motor1_speed = -self.mcode_motor1_speed
                    p.add_16(self.mcode_motor1_speed)
                else:
                    p.add_16(self.mcode_motor1_speed)
                self.sendPacket(p, self._rb_dummy)

                self.extruder_state = self.extruder_ready_check
            self.c['mapp.done'] = self.c['mapp.seqid']
            self.extruder_ready_check = 0

    def _trigger_heater1_sv(self, name, value):
        p = SimplePacket()
        p.add_8(SLAVE_ADDRESS)
        p.add_8(SLAVE_CMD_SET_HEATER1_SV)
        p.add_16(value)
        self.sendPacket(p, self._rb_dummy)

    def _trigger_motor1_speed(self, name, value):
        if not value:
            return
        p = SimplePacket()
        p.add_8(SLAVE_ADDRESS)
        p.add_8(SLAVE_CMD_SET_MOTOR1_SPEED)
        p.add_16(self.c['motor1.speed'])
        self.sendPacket(p, self._rb_dummy)

    def _trigger_motor1_spindle(self, name, value):
        if not value:
            self.mcode_motor1_speed = 0
        else:
            #self.mcode_motor1_speed = int(self.c['motor1.spindle'] * self.c['steps_per_mm_cube'] * 10)
            self.mcode_motor1_speed = int(
                self.c['motor1.spindle'] * 10000
            )  # self.mcode_motor1_speed = 10000 equals to 1 rotation per second.

        p = SimplePacket()
        p.add_8(SLAVE_ADDRESS)
        p.add_8(SLAVE_CMD_SET_MOTOR1_SPEED)
        p.add_16(self.mcode_motor1_speed)
        self.sendPacket(p, self._rb_dummy)

    def _mapp_heater1_set_sv(self):
        p = SimplePacket()
        p.add_8(SLAVE_ADDRESS)
        p.add_8(SLAVE_CMD_SET_HEATER1_SV)
        p.add_16(self.mcode_heater1_sv)
        self.sendPacket(p, self._rb_dummy)

    def _trigger_mapp(self, name, value):
        seqid = value
        mcode = self.c['mapp.mcode']
        if mcode == 101:
            # Extruder Heatup + Forward
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()
        elif mcode == 102:
            # Extruder Heatup + Reverse
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()
        elif mcode == 103:
            # Extruder Heatup + Motor Off
            self._mapp_heater1_set_sv()
            p = SimplePacket()
            p.add_8(SLAVE_ADDRESS)
            p.add_8(SLAVE_CMD_SET_MOTOR1_SPEED)
            p.add_8(0)
            p.add_8(0)
            self.sendPacket(p, self._rb_dummy)
            self.extruder_state = 0

            self.c['mapp.done'] = seqid
        elif mcode == 104:
            # Set extruder temp
            self.mcode_heater1_sv = int(self.c['mapp.p'])
            self._mapp_heater1_set_sv()
            self.c['mapp.done'] = seqid

        elif mcode == 108:
            # Set future extruder speed
            # Won't take effect until next M101/M102
            #self.mcode_motor1_speed = int(self.c['mapp.p'] * self.c['steps_per_mm_cube'] * 10);
            self.mcode_motor1_speed = int(self.c['mapp.p'] * 10000)
            # self.mcode_motor1_speed = 10000 equals to 1 rotation per second.
            self.c['mapp.done'] = seqid

        elif mcode == 150:
            # Wait for temperature to reach the set value
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()

        else:
            # Release all unknown MCode
            self.c['mapp.done'] = seqid

    def _rb_mcode(self, p, seqid):
        # Not used yet
        self.c['mapp.done'] = seqid

    def _rb_dummy(self, p):
        pass

    def _rb_status(self, p):
        new_estop_state = p.get_8(1) & 1
        if new_estop_state and not self.estop_state:
            self.c['estop'] = 1
        else:
            self.c['estop'] = 0
        self.estop_state = new_estop_state
        self.c['online'] = p.get_8(1) & 2
        self.c['heater1.on'] = (p.get_8(6) & 1) != 0

    def _rb_enable(self, p):
        self.c['fault.communication'] = 0
        self.estop_state = 0

    def _rb_heater1_pvsv(self, p):
        self.c['heater1.pv'] = p.get_16(1)
        self.c['heater1.sv'] = p.get_16(3)
        self._extruder_ready_poll()

    def _rb_motor1_pvsv(self, p):
        self.c['motor1.pv'] = p.get_16(1)
        self.c['motor1.sv'] = p.get_16(3)

    def sendPacket(self, p, CallBack):
        RetryCounter = 0
        sendFlag = True
        SlaveAnswer = None

        while RetryCounter < 10:
            # Send the Packet to slave.
            if sendFlag:
                self.comm.send(p)
                sendFlag = False

            # Read answer from slave.
            SlaveAnswer = self.comm.readback()
            if SlaveAnswer != None:
                if SlaveAnswer.rc != SimplePacket.RC_OK:
                    # There was a CRC mismatch on slave and we send again the command, since
                    # slave do not execute the command if found a CRC mismatch.
                    sendFlag = True
                    RetryCounter = RetryCounter + 1
                    #DEBUG
                    print >> sys.stderr, datetime.now()
                    print >> sys.stderr, "Extruder communication error: RC: %d" % (
                        SlaveAnswer.rc)
                    print >> sys.stderr, " "
                    #end DEBUG

                else:
                    # Execute the call back if there was no CRC mismatch on slave...
                    self.c['connection'] = 1  # Turn on connection.
                    (CallBack)(SlaveAnswer)
                    return True

        # There was to much CRC errors and/or timeouts...
        # Shut down system
        self.c['fault.communication'] = 1
        self.c['connection'] = 0
        self.c['estop'] = 1
        self.c['online'] = 0
        self.extruder_ready_check = 0
        self.extruder_state = 0
        self.c['mapp.done'] = self.c['mapp.seqid']

        self.comm.reset()

        #DEBUG
        print >> sys.stderr, datetime.now()
        print >> sys.stderr, "To much extruder communication errors... do EMC2 estop"
        print >> sys.stderr, " "
        #end DEBUG

        # Turn Off
        #p = SimplePacket()
        #p.add_8(0)
        #p.add_8(82)
        #self.sendPacket(p, self._rb_dummy)
        return None
class Extruder:
    def __init__(self, hal_component):
        self.c = hal_component
        self._trigger_dict = {
            "heater1.set-sv": self._trigger_heater1_sv,
            "heater2.set-sv": self._trigger_heater2_sv,
            "motor1.rel-pos.trigger": self._trigger_motor1_rel_pos,
            "motor1.speed.trigger": self._trigger_motor1_speed,
            "motor1.spindle.on": self._trigger_motor1_spindle,
            "motor1.mmcube.trigger": self._trigger_motor1_mmcube,
            "motor1.pwm.r-fast": self._trigger_motor1_pwm,
            "motor1.pwm.r-slow": self._trigger_motor1_pwm,
            "motor1.pwm.f-slow": self._trigger_motor1_pwm,
            "motor1.pwm.f-fast": self._trigger_motor1_pwm,
            "motor1.tuning.trigger": self._trigger_motor1_tuning,
            "mapp.seqid": self._trigger_mapp,
            "running": self._trigger_running,
        }
        self._trigger_state = {}

        self.comm = None
        self.readback_queue = []

        self.estop_state = 0
        self.enable_state = 0

        self.extruder_state = 0
        self.extruder_ready_check = 0
        self.mcode_heater1_sv = 0
        self.mcode_motor1_speed = 0

    def execute(self):
        """
        Start the main process loop.
        This will return only when error (Communication, Exception, etc) is encountered.
        """
        next_status_read = datetime.now()
        next_temp_read = datetime.now()
        next_motor_read = datetime.now()
        self.readback_queue = []
        self._init_trigger_state()

        self.comm = None
        try:
            self.comm = RepRapSerialComm(port=COMM_PORT, baudrate=COMM_BAUDRATE)
            self.comm.reset()
            p = self.comm.readback()

            while True:
                time.sleep(0.005)

                # Process any packets
                if len(self.readback_queue) > 0:
                    p = self.comm.readback()
                    if p != None:
                        if p.rc != SimplePacket.RC_OK:
                            print >>sys.stderr, "Extruder communication error: RC: %d" % (p.rc)
                            self.c["fault.communication"] = 1
                            self.c["connection"] = 0
                            self.c["estop"] = 1
                            self.c["online"] = 0
                            self.extruder_ready_check = 0
                            self.extruder_state = 0
                            self.c["mapp.done"] = self.c["mapp.seqid"]

                            self.comm.reset()
                            self.readback_queue = []

                            # Turn Off
                            p = SimplePacket()
                            p.add_8(0)
                            p.add_8(82)
                            self.comm.send(p)
                            self.readback_queue.append(self._rb_dummy)
                        else:
                            self.c["connection"] = 1
                            (self.readback_queue[0])(p)
                            del self.readback_queue[0]

                if len(self.readback_queue) > 20:
                    raise SystemExit(
                        "The readback queue is too long. Suggesting microcontroller overflow or other bus problem"
                    )

                # Enable
                if self.enable_state != self.c["enable"]:
                    self.enable_state = self.c["enable"]
                    p = SimplePacket()
                    p.add_8(0)
                    if self.enable_state:
                        p.add_8(81)
                    else:
                        self.extruder_ready_check = 0
                        self.extruder_state = 0
                        self.c["mapp.done"] = self.c["mapp.mcode"]
                        p.add_8(82)
                    self.comm.send(p)
                    self.readback_queue.append(self._rb_enable)

                # Check button trigger
                self._check_trigger()

                # Read Status
                if datetime.now() > next_status_read:
                    next_status_read = datetime.now() + timedelta(milliseconds=50)
                    p = SimplePacket()
                    p.add_8(0)
                    p.add_8(80)
                    self.comm.send(p)
                    self.readback_queue.append(self._rb_status)

                # Read Heater PV/SV
                if datetime.now() > next_temp_read:
                    next_temp_read = datetime.now() + timedelta(milliseconds=250)
                    p = SimplePacket()
                    p.add_8(0)
                    p.add_8(91)
                    self.comm.send(p)
                    self.readback_queue.append(self._rb_heater1_pvsv)
                    p = SimplePacket()
                    p.add_8(0)
                    p.add_8(93)
                    self.comm.send(p)
                    self.readback_queue.append(self._rb_heater2_pvsv)

                # Read Motor PV/SV
                if datetime.now() > next_motor_read:
                    next_motor_read = datetime.now() + timedelta(milliseconds=50)
                    p = SimplePacket()
                    p.add_8(0)
                    p.add_8(95)
                    self.comm.send(p)
                    self.readback_queue.append(self._rb_motor1_pvsv)

        except KeyboardInterrupt:
            if self.comm != None:
                p = SimplePacket()
                p.add_8(0)
                p.add_8(82)
                self.comm.send(p)
                self.comm.readback()
                raise SystemExit
        finally:
            if self.comm != None:
                self.comm.close()
                self.comm = None

    def _init_trigger_state(self):
        """
        Setup the trigger dictionary
        """
        for key in self._trigger_dict.keys():
            self._trigger_state[key] = 0

    def _check_trigger(self):
        """
        Look for any pin changes we are interested in and trigger the handlers
        """
        for key in self._trigger_dict.keys():
            if self._trigger_state[key] != self.c[key]:
                self._trigger_state[key] = self.c[key]
                self._trigger_dict[key](name=key, value=self._trigger_state[key])

    def __del__(self):
        if self.comm != None:
            self.comm.close()
            self.comm = None

    def _extruder_ready_poll(self):
        """
        Check if the temperature reached the set value, and signal the motor movement accordingly.
        """
        if self.extruder_ready_check > 0 and self.c["heater1.pv"] >= self.mcode_heater1_sv - 5:
            if self.extruder_ready_check != 150:
                p = SimplePacket()
                p.add_8(0)
                p.add_8(97)
                if self.extruder_ready_check == 101:
                    p.add_16(self.mcode_motor1_speed)
                else:
                    p.add_16(-self.mcode_motor1_speed)
                self.comm.send(p)
                self.readback_queue.append(self._rb_dummy)

                self.extruder_state = self.extruder_ready_check
            self.c["mapp.done"] = self.c["mapp.seqid"]
            self.extruder_ready_check = 0

    def _trigger_heater1_sv(self, name, value):
        p = SimplePacket()
        p.add_8(0)
        p.add_8(92)
        p.add_16(value)
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_heater2_sv(self, name, value):
        p = SimplePacket()
        p.add_8(0)
        p.add_8(94)
        p.add_16(value)
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_rel_pos(self, name, value):
        if not value:
            return
        p = SimplePacket()
        p.add_8(0)
        p.add_8(96)
        p.add_16(self.c["motor1.rel-pos"])
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_speed(self, name, value):
        if not value:
            return
        p = SimplePacket()
        p.add_8(0)
        p.add_8(97)
        p.add_16(self.c["motor1.speed"])
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_spindle(self, name, value):
        if not value:
            self.mcode_motor1_speed = 0
        else:
            self.mcode_motor1_speed = int(self.c["motor1.spindle"] * self.c["steps_per_mm_cube"] * 2 ** 8)
        p = SimplePacket()
        p.add_8(0)
        p.add_8(97)
        p.add_16(self.mcode_motor1_speed)
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_mmcube(self, name, value):
        if not value:
            return
        p = SimplePacket()
        p.add_8(0)
        p.add_8(97)
        p.add_16(int(self.c["motor1.mmcube"] * self.c["steps_per_mm_cube"] * 2 ** 8))
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_pwm(self, name, value):
        p = SimplePacket()
        p.add_8(0)
        p.add_8(98)
        p.add_8(name.find("f-") >= 0)
        if not value:
            p.add_8(0)
        else:
            if name.find("fast") >= 0:
                p.add_8(192)
            else:
                p.add_8(128)
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_motor1_tuning(self, name, value):
        if not value:
            return
        p = SimplePacket()
        p.add_8(0)
        p.add_8(100)
        if self.c["motor1.tuning.p"] > 0:
            p.add_16(int(2 ** abs(self.c["motor1.tuning.p"])))
            pass
        elif self.c["motor1.tuning.p"] < 0:
            p.add_16(-int(2 ** abs(self.c["motor1.tuning.p"])))
        else:
            p.add_16(0)

        if self.c["motor1.tuning.i"] > 0:
            p.add_16(int(2 ** abs(self.c["motor1.tuning.i"])))
        elif self.c["motor1.tuning.i"] < 0:
            p.add_16(-int(2 ** abs(self.c["motor1.tuning.i"])))
        else:
            p.add_16(0)

        if self.c["motor1.tuning.d"] > 0:
            p.add_16(int(2 ** abs(self.c["motor1.tuning.d"])))
        elif self.c["motor1.tuning.d"] < 0:
            p.add_16(-int(2 ** abs(self.c["motor1.tuning.d"])))
        else:
            p.add_16(0)

        if self.c["motor1.tuning.iLimit"] > 0:
            p.add_16(int(2 ** abs(self.c["motor1.tuning.iLimit"])))
        elif self.c["motor1.tuning.iLimit"] < 0:
            p.add_16(-int(2 ** abs(self.c["motor1.tuning.iLimit"])))
        else:
            p.add_16(0)

        p.add_8(self.c["motor1.tuning.deadband"])
        p.add_8(self.c["motor1.tuning.minOutput"])

        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _mapp_heater1_set_sv(self):
        p = SimplePacket()
        p.add_8(0)
        p.add_8(92)
        p.add_16(self.mcode_heater1_sv)
        self.comm.send(p)
        self.readback_queue.append(self._rb_dummy)

    def _trigger_mapp(self, name, value):
        seqid = value
        mcode = self.c["mapp.mcode"]
        if mcode == 101:
            # Extruder Heatup + Forward
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()
        elif mcode == 102:
            # Extruder Heatup + Reverse
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()
        elif mcode == 103:
            # Extruder Heatup + Motor Off
            self._mapp_heater1_set_sv()
            p = SimplePacket()
            p.add_8(0)
            p.add_8(97)
            p.add_8(0)
            p.add_8(0)
            self.comm.send(p)
            self.readback_queue.append(self._rb_dummy)
            self.extruder_state = 0

            self.c["mapp.done"] = seqid
        elif mcode == 104:
            # Set extruder temp
            self.mcode_heater1_sv = int(self.c["mapp.p"])
            self._mapp_heater1_set_sv()
            self.c["mapp.done"] = seqid

        # 105: Get temperature: Do nothing
        # 106: TODO FAN ON
        # 107: TODO FAN OFF

        elif mcode == 108:
            # Set future extruder speed
            # Won't take effect until next M101/M102
            self.mcode_motor1_speed = int(self.c["mapp.p"] * self.c["steps_per_mm_cube"] * 2 ** 8)
            self.c["mapp.done"] = seqid

        elif mcode == 150:
            # Wait for temperature to reach the set value
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()

        else:
            # Release all unknown MCode
            self.c["mapp.done"] = seqid

        # self.readback_queue.append(lambda p: _rb_mcode(seqid))

    def _trigger_running(self, name, value):
        if not value:
            p = SimplePacket()
            p.add_8(0)
            p.add_8(98)  # Use PWM instead of SPEED. PWM=0 frees the motor1. SPEED=0 keeps motor locked at position
            p.add_8(0)
            p.add_8(0)
            self.comm.send(p)
            self.readback_queue.append(self._rb_dummy)
        elif self.extruder_state and self.mcode_motor1_speed != 0:
            self._mapp_heater1_set_sv()
            p = SimplePacket()
            p.add_8(0)
            p.add_8(97)
            if self.extruder_state == 101:
                p.add_16(self.mcode_motor1_speed)
            else:
                p.add_16(-self.mcode_motor1_speed)
            self.comm.send(p)
            self.readback_queue.append(self._rb_dummy)

    def _rb_mcode(self, p, seqid):
        # Not used yet
        self.c["mapp.done"] = seqid

    def _rb_dummy(self, p):
        pass

    def _rb_status(self, p):
        new_estop_state = p.get_8(1) & 1
        if new_estop_state and not self.estop_state:
            self.c["estop"] = 1
        else:
            self.c["estop"] = 0
        self.estop_state = new_estop_state

        self.c["online"] = p.get_8(1) & 2
        self.c["fault.thermistor-disc"] = p.get_8(2) != 0
        self.c["fault.heater-response"] = p.get_8(3) != 0
        self.c["fault.motor-jammed"] = p.get_8(4) != 0
        self.c["fault.no-plastic"] = p.get_8(5) != 0

        self.c["heater1.on"] = (p.get_8(6) & 1) != 0
        self.c["heater2.on"] = (p.get_8(6) & 2) != 0

    def _rb_enable(self, p):
        self.c["fault.communication"] = 0
        self.estop_state = 0

    def _rb_heater1_pvsv(self, p):
        self.c["heater1.pv"] = p.get_16(1)
        self.c["heater1.sv"] = p.get_16(3)
        self._extruder_ready_poll()

    def _rb_heater2_pvsv(self, p):
        self.c["heater2.pv"] = p.get_16(1)
        self.c["heater2.sv"] = p.get_16(3)
        self._extruder_ready_poll()

    def _rb_motor1_pvsv(self, p):
        self.c["motor1.pv"] = p.get_16(1)
        self.c["motor1.sv"] = p.get_16(3)
class Extruder:
    def __init__(self, hal_component):
        self.c = hal_component            
        self._trigger_dict = {
            'heater1.set-sv': self._trigger_heater1_sv,
            'motor1.speed.trigger': self._trigger_motor1_speed,
            'motor1.spindle.on': self._trigger_motor1_spindle,
            'mapp.seqid': self._trigger_mapp
        }
        self._trigger_state = {}
        
        self.comm = None

        self.estop_state = 0
        self.enable_state = 0
        
        self.extruder_state = 0;
        self.extruder_ready_check = 0;
        self.mcode_heater1_sv = 0;
        self.mcode_motor1_speed = 0;
    
    def execute(self):    
        """
        Start the main process loop.
        This will return only when error (Communication, Exception, etc) is encountered.
        """
        next_status_read = datetime.now();
        next_temp_read = datetime.now();
        next_motor_read = datetime.now();
        self._init_trigger_state()
        
        self.comm = None
        try:            
            self.comm = RepRapSerialComm(port = COMM_PORT, baudrate = COMM_BAUDRATE)
            self.comm.reset()            
            p = self.comm.readback()

            while True:
                time.sleep(0.005)

                # Enable the system if it should be enable
                if self.enable_state != self.c['enable']:
                    self.enable_state = self.c['enable']
                    p = SimplePacket()
                    p.add_8(SLAVE_ADDRESS)
                    if self.enable_state:
                        p.add_8(SLAVE_CMD_TURN_ON)
                    else:                    
                        self.extruder_ready_check = 0
                        self.extruder_state = 0
                        self.c['mapp.done'] = self.c['mapp.mcode']
                        p.add_8(SLAVE_CMD_TURN_OFF)
                    self.sendPacket(p, self._rb_enable)

                # Check button trigger
                self._check_trigger()
                
                # Read Status
                if datetime.now() > next_status_read:
                    next_status_read = datetime.now() + timedelta(milliseconds = 50)
                    p = SimplePacket()
                    p.add_8(SLAVE_ADDRESS)
                    p.add_8(SLAVE_CMD_STATUS)
                    self.sendPacket(p, self._rb_status)
                    
                # Read Heater PV/SV
                if datetime.now() > next_temp_read:
                    next_temp_read = datetime.now() + timedelta(milliseconds = 50)
                    p = SimplePacket()
                    p.add_8(SLAVE_ADDRESS)
                    p.add_8(SLAVE_CMD_GET_HEATER1_PVSV)
                    self.sendPacket(p, self._rb_heater1_pvsv)
                    
                # Read Motor PV/SV
                if datetime.now() > next_motor_read:
                    next_motor_read = datetime.now() + timedelta(milliseconds = 50)
                    p = SimplePacket()
                    p.add_8(SLAVE_ADDRESS)
                    p.add_8(SLAVE_CMD_GET_MOTOR1_PVSV)
                    self.sendPacket(p, self._rb_motor1_pvsv)

        except KeyboardInterrupt:
		# Disable system if we hit keyboard interrupt		    
            if self.comm != None:
                p = SimplePacket()
                p.add_8(SLAVE_ADDRESS)
                p.add_8(SLAVE_CMD_TURN_OFF)
                self.sendPacket(p, self._rb_dummy)
                raise SystemExit
        finally:
            if self.comm != None:
                self.comm.close()
                self.comm = None

    def _init_trigger_state(self):
        """
        Setup the trigger dictionary
        """
        for key in self._trigger_dict.keys():
            self._trigger_state[key] = 0    
    
    def _check_trigger(self):
        """
        Look for any pin changes we are interested in and trigger the handlers
        """
        for key in self._trigger_dict.keys():       
            if self._trigger_state[key] != self.c[key]:
                self._trigger_state[key] = self.c[key]
                self._trigger_dict[key](name = key, value = self._trigger_state[key])
            
    def __del__(self):
        if self.comm != None:
            self.comm.close()
            self.comm = None
            
    def _extruder_ready_poll(self):
        """
        Check if the temperature reached the set value, and signal the motor movement accordingly.
        """
        if self.extruder_ready_check > 0 and self.c['heater1.pv'] >= self.mcode_heater1_sv - 5:
                if self.extruder_ready_check != 150:
	                p = SimplePacket()
	                p.add_8(SLAVE_ADDRESS)
	                p.add_8(SLAVE_CMD_SET_MOTOR1_SPEED)
	                if self.extruder_ready_check == 101:
	                    if self.mcode_motor1_speed == 0:
	                        self.mcode_motor1_speed = 1
	                    elif self.mcode_motor1_speed < 0:
	                        self.mcode_motor1_speed = -self.mcode_motor1_speed
	                    p.add_16(self.mcode_motor1_speed)
	                else:
	                    p.add_16(self.mcode_motor1_speed)
	                self.sendPacket(p, self._rb_dummy)

	                self.extruder_state = self.extruder_ready_check           
                self.c['mapp.done'] = self.c['mapp.seqid']
                self.extruder_ready_check = 0
                             
    def _trigger_heater1_sv(self, name, value):
        p = SimplePacket()
        p.add_8(SLAVE_ADDRESS)
        p.add_8(SLAVE_CMD_SET_HEATER1_SV)
        p.add_16(value)
        self.sendPacket(p, self._rb_dummy)
        
    def _trigger_motor1_speed(self, name, value):
        if not value:
            return
        p = SimplePacket()
        p.add_8(SLAVE_ADDRESS)
        p.add_8(SLAVE_CMD_SET_MOTOR1_SPEED)
        p.add_16(self.c['motor1.speed'])
        self.sendPacket(p, self._rb_dummy)
        
    def _trigger_motor1_spindle(self, name, value):
        if not value:
            self.mcode_motor1_speed = 0
        else:
            #self.mcode_motor1_speed = int(self.c['motor1.spindle'] * self.c['steps_per_mm_cube'] * 10)
            self.mcode_motor1_speed = int(self.c['motor1.spindle']) # self.mcode_motor1_speed = 10000 equals to 1 rotation per second.
        
        p = SimplePacket()
        p.add_8(SLAVE_ADDRESS)
        p.add_8(SLAVE_CMD_SET_MOTOR1_SPEED)
        p.add_16(self.mcode_motor1_speed)
        self.sendPacket(p, self._rb_dummy)
        
    def _mapp_heater1_set_sv(self):
        p = SimplePacket()
        p.add_8(SLAVE_ADDRESS)
        p.add_8(SLAVE_CMD_SET_HEATER1_SV)
        p.add_16(self.mcode_heater1_sv)
        self.sendPacket(p, self._rb_dummy)

    def _trigger_mapp(self, name, value):
        seqid = value
        mcode = self.c['mapp.mcode']
        if mcode == 101:
            # Extruder Heatup + Forward            
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()            
        elif mcode == 102:
            # Extruder Heatup + Reverse
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()
        elif mcode == 103:
            # Extruder Heatup + Motor Off
            self._mapp_heater1_set_sv()
            p = SimplePacket()
            p.add_8(SLAVE_ADDRESS)
            p.add_8(SLAVE_CMD_SET_MOTOR1_SPEED)
            p.add_8(0)
            p.add_8(0)
            self.sendPacket(p, self._rb_dummy)
            self.extruder_state = 0
            
            self.c['mapp.done'] = seqid    
        elif mcode == 104:
            # Set extruder temp
            self.mcode_heater1_sv = int(self.c['mapp.p'])
            self._mapp_heater1_set_sv()
            self.c['mapp.done'] = seqid
            
        elif mcode == 108:
            # Set future extruder speed
            # Won't take effect until next M101/M102
            #self.mcode_motor1_speed = int(self.c['mapp.p'] * self.c['steps_per_mm_cube'] * 10);
            self.mcode_motor1_speed = int(self.c['mapp.p'] * 10000); # self.mcode_motor1_speed = 10000 equals to 1 rotation per second.
            self.c['mapp.done'] = seqid
            
        elif mcode == 150:
            # Wait for temperature to reach the set value
            self._mapp_heater1_set_sv()
            self.extruder_ready_check = mcode
            self._extruder_ready_poll()
        
        else:
            # Release all unknown MCode
            self.c['mapp.done'] = seqid
            
    def _rb_mcode(self, p, seqid):
        # Not used yet
        self.c['mapp.done'] = seqid
    
    def _rb_dummy(self, p):
        pass
        
    def _rb_status(self, p):
        new_estop_state = p.get_8(1) & 1
        if new_estop_state and not self.estop_state:
            self.c['estop'] = 1
        else:
            self.c['estop'] = 0
        self.estop_state = new_estop_state
        self.c['online'] = p.get_8(1) & 2
        self.c['heater1.on'] = (p.get_8(6) & 1) != 0
        
    def _rb_enable(self, p):
        self.c['fault.communication'] = 0
        self.estop_state = 0
                    
    def _rb_heater1_pvsv(self, p):
        self.c['heater1.pv'] = p.get_16(1)
        self.c['heater1.sv'] = p.get_16(3)
        self._extruder_ready_poll()
        
    def _rb_motor1_pvsv(self, p):
        self.c['motor1.pv'] = p.get_16(1)
        self.c['motor1.sv'] = p.get_16(3)
        
    def sendPacket(self, p, CallBack):
        RetryCounter = 0
        sendFlag = True
        SlaveAnswer = None
        
        while RetryCounter < 10:
            # Send the Packet to slave.
            if sendFlag:
                self.comm.send(p)
                sendFlag = False
                
            # Read answer from slave.
            SlaveAnswer_Packet = self.comm.readback()
            if SlaveAnswer_Packet != None:
                if not(SlaveAnswer_Packet.rc != SimplePacket.RC_OK) and not(SlaveAnswer_Packet.get_8(0) != 1) and not(SlaveAnswer_Packet.id_received != SlaveAnswer_Packet.id()):
                    # Execute the call back if there was no CRC mismatch on slave...
                    self.c['connection'] = 1 # Turn on connection.
                    (CallBack)(SlaveAnswer_Packet)
                    return True

                if SlaveAnswer_Packet.rc != SimplePacket.RC_OK:
                    print >> sys.stderr, datetime.now()
                    print >> sys.stderr, "Slave -> Master communication error: RC: %d" %(SlaveAnswer_Packet.rc)
                    print >> sys.stderr, " "

                if SlaveAnswer_Packet.get_8(0) != 1:
                    print >> sys.stderr, datetime.now()
                    print >> sys.stderr, "Master -> Slave communication error: response_code: %d" %(SlaveAnswer_Packet.get_8(0))
                    print >> sys.stderr, " "

                if SlaveAnswer_Packet.id_received != SlaveAnswer_Packet.id():
                    print >> sys.stderr, datetime.now()
                    print >> sys.stderr, "Packet id error. id = %d; id_received = %d" % (SlaveAnswer_Packet.id(), SlaveAnswer_Packet.id_received)
                    print >> sys.stderr, " "

                sendFlag = True
                RetryCounter += 1
                

        # There was to much CRC errors and/or timeouts...
        # Shut down system
        self.c['fault.communication'] = 1
        self.c['connection'] = 0
        self.c['estop'] = 1
        self.c['online'] = 0     
        self.extruder_ready_check = 0
        self.extruder_state = 0
        self.c['mapp.done'] = self.c['mapp.seqid']

        self.comm.reset()

        #DEBUG
        print >> sys.stderr, datetime.now()
        print >> sys.stderr, "To much extruder communication errors... do EMC2 estop"
        print >> sys.stderr, " "
        #end DEBUG

        # Turn Off
        #p = SimplePacket()
        #p.add_8(0)
        #p.add_8(82)
        #self.sendPacket(p, self._rb_dummy)     
        return None