/**
* @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];
}