cocorobolx/blockly/generators/python/motion.js

/**
 * @license Licensed under the Apache License, Version 2.0 (the "License"):
 *          http://www.apache.org/licenses/LICENSE-2.0
 */

/**
 * @fileoverview Code generator for the test 2 blocks.
 */
'use strict';

goog.provide('Blockly.Python.motion');
goog.require('Blockly.Python');


Blockly.Python['motion_setup'] = function(block) {
    var motion_include = '#include "I2Cdev.h"\n' +
        '#include "MPU6050_6Axis_MotionApps20.h"\n' +
        '#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE\n' +
        '#include "Wire.h"\n' +
        '#endif\n';
    Blockly.Python.addInclude('motion_include', motion_include);

    var motion_declaration = '' +
        'MPU6050 mpu\n' +
        '\n' +
        '#define _INTERRUPT_PIN 2\n' +
        '#define _LED_PIN 13\n' +
        'bool _blinkState = false\n' +
        '\n' +
        'bool dmpReady = false;  // set true if DMP init was successful\n' +
        'uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU\n' +
        'uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)\n' +
        'uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)\n' +
        'uint16_t fifoCount;     // count of all bytes currently in FIFO\n' +
        'uint8_t fifoBuffer[64]; // FIFO storage buffer\n' +
        '\n' +
        'Quaternion q;           // [w, x, y, z]         quaternion container\n' +
        'VectorInt16 aa;         // [x, y, z]            accel sensor measurements\n' +
        'VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements\n' +
        'VectorFloat gravity;    // [x, y, z]            gravity vector\n' +
        'float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector\n' +
        '\n' +
        'volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high\n' +
        '\n' +
        'float _motionGetYaw, _motionGetPitch, _motionGetRoll\n' +
        'int _directionState = 0\n' +
        'float _directionValue = 0.00\n' +
        'void dmpDataReady() {\n' +
        'mpuInterrupt = true\n' +
        '}\n' +
        '';
    Blockly.Python.addDeclaration('motion_declaration', motion_declaration);

    var motion_Setup = '' +
        'void _motionSensorSetup() {\n' +
        '#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE\n' +
        'Wire.begin()\n' +
        'Wire.setClock(400000)\n' +
        '#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE\n' +
        'Fastwire::setup(400, true)\n' +
        '#endif\n' +
        'Serial.begin(115200)\n' +
        '// while (!Serial)\n' +
        'Serial.println(F("Initializing I2C devices..."))\n' +
        'mpu.initialize()\n' +
        'pinMode(_INTERRUPT_PIN, INPUT)\n' +
        'Serial.println(F("Testing device connections..."))\n' +
        'Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"))\n' +
        'Serial.println("Waiting for calibrating... 2s")\n' +
        'delay(1000)\n' +
        'Serial.println("Waiting for calibrating... 1s")\n' +
        'delay(1000)\n' +
        'Serial.println(F("Initializing DMP..."))\n' +
        'devStatus = mpu.dmpInitialize()\n' +
        'mpu.setXGyroOffset(220)\n' +
        'mpu.setYGyroOffset(76)\n' +
        'mpu.setZGyroOffset(-85)\n' +
        'mpu.setZAccelOffset(1788)\n' +
        'if (devStatus == 0) {\n' +
        'mpu.CalibrateAccel(6)\n' +
        'mpu.CalibrateGyro(6)\n' +
        'mpu.PrintActiveOffsets()\n' +
        'Serial.println(F("Enabling DMP..."))\n' +
        'mpu.setDMPEnabled(true)\n' +
        'Serial.print(F("Enabling interrupt detection (Arduino external interrupt "))\n' +
        'Serial.print(digitalPinToInterrupt(_INTERRUPT_PIN))\n' +
        'Serial.println(F(")..."))\n' +
        'attachInterrupt(digitalPinToInterrupt(_INTERRUPT_PIN), dmpDataReady, RISING)\n' +
        'mpuIntStatus = mpu.getIntStatus()\n' +
        'Serial.println(F("DMP ready! Waiting for first interrupt..."))\n' +
        'dmpReady = true\n' +
        'packetSize = mpu.dmpGetFIFOPacketSize()\n' +
        '} else {\n' +
        'Serial.print(F("DMP Initialization failed (code "))\n' +
        'Serial.print(devStatus)\n' +
        'Serial.println(F(")"))\n' +
        '}\n' +
        'pinMode(_LED_PIN, OUTPUT)\n' +
        '}\n' +
        '';
    Blockly.Python.addFunction('motion_SetupFunc', motion_Setup);

    var motion_Loop = '' +
        'void _motionSensorMain() {\n' +
        'if (!dmpReady) return\n' +
        'while (!mpuInterrupt && fifoCount < packetSize) {\n' +
        'if (mpuInterrupt && fifoCount < packetSize) {\n' +
        'fifoCount = mpu.getFIFOCount()\n' +
        '}\n' +
        '}\n' +
        'mpuInterrupt = false\n' +
        'mpuIntStatus = mpu.getIntStatus()\n' +
        'fifoCount = mpu.getFIFOCount()\n' +
        'if (fifoCount < packetSize) {\n' +
        '}\n' +
        'else if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {\n' +
        ' mpu.resetFIFO()\n' +
        'Serial.println(F("FIFO overflow!"))\n' +
        '} else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {\n' +
        'while (fifoCount >= packetSize) {\n' +
        'mpu.getFIFOBytes(fifoBuffer, packetSize)\n' +
        'fifoCount -= packetSize\n' +
        '}\n' +
        'mpu.dmpGetQuaternion(&q, fifoBuffer)\n' +
        'mpu.dmpGetGravity(&gravity, &q)\n' +
        'mpu.dmpGetYawPitchRoll(ypr, &q, &gravity)\n' +
        '_motionGetYaw = ypr[0] * 180 / M_PI\n' +
        '_motionGetPitch = ypr[1] * 180 / M_PI\n' +
        '_motionGetRoll = ypr[2] * 180 / M_PI\n' +
        'mpu.dmpGetQuaternion(&q, fifoBuffer)\n' +
        'mpu.dmpGetAccel(&aa, fifoBuffer)\n' +
        'mpu.dmpGetGravity(&gravity, &q)\n' +
        'mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity)\n' +
        '_blinkState = !_blinkState\n' +
        'digitalWrite(_LED_PIN, _blinkState)\n' +
        '}\n' +
        '}\n' +
        '';
    Blockly.Python.addFunction('motion_LoopFunc', motion_Loop);

    Blockly.Python.addSetup("motion_setup", "_motionSensorSetup()\n");

    var code = "_motionSensorMain()\n"
    return code;
};

Blockly.Python['motion_onshake'] = function(block) {
    var onshake_func = '' +
        'boolean _motionOnShake() {\n' +
        'if (aaReal.x < -5000 || aaReal.x > 5000 || aaReal.y < -5000 || aaReal.y > 5000 || aaReal.z < -5000 || aaReal.z > 5000) {\n' +
        'return true\n' +
        '} else {\n' +
        'return false\n' +
        '}\n' +
        '}\n' +
        '';

    Blockly.Python.addFunction("onShake_function", onshake_func);
    var code = "_motionOnShake()";

    return [code, Blockly.Python.ORDER_ATOMIC];
}

Blockly.Python['motion_onDirection'] = function(block) {

    var direction = block.getFieldValue("DIRECTION");
    var ondirection_func = "int _motionOnTilting(int direction) {\n" +
        "  _directionState = 0\n" +
        "  switch (direction) {\n" +
        "    case 1:\n" +
        "      if (ypr[2] * 180 / M_PI < -20) {\n" +
        "        _directionState = 1\n" +
        "      }\n" +
        "      break\n" +
        "    case 2:\n" +
        "      if (ypr[2] * 180 / M_PI > 20) {\n" +
        "        _directionState = 1\n" +
        "      }\n" +
        "      break\n" +
        "    case 3:\n" +
        "      if (ypr[1] * 180 / M_PI < -20) {\n" +
        "        _directionState = 1\n" +
        "      }\n" +
        "      break\n" +
        "    case 4:\n" +
        "      if (ypr[1] * 180 / M_PI > 20) {\n" +
        "        _directionState = 1\n" +
        "      }\n" +
        "      break\n" +
        "    case 5:\n" +
        "      if (ypr[0] * 180 / M_PI +180 - _directionValue > 0) {\n" +
        "        _directionState = 1\n" +
        "      }\n" +
        "      break\n" +
        "    case 6:\n" +
        "      if (ypr[0] * 180 / M_PI +180 - _directionValue < 0) {\n" +
        "        _directionState = 1\n" +
        "      }\n" +
        "      break\n" +
        "    default:\n" +
        "      break\n" +
        "  }\n" +
        "  _directionValue = ypr[0] * 180 / M_PI +180\n" +
        "  return _directionState\n" +
        "}\n";
    Blockly.Python.addFunction("onDirection_function", ondirection_func);

    var direction_tag
    switch (direction) {
        case "left":
            direction_tag = 1;
            break;
        case "right":
            direction_tag = 2;
            break;
        case "front":
            direction_tag = 3;
            break;
        case "back":
            direction_tag = 4;
            break;
        case "clockwise":
            direction_tag = 5;
            break;
        case "counterClockwise":
            direction_tag = 6;
            break;
        default:
            break;
    }
    var code = "_motionOnTilting(" + direction_tag + ")";
    return [code, Blockly.Python.ORDER_ATOMIC];
}

Blockly.Python['motion_getRotation_ypr'] = function(block) {
    var rotation = block.getFieldValue("Rotation");
    var code = "ypr[" + rotation + "] * 180 / M_PI";
    return [code, Blockly.Python.ORDER_ATOMIC];
}

Blockly.Python['motion_getAcceleration'] = function(block) {
    var acc = block.getFieldValue("Acceleration");
    var code = "aaReal." + acc;
    return [code, Blockly.Python.ORDER_ATOMIC];
}