Announcement

Collapse

Technology Forum Has Moved!

The FIRST Tech Challenge Technology forum has moved to a new location! Please take a look at our forum blog for links and instructions on how to access the new forum.

The official blog of the FIRST Tech Challenge - a STEM robotics programs for students grades 7-12.


Note that volunteers (except for FTA/WTA/CSA will still access their role specific forum through this site. The blog also outlines how to access the volunteer forums.
See more
See less

Accessing the phone's sensors

Collapse
X
 
  • Filter
  • Time
  • Show
Clear All
new posts

  • Accessing the phone's sensors

    Is it possible to access the phone's sensors and used them in an OpMode? How can we do that?
    We are trying to create a class that reads the Phone's Gyro and use this class in an opMode. As far as we understand, we need to use the getSystemService method, however, it is defined only for activity classes. We found out that this method can be called in a non-activity class, if the context of the activity is passed to that class in its constructor. How do we do that?

  • #2
    Originally posted by yifat View Post
    Is it possible to access the phone's sensors and used them in an OpMode? How can we do that?
    We are trying to create a class that reads the Phone's Gyro and use this class in an opMode. As far as we understand, we need to use the getSystemService method, however, it is defined only for activity classes. We found out that this method can be called in a non-activity class, if the context of the activity is passed to that class in its constructor. How do we do that?
    Our library supports Android sensors. You may want to look at this code to get an idea on how to access them.

    Comment


    • #3
      Sample Phone Gyro Code

      You can take a look at the below sample code which we have used in the past

      Code:
      /* Copyright (c) 2014 Qualcomm Technologies IncAll rights reserved.Redistribution and use in source and binary forms, with or without modification,are permitted (subject to the limitations in the disclaimer below) provided thatthe following conditions are met:Redistributions of source code must retain the above copyright notice, this listof conditions and the following disclaimer.Redistributions in binary form must reproduce the above copyright notice, thislist of conditions and the following disclaimer in the documentation and/orother materials provided with the distribution.Neither the name of Qualcomm Technologies Inc nor the names of its contributorsmay be used to endorse or promote products derived from this software withoutspecific prior written permission.NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THISLICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSEARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLEFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIALDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS ORSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVERCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USEOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */package org.batteriesinblack.ftc.OpMode;import android.content.Context;import android.hardware.Sensor;import android.hardware.SensorEvent;import android.hardware.SensorEventListener;import android.hardware.SensorManager;import com.qualcomm.ftccommon.DbgLog;import com.qualcomm.robotcore.eventloop.opmode.Autonomous;import com.qualcomm.robotcore.eventloop.opmode.Disabled;import com.qualcomm.robotcore.eventloop.opmode.OpMode;import com.qualcomm.robotcore.hardware.DcMotor;import com.qualcomm.robotcore.util.Range;import org.batteriesinblack.ftc.utility.DataLogger;import java.io.BufferedWriter;import java.io.File;import java.io.FileWriter;import java.io.IOException;import java.text.DecimalFormat;import java.text.SimpleDateFormat;import java.util.Date;import java.util.Locale;import java.util.Timer;import java.util.TimerTask;/** * TeleOp Mode * <p> * Enables control of the robot via the gamepad */@Autonomous(name = "GyroIntegration4855", group = "NavX")@Disabledpublic class GyroIntegration4855 extends OpMode implements SensorEventListener {    /*     * Note: the configuration of the servos is such that     * as the arm servo approaches 0, the arm position moves up (away from the floor).     * Also, as the claw servo approaches 0, the claw opens up (drops the game element).     */    private long eventTimeStamp;    private String startDate;    private String updateDate;    private boolean bInitialized = false;    private SensorManager mSensorManager = null;    private final float[] linearacceleration = new float[3];    // angular speeds from gyro    private final float[] gyro = new float[3];    // rotation matrix from gyro data    private float[] gyroMatrix = new float[9];    // orientation angles from gyro matrix    private final float[] gyroOrientation = new float[3];    // magnetic field vector    private final float[] magnet = new float[3];    // accelerometer vector    private final float[] accel = new float[3];    // orientation angles from accel and magnet    private final float[] accMagOrientation = new float[3];    // final orientation angles from sensor fusion    private final float[] fusedOrientation = new float[3];    // accelerometer and magnetometer based rotation matrix    private final float[] rotationMatrix = new float[9];    private static final float EPSILON = 0.000000001f;    private static final float NS2S = 1.0f / 1000000000.0f;    private long timestamp;    private boolean initState = true;    private static final int TIME_CONSTANT = 30;    private static final float FILTER_COEFFICIENT = 0.98f;    private final Timer fuseTimer = new Timer();    private DataLogger out;    @Override    public void init(){        out = new DataLogger("Sensor Data Orientation 1");    }    // The following members are only for displaying the sensor output.    private final DecimalFormat d = new DecimalFormat("#.##");    /**     * Constructor     */    public GyroIntegration4855() {        bInitialized = false;    }    /*     * Code to run when the op mode is first enabled goes here     *      * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()     */    @Override    public void start() {        /*         * Use the hardwareMap to get the dc motors and servos by name. Note         * that the names of the devices must match the names used when you         * configured your robot and created the configuration file.         */                /*         * For the demo Tetrix K9 bot we assume the following,         *   There are two motors "motor_1" and "motor_2"         *   "motor_1" is on the right side of the bot.         *   "motor_2" is on the left side of the bot.         *            * We also assume that there are two servos "servo_1" and "servo_6"         *    "servo_1" controls the arm joint of the manipulator.         *    "servo_6" controls the claw joint of the manipulator.         */        startDate = new SimpleDateFormat("yyyy/MM/dd HH:mm:ss", Locale.US).format(new Date());        gyroOrientation[0] = 0.0f;        gyroOrientation[1] = 0.0f;        gyroOrientation[2] = 0.0f;        // initialise gyroMatrix with identity matrix        gyroMatrix[0] = 1.0f; gyroMatrix[1] = 0.0f; gyroMatrix[2] = 0.0f;        gyroMatrix[3] = 0.0f; gyroMatrix[4] = 1.0f; gyroMatrix[5] = 0.0f;        gyroMatrix[6] = 0.0f; gyroMatrix[7] = 0.0f; gyroMatrix[8] = 1.0f;        // get sensorManager and initialise sensor listeners        mSensorManager = (SensorManager) hardwareMap.appContext.getSystemService(Context.SENSOR_SERVICE);        if (mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER) == null ||                mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE) == null ||                mSensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION) == null ||                mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD) == null) {            DbgLog.msg("FTC4855 - For this OpMode a phone with Gyro / Accelerometer / Magnetometer is required");            bInitialized = false;        }        else {            bInitialized = true;            initListeners();            // wait for one second until gyroscope and magnetometer/accelerometer            // data is initialised then scedule the complementary filter task            fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(),                    1000, TIME_CONSTANT);        }        out.addField("Magnetic Orientation X");        out.addField("Magnetic Orientation Y");        out.addField("Magnetic Orientation Z");        out.addField("Gyro Orientation X");        out.addField("Gyro Orientation Y");        out.addField("Gyro Orientation Z");        out.addField("Gyro Fused Orientation X");        out.addField("Gyro Fused Orientation Y");        out.addField("Gyro Fused Orientation Z");        out.addField("Linear Acceleration X");        out.addField("Linear Acceleration Y");        out.addField("Linear Acceleration Z");        out.newLine();    }    /*     * This method will be called repeatedly in a loop     *      * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()     */    @Override    public void loop() {        if (bInitialized) {        /*         * Send telemetry data back to driver station. Note that if we are using         * a legacy NXT-compatible motor controller, then the getPower() method         * will return a null value. The legacy NXT-compatible motor controllers         * are currently write only.         *///            telemetry.addData("1 Text", "*** Robot Data***" + startDate);//            telemetry.addData("4 Update", "Sensor Values updated at " + updateDate + " Event Timestamp " + String.format("%d", eventTimeStamp));//            telemetry.addData("5 Note1", " ");////            telemetry.addData("6 Magnetic Orientation azimuth / pitch / roll", d.format(accMagOrientation[0] * 180 / Math.PI) + "∞ "//                    + d.format(accMagOrientation[1] * 180 / Math.PI) + "∞ "//                    + d.format(accMagOrientation[2] * 180 / Math.PI) + "∞ ");////            out.addField(accMagOrientation[0] * 180 / Math.PI);//            out.addField(accMagOrientation[1] * 180 / Math.PI);//            out.addField(accMagOrientation[2] * 180 / Math.PI);////            telemetry.addData("7 Gyro Orientation azimuth / pitch / roll", d.format(gyroOrientation[0] * 180 / Math.PI) + "∞ "//                    + d.format(gyroOrientation[1] * 180 / Math.PI) + "∞ "//                    + d.format(gyroOrientation[2] * 180 / Math.PI) + "∞ ");////            out.addField(gyroOrientation[0] * 180 / Math.PI);//            out.addField(gyroOrientation[1] * 180 / Math.PI);//            out.addField(gyroOrientation[2] * 180 / Math.PI);////            telemetry.addData("8 Gyro Orientation azimuth / pitch / roll", d.format(fusedOrientation[0] * 180 / Math.PI) + "∞ "//                    + d.format(fusedOrientation[1] * 180 / Math.PI) + "∞ "//                    + d.format(fusedOrientation[2] * 180 / Math.PI) + "∞ ");////            out.addField(fusedOrientation[0] * 180 / Math.PI);//            out.addField(fusedOrientation[1] * 180 / Math.PI);//            out.addField(fusedOrientation[2] * 180 / Math.PI);////            telemetry.addData("9 Linear Acceleration", "x-axis = " + String.format("%.2f", linearacceleration[0])//                    + " y-axis = " + String.format("%.2f", linearacceleration[1])//                    + " z-axis = " + String.format("%.2f", linearacceleration[2]));////            out.addField(linearacceleration[0] * 180 / Math.PI);//            out.addField(linearacceleration[0] * 180 / Math.PI);//            out.addField(linearacceleration[0] * 180 / Math.PI);////            out.newLine();            if(gyroOrientation[2] * 180 / Math.PI < -15){                telemetry.addData("Tilting", "Left");            }            else if(gyroOrientation[2] * 180 / Math.PI > 15){                telemetry.addData("Tilting", "Right");            }            else if (gyroOrientation[1] * 180 / Math.PI < -15){                telemetry.addData("Tilting", "Back");            }            else if (gyroOrientation[1] * 180 / Math.PI > 15){                telemetry.addData("Tilting", "Forward");            }            else{                telemetry.addData("Not Tilting", "Machine Learning");            }        }  else {            telemetry.addData("1 Text", "*** Robot Data***" + startDate);            telemetry.addData("2 Fatal", "For this OpMode a phone with Gyro / Accelerometer / Magnetometer is required");            out.addField("Fatal, For this OpMode a phone with Gyro / Accelerometer / Magnetometer is required");        }    }    /*     * Code to run when the op mode is first disabled goes here     *      * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#stop()     */    @Override    public void stop() {        out.closeDataLogger();        if (bInitialized) {            mSensorManager.unregisterListener(this);        }    }    /*     * This method scales the joystick input so for low joystick values, the      * scaled value is less than linear.  This is to make it easier to drive     * the robot more precisely at slower speeds.     */    private double scaleInput(double dVal)  {        double[] scaleArray = { 0.0, 0.05, 0.09, 0.10, 0.12, 0.15, 0.18, 0.24,                0.30, 0.36, 0.43, 0.50, 0.60, 0.72, 0.85, 1.00, 1.00 };                // get the corresponding index for the scaleInput array.        int index = (int) (dVal * 16.0);        if (index < 0) {            index = -index;        } else if (index > 16) {            index = 16;        }                double dScale;        if (dVal < 0) {            dScale = -scaleArray[index];        } else {            dScale = scaleArray[index];        }                return dScale;    }    // This function registers sensor listeners for the accelerometer, magnetometer and gyroscope.    private void initListeners(){        mSensorManager.registerListener(this,                mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),                SensorManager.SENSOR_DELAY_FASTEST);        mSensorManager.registerListener(this,                mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE),                SensorManager.SENSOR_DELAY_FASTEST);        mSensorManager.registerListener(this,                mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),                SensorManager.SENSOR_DELAY_FASTEST);        mSensorManager.registerListener(this,                mSensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION),                SensorManager.SENSOR_DELAY_FASTEST);    }    public void onAccuracyChanged(Sensor sensor, int accuracy) {        // not sure if needed, placeholder just in case    }    public void onSensorChanged(SensorEvent event) {        switch(event.sensor.getType()) {            case Sensor.TYPE_ACCELEROMETER:                // copy new accelerometer data into accel array and calculate orientation                DbgLog.msg("FTC4855 - Accelerometer Sensor Event");                System.arraycopy(event.values, 0, accel, 0, 3);                calculateAccMagOrientation();                break;            case Sensor.TYPE_GYROSCOPE:                DbgLog.msg("FTC4855 - Gyroscope Sensor Event");                // process gyro data                gyroFunction(event);                break;            case Sensor.TYPE_MAGNETIC_FIELD:                // copy new magnetometer data into magnet array                DbgLog.msg("FTC4855 - Magnetometer Sensor Event");                System.arraycopy(event.values, 0, magnet, 0, 3);                break;            case Sensor.TYPE_LINEAR_ACCELERATION:                DbgLog.msg("FTC4855 - Linear Acceleration Sensor Event");                System.arraycopy(event.values, 0, linearacceleration, 0, 3);                break;        }        updateDate = new SimpleDateFormat("yyyy/MM/dd HH:mm:ss", Locale.US).format(new Date());        eventTimeStamp = event.timestamp;    }    // calculates orientation angles from accelerometer and magnetometer output    private void calculateAccMagOrientation() {        if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) {            SensorManager.getOrientation(rotationMatrix, accMagOrientation);        }    }    // This function is borrowed from the Android reference    // at http://developer.android.com/reference/android/hardware/SensorEvent.html#values    // It calculates a rotation vector from the gyroscope angular speed values.    private void getRotationVectorFromGyro(float[] gyroValues,                                           float[] deltaRotationVector,                                           float timeFactor)    {        float[] normValues = new float[3];        // Calculate the angular speed of the sample        float omegaMagnitude =                (float)Math.sqrt(gyroValues[0] * gyroValues[0] +                        gyroValues[1] * gyroValues[1] +                        gyroValues[2] * gyroValues[2]);        // Normalize the rotation vector if it's big enough to get the axis        if(omegaMagnitude > EPSILON) {            normValues[0] = gyroValues[0] / omegaMagnitude;            normValues[1] = gyroValues[1] / omegaMagnitude;            normValues[2] = gyroValues[2] / omegaMagnitude;        }        // Integrate around this axis with the angular speed by the timestep        // in order to get a delta rotation from this sample over the timestep        // We will convert this axis-angle representation of the delta rotation        // into a quaternion before turning it into the rotation matrix.        float thetaOverTwo = omegaMagnitude * timeFactor;        float sinThetaOverTwo = (float)Math.sin(thetaOverTwo);        float cosThetaOverTwo = (float)Math.cos(thetaOverTwo);        deltaRotationVector[0] = sinThetaOverTwo * normValues[0];        deltaRotationVector[1] = sinThetaOverTwo * normValues[1];        deltaRotationVector[2] = sinThetaOverTwo * normValues[2];        deltaRotationVector[3] = cosThetaOverTwo;    }    // This function performs the integration of the gyroscope data.    // It writes the gyroscope based orientation into gyroOrientation.    private void gyroFunction(SensorEvent event) {        // don't start until first accelerometer/magnetometer orientation has been acquired        if (accMagOrientation == null)            return;        // initialisation of the gyroscope based rotation matrix        if(initState) {            float[] initMatrix;            initMatrix = getRotationMatrixFromOrientation(accMagOrientation);            float[] test = new float[3];            SensorManager.getOrientation(initMatrix, test);            gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix);            initState = false;        }        // copy the new gyro values into the gyro array        // convert the raw gyro data into a rotation vector        float[] deltaVector = new float[4];        if(timestamp != 0) {            final float dT = (event.timestamp - timestamp) * NS2S;            System.arraycopy(event.values, 0, gyro, 0, 3);            getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);        }        // measurement done, save current time for next interval        timestamp = event.timestamp;        // convert rotation vector into rotation matrix        float[] deltaMatrix = new float[9];        SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);        // apply the new rotation interval on the gyroscope based rotation matrix        gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);        // get the gyroscope based orientation from the rotation matrix        SensorManager.getOrientation(gyroMatrix, gyroOrientation);    }    private float[] getRotationMatrixFromOrientation(float[] o) {        float[] xM = new float[9];        float[] yM = new float[9];        float[] zM = new float[9];        float sinX = (float)Math.sin(o[1]);        float cosX = (float)Math.cos(o[1]);        float sinY = (float)Math.sin(o[2]);        float cosY = (float)Math.cos(o[2]);        float sinZ = (float)Math.sin(o[0]);        float cosZ = (float)Math.cos(o[0]);        // rotation about x-axis (pitch)        xM[0] = 1.0f; xM[1] = 0.0f; xM[2] = 0.0f;        xM[3] = 0.0f; xM[4] = cosX; xM[5] = sinX;        xM[6] = 0.0f; xM[7] = -sinX; xM[8] = cosX;        // rotation about y-axis (roll)        yM[0] = cosY; yM[1] = 0.0f; yM[2] = sinY;        yM[3] = 0.0f; yM[4] = 1.0f; yM[5] = 0.0f;        yM[6] = -sinY; yM[7] = 0.0f; yM[8] = cosY;        // rotation about z-axis (azimuth)        zM[0] = cosZ; zM[1] = sinZ; zM[2] = 0.0f;        zM[3] = -sinZ; zM[4] = cosZ; zM[5] = 0.0f;        zM[6] = 0.0f; zM[7] = 0.0f; zM[8] = 1.0f;        // rotation order is y, x, z (roll, pitch, azimuth)        float[] resultMatrix = matrixMultiplication(xM, yM);        resultMatrix = matrixMultiplication(zM, resultMatrix);        return resultMatrix;    }    private float[] matrixMultiplication(float[] A, float[] B) {        float[] result = new float[9];        result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];        result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];        result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];        result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];        result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];        result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];        result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];        result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];        result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];        return result;    }    private class calculateFusedOrientationTask extends TimerTask {        public void run() {            float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT;            /*             * Fix for 179∞ <--> -179∞ transition problem:             * Check whether one of the two orientation angles (gyro or accMag) is negative while the other one is positive.             * If so, add 360∞ (2 * math.PI) to the negative value, perform the sensor fusion, and remove the 360∞ from the result             * if it is greater than 180∞. This stabilizes the output in positive-to-negative-transition cases.             */            DbgLog.msg("FTC4855 - FusedOrientationTask timer fired");            // azimuth            if (gyroOrientation[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) {                fusedOrientation[0] = (float) (FILTER_COEFFICIENT * (gyroOrientation[0] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[0]);                fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;            }            else if (accMagOrientation[0] < -0.5 * Math.PI && gyroOrientation[0] > 0.0) {                fusedOrientation[0] = (float) (FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * (accMagOrientation[0] + 2.0 * Math.PI));                fusedOrientation[0] -= (fusedOrientation[0] > Math.PI)? 2.0 * Math.PI : 0;            }            else {                fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0];            }            // pitch            if (gyroOrientation[1] < -0.5 * Math.PI && accMagOrientation[1] > 0.0) {                fusedOrientation[1] = (float) (FILTER_COEFFICIENT * (gyroOrientation[1] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[1]);                fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0;            }            else if (accMagOrientation[1] < -0.5 * Math.PI && gyroOrientation[1] > 0.0) {                fusedOrientation[1] = (float) (FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * (accMagOrientation[1] + 2.0 * Math.PI));                fusedOrientation[1] -= (fusedOrientation[1] > Math.PI)? 2.0 * Math.PI : 0;            }            else {                fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * accMagOrientation[1];            }            // roll            if (gyroOrientation[2] < -0.5 * Math.PI && accMagOrientation[2] > 0.0) {                fusedOrientation[2] = (float) (FILTER_COEFFICIENT * (gyroOrientation[2] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[2]);                fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0;            }            else if (accMagOrientation[2] < -0.5 * Math.PI && gyroOrientation[2] > 0.0) {                fusedOrientation[2] = (float) (FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * (accMagOrientation[2] + 2.0 * Math.PI));                fusedOrientation[2] -= (fusedOrientation[2] > Math.PI)? 2.0 * Math.PI : 0;            }            else {                fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * accMagOrientation[2];            }            // overwrite gyro matrix and orientation with fused orientation            // to comensate gyro drift            gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);            System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);            DbgLog.msg("FTC4855 - FusedOrientationTask timer done");        }    }    public void appendLog(String text)    {        File logFile = new File("sdcard/ftc_4855_sensor_log.file");        if (!logFile.exists())        {            try            {                //noinspection ResultOfMethodCallIgnored                logFile.createNewFile();            }            catch (IOException e)            {                // TODO Auto-generated catch block                e.printStackTrace();            }        }        try        {            //BufferedWriter for performance, true to set append to file flag            BufferedWriter buf = new BufferedWriter(new FileWriter(logFile, true));            buf.append(text);            buf.newLine();            buf.close();        }        catch (IOException e)        {            // TODO Auto-generated catch block            e.printStackTrace();        }    }}

      Comment


      • #4
        The app context can be accessed from an OpMode through the HardwareMap:
        Code:
        hardwareMap.appContext

        Comment

        Working...
        X