Take side cherry

Author: MarijaGolubovic

mep3_behavior/strategies/project.btproj

@@ -28,6 +28,7 @@
     <include path="skill_put_cherry_up.xml"/>
     <include path="skill_put_one_cherry.xml"/>
     <include path="skill_take_cherry_blue_start.xml"/>
+    <include path="skill_take_side_cherry.xml"/>
     <include path="small_homologation.xml"/>
     <include path="small_strategy.xml"/>
     <include path="small_strategy_demo.xml"/>

mep3_behavior/strategies/skill_take_cherry_blue_start.xml

@@ -14,7 +14,7 @@
                  node="{node}"
                  table="{table}"/>
       </ForceSuccess>
-      <Wait duration="1"/>
+      <Wait duration="2"/>
       <CanbusSend can_id="27650"
                   message="0"/>
     </TaskSequence>

mep3_behavior/strategies/skill_take_side_cherry.xml

@@ -0,0 +1,45 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="skill_take_side_cherry">
+    <Sequence>
+      <Navigate behavior_tree="0.2;1.08;90"
+                goal=""/>
+      <Sequence>
+        <ForceSuccess>
+          <CanbusSend can_id="27650"
+                      message="250"/>
+        </ForceSuccess>
+        <Wait duration="0.2"/>
+        <SubTree ID="move_cherry"
+                 color="{color}"
+                 goal="0.2;1.15;90"
+                 node="{node}"
+                 table="{table}"/>
+        <Wait duration="2"/>
+        <ForceSuccess>
+          <CanbusSend can_id="27650"
+                      message="250"/>
+        </ForceSuccess>
+      </Sequence>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Action ID="CanbusSend"
+            editable="true">
+      <input_port name="can_id">For example, 0x12345</input_port>
+      <input_port name="message">For example, 0x32;0x34</input_port>
+    </Action>
+    <Action ID="Navigate"
+            editable="true">
+      <input_port name="behavior_tree"/>
+      <input_port name="goal"/>
+    </Action>
+    <Action ID="Wait"
+            editable="true">
+      <input_port name="duration"/>
+    </Action>
+  </TreeNodesModel>
+
+</root>

mep3_behavior/strategies/small_strategy.xml

@@ -29,6 +29,23 @@
                              node="{node}"
                              table="{table}"/>
                   </ForceSuccess>
+                  <ForceSuccess>
+                    <Sequence>
+                      <Translate goal="0.3"
+                                 max_acceleration="0.5"
+                                 max_velocity="0.8"/>
+                      <SubTree ID="skill_take_side_cherry"
+                               color="{color}"
+                               node="{node}"
+                               table="{table}"/>
+                      <ForceSuccess>
+                        <SubTree ID="skill_basket_blue"
+                                 color="{color}"
+                                 node="{node}"
+                                 table="{table}"/>
+                      </ForceSuccess>
+                    </Sequence>
+                  </ForceSuccess>
                 </TaskSequence>
               </Timeout>
             </ForceSuccess>
@@ -88,6 +105,15 @@
     </Action>
     <Control ID="TaskSequence"
              editable="true"/>
+    <Action ID="Translate"
+            editable="true">
+      <input_port name="goal"
+                  default="0"/>
+      <input_port name="max_acceleration"
+                  default="0.5"/>
+      <input_port name="max_velocity"
+                  default="0.8"/>
+    </Action>
     <Action ID="WaitMatchStart"
             editable="true">
       <input_port name="state"