ROBOTC.net forums
http://robotc.net/forums/

Pointing Downwards
http://robotc.net/forums/viewtopic.php?f=15&t=5489
Page 1 of 1

Author:  Coder A [ Tue Feb 12, 2013 9:05 pm ]
Post subject:  Pointing Downwards

This code is pretty self-explanatory; I just want to say the most enjoyable part of producing this code was thinking through the math and seeing the result. If I may say so, interesting code, but rather lame result! It's not without its uses, though.

Code:
//Warning: This code completely doesn't work if the sensor is configured while moving or tilted. Be careful!

/* This code uses various laws and equations of physics to determine the direction downward is.
Downward is pretty vague, what it means for our purposes is the arbitrary source of acceleration.
When at rest, downward is toward the center of the earth, and when moving, the arrow is slightly pointed towards the
direction of the point it is accelerating away from.*/

#pragma config(Sensor, S1,     DIMU,                sensorI2CCustomFastSkipStates)

#include "dexterind-imu.h"

float z_val, x_val, Gforce, pXrads[2], pZrads[2];

// z_val and x_val are the accel values on the Z and X axes.
// Gforce is the total force felt along both axes.
// pXrads and pZrads are arrays holding both possible values
// for the tilt of each axis.

//---------------------------------------------------------------------------------------------------------------------//

/* This finds the x tilt and z tilt that match up and returns that value.
Since a computer may not store exact values, or the sensor could give an imperfect reading,
the second block of code runs if no exact match is found. If no match is still found, it returns zero radians.*/

float match()
{
   if(pXrads[0] == pZrads[0])return pXrads[0];
   if(pXrads[0] == pZrads[1])return pXrads[0];
   if(pXrads[1] == pZrads[0])return pXrads[1];
   if(pXrads[1] == pZrads[1])return pXrads[1];
   if(abs(pXrads[0] - pZrads[0]) < .01)return pXrads[0];
   if(abs(pXrads[0] - pZrads[1]) < .01)return pXrads[0];
   if(abs(pXrads[1] - pZrads[0]) < .01)return pXrads[1];
   if(abs(pXrads[1] - pZrads[1]) < .01)return pXrads[1];
   return 0;
}

//---------------------------------------------------------------------------------------------------------------------//

/* This displays an arrow on the screen pointing downwards.  Note that I wrote this code for degrees, but the rest of
the code is written in radians. This is mainly because I understand degrees better, and it also demonstrates a
conversion, as this is necessary(see line 100).*/

void display_arrow(int degs_from_down)
{
   eraseDisplay();
   degs_from_down = degs_from_down-180;// Otherwise, the arrow would point up.
   nxtDrawLine(49,31,(cosDegrees(degs_from_down)*20)+49,(sinDegrees(degs_from_down)*20)+31);//If you don't
   nxtDrawLine((cosDegrees(degs_from_down-20)*15)+49,(sinDegrees(degs_from_down-20)*15)+31,///know
                     (cosDegrees(degs_from_down)*20)+49,(sinDegrees(degs_from_down)*20)+31);////////trigonometry,
   nxtDrawLine((cosDegrees(degs_from_down+20)*15)+49,(sinDegrees(degs_from_down+20)*15)+31,///ignore
                     (cosDegrees(degs_from_down)*20)+49,(sinDegrees(degs_from_down)*20)+31);////////this part.
}

//---------------------------------------------------------------------------------------------------------------------//

/*This is so all values can be compared for equality. Baically, it makes all the values non-negative.*/

void normalize()
{
   if(pXrads[0]<0)
      pXrads[0] = pXrads[0]+(2*PI);
   if(pXrads[1]<0)
      pXrads[1] = pXrads[1]+(2*PI);
   if(pZrads[0]<0)
      pZrads[0] = pZrads[0]+(2*PI);
   if(pZrads[1]<0)
      pZrads[1] = pZrads[1]+(2*PI);
}

//---------------------------------------------------------------------------------------------------------------------//

task main()
{
   if (!DIMUconfigAccel(DIMU, DIMU_ACC_RANGE_2G))// If configuration fails, the program ends.
   {
      PlaySound(soundException);
     while(bSoundActive){}
     StopAllTasks();
  }

  while(nNxtButtonPressed == -1)
  {
      z_val = DIMUreadAccelZAxis10Bit(DIMU);
      x_val = DIMUreadAccelXAxis10Bit(DIMU);

      /* Since the axes are constantly 90 degrees apart, we can use the sum of forces law,
      which looks like the pythagorean theorem, to discover the total force along both axes.*/
      Gforce = sqrt(pow(z_val,2) + pow(x_val,2));
      
      /* Then we divide both values received by the total force to get numbers on the interval [-1,1].
      This way we can input them into the arcsine and arccosine functions.*/
      z_val = z_val/Gforce;
      x_val = x_val/Gforce;

      pXrads[0] = asin(x_val);
      pXrads[1] = PI-pXrads[0];//other possible X tilt value.
      pZrads[0] = acos(z_val);
      pZrads[1] = -1*pZrads[0];//other possible Z tilt value.
      
      normalize();// See line 57.
      display_arrow(radiansToDegrees(match()));// See line 42.

      wait1Msec(100);// I guess if you want you can get rid of this.
      //It's so the screen doesn't seem to flash with the constant erasing.
   }
}

// Credits:

/*
   Xander Soldaat-    accelerometer sensor driver

   Dexter Industries- accelerometer sensor manufacturer

   LEGO-              MINDSTORMS products

   Robomatter-        development of ROBOTC for LEGO MINDSTORMS

   My Parents-        the providing of these products for my eduction & recreation.
*/



// Avery Radmacher 2013. Use of the above code is granted for all users, provided
// credit to the author is given if used in a competition or commercial product of any type.


The dIMU should be placed on the right side of the nxt, the LED pointing rightward, and the sensor's port pointing to the bottom of the NXT, connected to port 1. Have fun!

-Coder A

Author:  mightor [ Wed Feb 13, 2013 1:21 pm ]
Post subject:  Re: Pointing Downwards

Hey Avery,

Would you mind if I turned this program into an example program to go with my Suite?


= Xander

Author:  Coder A [ Wed Feb 13, 2013 1:55 pm ]
Post subject:  Re: Pointing Downwards

I would be honored! Of course you may. Just remember to include the information on the position of the sensor on the NXT. If you want, you can change the code or delete the credits.

Author:  mightor [ Wed Feb 13, 2013 2:08 pm ]
Post subject:  Re: Pointing Downwards

The file will have the same format as my other examples, but I'll be sure to give credit to you for writing the original code, but maybe not as far as thanking your parents, I'll leave that to you :)

= Xander

Author:  Coder A [ Wed Feb 13, 2013 2:14 pm ]
Post subject:  Re: Pointing Downwards

Don't worry about my parents. Like I said, alter the code however you wish. This is so cool!


Another thing: all of your sample programs have "drivers/dexterind-imu.h" as their driver file location. My computer couldn't locate that file so I changed it to "dexterind-imu.h" and it worked. Make sure you change that part back however you need it.

Author:  mightor [ Wed Feb 13, 2013 2:55 pm ]
Post subject:  Re: Pointing Downwards

Hey Avery,

I have attached the example as I had it in mind to add to the suite. I can't test it because I seem to have misplaced my dIMU somewhere (I'm a very unhappy camper atm). Could please you test it to make sure it works?

Thanks!

= Xander

Attachments:
dexterind-imu-test3.c [5.4 KiB]
Downloaded 605 times

Author:  Coder A [ Wed Feb 13, 2013 3:15 pm ]
Post subject:  Re: Pointing Downwards

Everything worked. Just make sure that users know how to position the accel on the NXT, as this does affect the program and is different from configuration. Also line 158 has an error: I don't think the Netherlands are still in December!

You're welcome!

-Coder A

Author:  mightor [ Wed Feb 13, 2013 3:23 pm ]
Post subject:  Re: Pointing Downwards

Coder A wrote:
Everything worked. Just make sure that users know how to position the accel on the NXT, as this does affect the program and is different from configuration. Also line 158 has an error: I don't think the Netherlands are still in December!

I've added the additional usage directions. As for the date, it's not a mistake, the info is automatically updated when I check the code into my Subversion Repository. This particular timestamp is from test2, I copied some of the stuff from it :)
For more information on SVN and tags, check out this book here: http://svnbook.red-bean.com/ . I use SVN to keep track of changes I make to my code. When I am happy with a specific version, I tag it and create a release.

= Xander

Author:  Coder A [ Wed Feb 13, 2013 3:57 pm ]
Post subject:  Re: Pointing Downwards

Thanks for doing all this. If you have any more questions ask and I'll do my best to answer.

Author:  Coder A [ Sat Mar 16, 2013 8:13 pm ]
Post subject:  Re: Pointing Downwards

It has been a while, but here is some new code:

Code:
#pragma config(Sensor, S1,     DIMU,           sensorI2CCustomFastSkipStates)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
 * $Id: dexterind-imu-test3.c 133 2013-03-10 15:15:38Z xander $
 */

/**
 * Warning: This code completely doesn't work if the sensor is configured while moving or tilted.
 * This code uses various laws and equations of physics to determine the direction downward is.
 * Downward is pretty vague, what it means for our purposes is the arbitrary source of acceleration.
 * When at rest, downward is toward the center of the earth, and when moving, the arrow is slightly
 * pointed towards the direction of the point it is accelerating away from.
 *
 * The dIMU should be placed on the right side of the nxt, the LED pointing rightward, and the
 * sensor's port pointing to the bottom of the NXT, connected to port 1.
 *
 * Changelog:
 * - 0.1: Initial release
 *
 * Credits:
 * - Avery Radmacher for writing the original code and letting me include it with the Suite!
 * - A shout out to his parents for giving him cool toys like the LEGO MINDSTORMS NXT
 * - Big thanks to Dexter Industries for providing me with the hardware necessary to write and test this.
 *
 * License: You may use this code as you wish, provided you give credit where it's due.
 *
 * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER.

 * Written by Avery Radmacher (Coder A on the ROBOTC Forums)
 * Modified by Xander Soldaat (xander_at_botbench.com)
 * 13 February 2013
 * version 0.1
 */

#include "dexterind-imu.h"

// z_val and x_val are the accel values on the Z and X axes.
// Gforce is the total force felt along both axes.

float z_val;
float x_val;
float Gforce;


/**
 * This displays an arrow on the screen pointing downwards.
 * @param degreesFromDown the number of degrees from down
 */
void displayArrow(int degreesFromDown)
{
   eraseDisplay();
   // Otherwise, the arrow would point up.
   degreesFromDown = degreesFromDown-180;

   //If you don't know trigonometry, you can ignore this part
   nxtDrawLine(49,
               31,
               round((float)(cosDegrees(degreesFromDown     ) * 20 * Gforce)) + 49,
               round((float)(sinDegrees(degreesFromDown     ) * 20 * Gforce)) + 31);
   nxtDrawLine(round((float)(cosDegrees(degreesFromDown - 20) * 15 * Gforce)) + 49,
               round((float)(sinDegrees(degreesFromDown - 20) * 15 * Gforce)) + 31,
               round((float)(cosDegrees(degreesFromDown     ) * 20 * Gforce)) + 49,
               round((float)(sinDegrees(degreesFromDown     ) * 20 * Gforce)) + 31);
   nxtDrawLine(round((float)(cosDegrees(degreesFromDown + 20) * 15 * Gforce)) + 49,
               round((float)(sinDegrees(degreesFromDown + 20) * 15 * Gforce)) + 31,
               round((float)(cosDegrees(degreesFromDown     ) * 20 * Gforce)) + 49,
               round((float)(sinDegrees(degreesFromDown     ) * 20 * Gforce)) + 31);
}


/**
 * The main task
 */
task main(){

  nxtDisplayCenteredTextLine(0, "Dexter Ind.");
  nxtDisplayCenteredBigTextLine(1, "IMU");
  nxtDisplayCenteredTextLine(3, "Test 3");
  nxtDisplayCenteredTextLine(5, "Connect sensor");
  nxtDisplayCenteredTextLine(6, "to S1");
  wait1Msec(2000);
  eraseDisplay();

  // If configuration fails, the program ends.
  if (!DIMUconfigAccel(DIMU, DIMU_ACC_RANGE_2G))
  {
      PlaySound(soundException);
      while(bSoundActive){}
      StopAllTasks();
  }

  while(nNxtButtonPressed == kNoButton)
  {
      z_val = DIMUreadAccelZAxis10Bit(DIMU);
      x_val = DIMUreadAccelXAxis10Bit(DIMU);

      // Since the axes are constantly 90 degrees apart, we can use the sum of forces law,
      // which looks like the pythagorean theorem, to discover the total force along both axes.
      Gforce = sqrt(pow(z_val, 2) + pow(x_val, 2));
      displayArrow(z_val == 0 ? (x_val < 0 ?270:90) :
                         z_val < 0 ? round(radiansToDegrees(atan(x_val/z_val)))+180 : round(radiansToDegrees(atan(x_val/z_val)))
                         );

      // This stops the screen from flashing.
      wait1Msec(100);
  }
}


/*
 * $Id: dexterind-imu-test3.c 133 2013-03-10 15:15:38Z xander $
 */



What got changed:

1. I originally used the arcsine and arccosine functions and lots of additional math to determine downwards. Later, I realized that arctangent did most of the work. That got rid of lots of the extra math.
2. The arrow now changes size based on the total force felt. It gets smaller when the nxt is laid down on a table flatly and gets larger when the nxt is completely upright.

The most important thing is that the code has been much simplified.

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/