adding custom blocks for microservo (work in progress)

Author: explosivose

coderbot.cfg

@@ -1 +1 @@
-{"move_tr_speed": "75", "move_fw_elapse": "1.5", "show_page_program": "true", "load_at_start": "", "move_tr_elapse": "90", "sound_start": "$startup.mp3", "sound_stop": "$shutdown.mp3", "camera_exposure_mode": "auto", "prog_move_motion": "no", "show_control_move_commands": "true", "prog_level": "adv", "prog_scrollbars": "true", "ctrl_fw_speed": "100", "move_fw_speed": "100", "show_page_control": "true", "sound_shutter": "$shutter.mp3", "show_page_prefs": "true", "prog_maxblocks": "-1", "ctrl_hud_image": "", "button_func": "none", "move_motor_mode": "servo", "ctrl_fw_elapse": "-1", "ctrl_tr_elapse": "-1", "move_power_angle_2": "20", "move_power_angle_3": "20", "ctrl_tr_speed": "80", "move_power_angle_1": "15"}
+{"move_tr_speed": "75", "move_fw_elapse": "1.5", "show_page_program": "true", "load_at_start": "", "move_tr_elapse": "90", "sound_start": "$startup.mp3", "sound_stop": "$shutdown.mp3", "camera_exposure_mode": "auto", "prog_move_motion": "no", "show_control_move_commands": "true", "prog_level": "adv", "prog_scrollbars": "true", "ctrl_fw_speed": "100", "move_fw_speed": "100", "show_page_control": "true", "sound_shutter": "$shutter.mp3", "show_page_prefs": "true", "prog_maxblocks": "-1", "ctrl_hud_image": "", "button_func": "none", "move_motor_mode": "servo", "ctrl_fw_elapse": "-1", "ctrl_tr_elapse": "-1", "move_power_angle_2": "20", "move_power_angle_3": "20", "ctrl_tr_speed": "80", "move_power_angle_1": "15","arm_angle_raised": "60","arm_angle_lowered": "120"}

coderbot.py

@@ -4,14 +4,14 @@
 
 PIN_MOTOR_ENABLE = 22
 PIN_LEFT_FORWARD = 25
-PIN_LEFT_BACKWARD = 24
+PIN_LEFT_BACKWARD = 24 # NOT USED FOR SERVOS
 PIN_RIGHT_FORWARD = 4
-PIN_RIGHT_BACKWARD = 17
+PIN_RIGHT_BACKWARD = 17 # NOT USED FOR SERVOS
 PIN_PUSHBUTTON = 18
 PIN_SERVO_3 = 9
 PIN_SERVO_4 = 10
 
-PWM_FREQUENCY = 100 #Hz
+PWM_FREQUENCY = 50 #Hz
 PWM_RANGE = 100 #0-100
 
 def coderbot_callback(gpio, level, tick):
@@ -114,15 +114,15 @@ def _servo_motor_control(self, pin, speed):
     self._is_moving = True
     speed = ((speed + 100) * 50 / 200) + 52
 
-    self.pi.set_PWM_range(pin, 1000)
-    self.pi.set_PWM_frequency(pin, 50)
+    self.pi.set_PWM_range(pin, PWM_RANGE)
+    self.pi.set_PWM_frequency(pin, PWM_FREQUENCY)
     self.pi.set_PWM_dutycycle(pin, speed)
 
   def _servo_control(self, pin, angle):
     duty = ((angle + 90) * 100 / 180) + 25
 
-    self.pi.set_PWM_range(pin, 1000)
-    self.pi.set_PWM_frequency(pin, 50)
+    self.pi.set_PWM_range(pin, PWM_RANGE)
+    self.pi.set_PWM_frequency(pin, PWM_FREQUENCY)
     self.pi.set_PWM_dutycycle(pin, duty)
 
   def stop(self):

static/js/blockly/blocks.js

@@ -760,3 +760,63 @@ Blockly.Python['coderbot_adv_findLogo'] = function(block) {
   var code = 'get_cam().find_logo()';
   return [code, Blockly.Python.ORDER_ATOMIC];
 };
+
+Blockly.Blocks['coderbot_armRaise'] = {
+  /**
+     * Block for armRaise function.
+     * @this Blockly.Block
+     */
+  init: function() {
+    this.setHelpUrl(Blockly.Msg.CODERBOT_ARM_RAISE_HELPURL);
+    this.setColour(40);
+    var di = this.appendDummyInput()    
+    if (CODERBOT_PROG_LEVEL.indexOf("basic")>=0) {
+      di.appendField(new Blockly.FieldImage('/images/blocks/arm_raise.png', 32, 32, '*'));
+    } else {
+      di.appendField(Blockly.Msg.CODERBOT_ARM_RAISE)
+    }
+    this.setPreviousStatement(true);
+    this.setNextStatement(true);
+    this.setTooltip('Coderbot_armRaiseTooltip');
+  }
+};
+
+Blockly.JavaScript['coderbot_armRaise'] = function(block) {
+  // Generate JavaScript for raising the arm
+  return 'get_bot()..servo3(' + CODERBOT_ARM_ANGLE_RAISED + ');\n';
+};
+
+Blockly.Python['coderbot_armRaise'] = function(block) {
+  // Generate Python code for raising the arm
+  return 'get_bot()..servo3(' + CODERBOT_ARM_ANGLE_RAISED + ')\n';
+};
+
+Blockly.Blocks['coderbot_armLower'] = {
+  /** 
+     * Block for armLower function.
+     * @this Blockly.Block
+     */
+  init: function() {
+    this.setHelpUrl(Blockly.Msg.CODERBOT_ARM_RAISE_HELPURL);
+    this.setColour(40);
+    var di = this.appendDummyInput()
+    if (CODERBOT_PROG_LEVEL.indexOf("basic")>=0) {
+      di.appendField(new Blockly.FieldImage('/image/blocks/arm_lower.png', 32, 32, '*'));
+    } else {
+      di.appendField(Blockly.Msg.CODERBOT_ARM_LOWER)
+    }
+    this.setPreviousStatement(true);
+    this.setNextStatement(true);
+    this.setTooltip('Coderbot_armLowerTooltip');
+  } 
+};
+
+Blockly.JavaScript['coderbot_armLower'] = function(block) {
+  // Generate JavaScript code for lowering the arm
+  return 'get_bot().servo3(' + CODERBOT_ARM_ANGLE_LOWERED + ');\n';
+};
+
+Blockly.Python['coderbot_armLower'] = function(block) {
+  // Generate Python code for lowering the arm
+  return 'get_bot().servo3(' + CODERBOT_ARM_ANGLE_LOWERED + ')\n';
+};

templates/blocks_std.xml

@@ -58,6 +58,10 @@
         </value>
       </block>
     </category>
+    <category name ="{% trans %}Arm{% endtrans %}">
+      <block type="coderbot_armRaise"></block>
+      <block type="coderbot_armLower"></block>
+    </category>
     <category name="{% trans %}Sensor{% endtrans %}">
       <block type="coderbot_adv_pathAhead"></block>
       <block type="coderbot_adv_findLine"></block>

templates/config_params.html

@@ -15,7 +15,8 @@
 var CODERBOT_CTRL_TR_ELAPSE="{{config.ctrl_tr_elapse}}";
 var CODERBOT_CTRL_COUNTER="{{config.ctrl_counter}}"=="yes";
 var CODERBOT_CTRL_MOVE_MOTION="{{config.ctrl_move_motion}}"=="yes";
-
+var CODERBOT_ARM_ANGLE_LOWERED="{{config.arm_angle_lowered}}";
+var CODERBOT_ARM_ANGLE_RAISED="{{config.arm_angle_raised}}";
 var BotMessages = Object();
 BotMessages.Input = "{%trans%}Say what:{%endtrans%}";
 BotMessages.Saved = "{%trans%}Saved{%endtrans%}";