commit by robo pro coding

.project.json

+{"uuid":"9213cba6-0e6d-620e-7bac-ef67eb24df3a","name":"RoboMission_linepid","mode":"ADVANCED","version":"1.0","controller":"TXT4"}
\ No newline at end of file

RoboMission_linepid.py

+import fischertechnik.utility.math as ft_math
+import math
+from fischertechnik.controller.Motor import Motor
+from lib.controller import *
+from lib.display import *
+
+speed = None
+i1 = None
+speed_slow = None
+i2 = None
+counter = None
+calib_i1_min = None
+calib_i1_max = None
+line = None
+control = None
+calib_i2_min = None
+calib_i2_max = None
+white = None
+Kp = None
+Ki = None
+Kd = None
+hue_grad = None
+setpoint = None
+
+
+def readSensors():
+    global speed, i1, speed_slow, i2, counter, calib_i1_min, calib_i1_max, line, control, calib_i2_min, calib_i2_max, white, Kp, Ki, Kd, hue_grad, setpoint
+    i1 = ft_math.map(TXT_M_I1_color_sensor.get_voltage(), calib_i1_min, calib_i1_max, 100, 0)
+    i2 = ft_math.map(TXT_M_I2_color_sensor.get_voltage(), calib_i2_min, calib_i2_max, 100, 0)
+    line = i1 - i2
+    white = i1 > 80 and i2 > 80
+
+
+def setMotor():
+    global speed, i1, speed_slow, i2, counter, calib_i1_min, calib_i1_max, line, control, calib_i2_min, calib_i2_max, white, Kp, Ki, Kd, hue_grad, setpoint
+    speed = round(control)
+    if speed > 0:
+        if speed < speed_slow:
+            speed = speed_slow
+        TXT_M_M1_encodermotor.set_speed(int(speed), Motor.CW)
+        TXT_M_M1_encodermotor.start()
+        TXT_M_M2_encodermotor.set_speed(int(speed), Motor.CCW)
+        TXT_M_M2_encodermotor.start()
+    elif speed < 0:
+        if math.fabs(speed) < speed_slow:
+            speed = -speed_slow
+        TXT_M_M1_encodermotor.set_speed(int(math.fabs(speed)), Motor.CCW)
+        TXT_M_M1_encodermotor.start()
+        TXT_M_M2_encodermotor.set_speed(int(math.fabs(speed)), Motor.CW)
+        TXT_M_M2_encodermotor.start()
+    else:
+        TXT_M_M1_encodermotor.stop()
+        TXT_M_M2_encodermotor.stop()
+
+
+def initPID():
+    global speed, i1, speed_slow, i2, counter, calib_i1_min, calib_i1_max, line, control, calib_i2_min, calib_i2_max, white, Kp, Ki, Kd, hue_grad, setpoint
+    global pid
+    from simple_pid import PID
+    pid = PID(0.001, 0.0, 0.0)
+    pid.sample_time = 0.01
+    pid.setpoint = 0.0
+    pid.output_limits = (-512, 512)
+
+
+def initDisplay():
+    global speed, i1, speed_slow, i2, counter, calib_i1_min, calib_i1_max, line, control, calib_i2_min, calib_i2_max, white, Kp, Ki, Kd, hue_grad, setpoint
+    Kp = pid.Kp
+    Ki = pid.Ki
+    Kd = pid.Kd
+    display.set_attr("txt_label_kp.text", str('Kp: ' + str(Kp)))
+    display.set_attr("txt_label_ki.text", str('Ki: ' + str(Ki)))
+    display.set_attr("txt_label_kd.text", str('Kd: ' + str(Kd)))
+    counter = 0
+
+
+def printConsole():
+    global speed, i1, speed_slow, i2, counter, calib_i1_min, calib_i1_max, line, control, calib_i2_min, calib_i2_max, white, Kp, Ki, Kd, hue_grad, setpoint
+    if counter >= 200:
+        setpoint = int(pid.setpoint)
+        print('white:{} line:{} control:{:.5f} speed:{}'.format(white, line, control, speed))
+        display.set_attr("txt_label_input.text", str('input: ' + str(line)))
+        display.set_attr("txt_label_output.text", str('output: ' + str(speed)))
+        display.set_attr("txt_label_setpoint.text", str('setpoint: ' + str(setpoint)))
+        counter = 0
+    counter = (counter if isinstance(counter, (int, float)) else 0) + 1
+
+
+speed = 512
+speed_slow = 110
+calib_i1_min = 190
+calib_i1_max = 1544
+calib_i2_min = 202
+calib_i2_max = 1602
+hue_grad = -1
+white = False
+line = 0
+i1 = 0
+i2 = 0
+initPID()
+initDisplay()
+while True:
+    readSensors()
+    control = pid(line)
+    if not white:
+        if math.fabs(control) > 0.001:
+            setMotor()
+        else:
+            TXT_M_M1_encodermotor.set_speed(int(512), Motor.CCW)
+            TXT_M_M1_encodermotor.start()
+            TXT_M_M2_encodermotor.set_speed(int(512), Motor.CCW)
+            TXT_M_M2_encodermotor.start()
+    else:
+        TXT_M_M1_encodermotor.stop()
+        TXT_M_M2_encodermotor.stop()
+    printConsole()

lib/controller.blockly

+<xml xmlns="https://developers.google.com/blockly/xml" version="16">
+  <block type="txt4_controller" id="]jyp1`LYXYlo/M!X.qRj" x="115" y="6">
+    <mutation>
+      <arg type="Input" name="Input-0" value="I1"/>
+      <arg type="Input" name="Input-1" value="I2"/>
+      <arg type="Input" name="Input-2" value="-"/>
+      <arg type="Output" name="Output-0" value="-"/>
+      <arg type="Motor" name="Motor-0" value="M1"/>
+      <arg type="Motor" name="Motor-1" value="M2"/>
+      <arg type="Motor" name="Motor-2" value="-"/>
+      <arg type="Servomotor" name="Servomotor-0" value="-"/>
+      <arg type="Counter" name="Counter-0" value="C1"/>
+      <arg type="Counter" name="Counter-1" value="C2"/>
+      <arg type="Counter" name="Counter-2" value="-"/>
+      <arg type="I2C" name="I2C-0" value="-"/>
+      <arg type="USB" name="USB-0" value="-"/>
+      <arg type="USB" name="USB-1" value="-"/>
+    </mutation>
+    <field name="mode">0</field>
+    <field name="controller_name">TXT</field>
+    <field name="Input-0">I1</field>
+    <field name="Input-1">I2</field>
+    <field name="Input-2">-</field>
+    <field name="Output-0">-</field>
+    <field name="Motor-0">M1</field>
+    <field name="Motor-1">M2</field>
+    <field name="Motor-2">-</field>
+    <field name="Servomotor-0">-</field>
+    <field name="Counter-0">C1</field>
+    <field name="Counter-1">C2</field>
+    <field name="Counter-2">-</field>
+    <field name="I2C-0">-</field>
+    <field name="USB-0">-</field>
+    <field name="USB-1">-</field>
+    <value name="Input-0">
+      <block type="color_sensor" id="2K4(*|Tl:giF*gFGeii|"/>
+    </value>
+    <value name="Input-1">
+      <block type="color_sensor" id=")G5P@mtC1/-aC|15xdb2"/>
+    </value>
+    <value name="Motor-0">
+      <block type="encodermotor" id="9.(GoRMQKw3xJ^sDiZK7"/>
+    </value>
+    <value name="Motor-1">
+      <block type="encodermotor" id="P)*|(bMu^h~zYL=VG.K0"/>
+    </value>
+    <value name="Counter-0">
+      <block type="motor_step_counter" id="w]E;l;Hnv.[`txXQsgF0">
+        <field name="instance_name">M1</field>
+      </block>
+    </value>
+    <value name="Counter-1">
+      <block type="motor_step_counter" id="090DS`!K@df$8Df)5/jg">
+        <field name="instance_name">M2</field>
+      </block>
+    </value>
+  </block>
+</xml>
\ No newline at end of file

lib/controller.py

+import fischertechnik.factories as txt_factory
+
+txt_factory.init()
+txt_factory.init_input_factory()
+txt_factory.init_motor_factory()
+txt_factory.init_counter_factory()
+
+TXT_M = txt_factory.controller_factory.create_graphical_controller()
+TXT_M_I1_color_sensor = txt_factory.input_factory.create_color_sensor(TXT_M, 1)
+TXT_M_I2_color_sensor = txt_factory.input_factory.create_color_sensor(TXT_M, 2)
+TXT_M_M1_encodermotor = txt_factory.motor_factory.create_encodermotor(TXT_M, 1)
+TXT_M_M2_encodermotor = txt_factory.motor_factory.create_encodermotor(TXT_M, 2)
+TXT_M_C1_motor_step_counter = txt_factory.counter_factory.create_encodermotor_counter(TXT_M, 1)
+TXT_M_C1_motor_step_counter.set_motor(TXT_M_M1_encodermotor)
+TXT_M_C2_motor_step_counter = txt_factory.counter_factory.create_encodermotor_counter(TXT_M, 2)
+TXT_M_C2_motor_step_counter.set_motor(TXT_M_M2_encodermotor)
+
+txt_factory.initialized()
\ No newline at end of file

lib/display.py

+import os
+import time
+import threading
+import signal
+import types
+import ftgui
+
+display = ftgui.fttxt2_gui_connector("app")
+display.open()
+
+def display_monitoring():
+    while display.is_open():
+        time.sleep(1)
+    os.kill(os.getpid(), signal.SIGTERM)
+    exit()
+
+threading.Thread(target=display_monitoring, daemon=True).start()
+
+def __set_base64_image(self, cameraDisplayName, base64Img):
+    width = str(self.get_attr(cameraDisplayName + ".width"))
+    height = str(self.get_attr(cameraDisplayName + ".height"))
+    imageTag = '<img src="' + str(base64Img) + '" width="' + width + '" height="' + height + '" />'
+    self.set_attr(cameraDisplayName + ".text", imageTag)
+    time.sleep(0.75)
+
+display.set_base64_image = types.MethodType(__set_base64_image, display)
+

lib/display.qml

+// auto generated content from display configuration
+import QtQuick 2.2
+import QtQuick.Window 2.0
+import QtQuick.Controls 1.1
+import QtQuick.Controls.Styles 1.1
+import QtQuick.Extras 1.4
+
+TXTWindow {
+  Rectangle {
+    id: rect
+    color: "grey"
+    anchors.fill: parent
+  }
+  TXTLabel {
+    id: txt_label_input
+    text: "input:"
+    font.pixelSize: 16
+    font.bold: false
+    font.italic: false
+    font.underline: false
+    horizontalAlignment: Text.AlignLeft
+    color: "#ffffff"
+    elide: Text.ElideRight
+    x: 5
+    y: 5
+    width: 100
+    height: 40
+  }
+  TXTLabel {
+    id: txt_label_output
+    text: "output:"
+    font.pixelSize: 16
+    font.bold: false
+    font.italic: false
+    font.underline: false
+    horizontalAlignment: Text.AlignLeft
+    color: "#ffffff"
+    elide: Text.ElideRight
+    x: 5
+    y: 45
+    width: 100
+    height: 40
+  }
+  TXTLabel {
+    id: txt_label_setpoint
+    text: "setpoint:"
+    font.pixelSize: 16
+    font.bold: false
+    font.italic: false
+    font.underline: false
+    horizontalAlignment: Text.AlignLeft
+    color: "#ffffff"
+    elide: Text.ElideRight
+    x: 5
+    y: 85
+    width: 100
+    height: 40
+  }
+  TXTLabel {
+    id: txt_label_kp
+    text: "Kp"
+    font.pixelSize: 16
+    font.bold: false
+    font.italic: false
+    font.underline: false
+    horizontalAlignment: Text.AlignLeft
+    color: "#ffffff"
+    elide: Text.ElideRight
+    x: 135
+    y: 5
+    width: 100
+    height: 40
+  }
+  TXTLabel {
+    id: txt_label_ki
+    text: "Ki"
+    font.pixelSize: 16
+    font.bold: false
+    font.italic: false
+    font.underline: false
+    horizontalAlignment: Text.AlignLeft
+    color: "#ffffff"
+    elide: Text.ElideRight
+    x: 135
+    y: 45
+    width: 100
+    height: 40
+  }
+  TXTLabel {
+    id: txt_label_kd
+    text: "Kd"
+    font.pixelSize: 16
+    font.bold: false
+    font.italic: false
+    font.underline: false
+    horizontalAlignment: Text.AlignLeft
+    color: "#ffffff"
+    elide: Text.ElideRight
+    x: 135
+    y: 85
+    width: 100
+    height: 40
+  }
+}

lib/display.xml

+<xml type="display" version="2"><item id="2" class="TXTLabel"><name>txt_label_input</name><text>input:</text><fontSize>16</fontSize><fontStyle/><horizontalAlignment>left</horizontalAlignment><color>#ffffff</color><geometry><x>5</x><y>5</y><width>100</width><height>40</height></geometry></item><item id="3" class="TXTLabel"><name>txt_label_output</name><text>output:</text><fontSize>16</fontSize><fontStyle/><horizontalAlignment>left</horizontalAlignment><color>#ffffff</color><geometry><x>5</x><y>45</y><width>100</width><height>40</height></geometry></item><item id="4" class="TXTLabel"><name>txt_label_setpoint</name><text>setpoint:</text><fontSize>16</fontSize><fontStyle/><horizontalAlignment>left</horizontalAlignment><color>#ffffff</color><geometry><x>5</x><y>85</y><width>100</width><height>40</height></geometry></item><item id="5" class="TXTLabel"><name>txt_label_kp</name><text>Kp</text><fontSize>16</fontSize><fontStyle/><horizontalAlignment>left</horizontalAlignment><color>#ffffff</color><geometry><x>135</x><y>5</y><width>100</width><height>40</height></geometry></item><item id="6" class="TXTLabel"><name>txt_label_ki</name><text>Ki</text><fontSize>16</fontSize><fontStyle/><horizontalAlignment>left</horizontalAlignment><color>#ffffff</color><geometry><x>135</x><y>45</y><width>100</width><height>40</height></geometry></item><item id="7" class="TXTLabel"><name>txt_label_kd</name><text>Kd</text><fontSize>16</fontSize><fontStyle/><horizontalAlignment>left</horizontalAlignment><color>#ffffff</color><geometry><x>135</x><y>85</y><width>100</width><height>40</height></geometry></item></xml>
\ No newline at end of file