View unanswered posts | View active topics It is currently Thu Dec 18, 2014 8:56 pm






Reply to topic  [ 10 posts ] 
Pointing Downwards 
Author Message
Moderator
Moderator

Joined: Thu Jan 03, 2013 5:10 pm
Posts: 198
Location: The plateau north of the Ohio River Valley, also known as Cave Country.
Post 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

_________________
I'm not a robot! I'm british! ~ quote from an asparagus
I am not a robot! I am a unicorn! ~ quote from a robot


Last edited by Coder A on Wed Feb 13, 2013 2:07 pm, edited 2 times in total.



Tue Feb 12, 2013 9:05 pm
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3297
Location: Rotterdam, The Netherlands
Post Re: Pointing Downwards
Hey Avery,

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


= Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Wed Feb 13, 2013 1:21 pm
Profile WWW
Moderator
Moderator

Joined: Thu Jan 03, 2013 5:10 pm
Posts: 198
Location: The plateau north of the Ohio River Valley, also known as Cave Country.
Post 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.

_________________
I'm not a robot! I'm british! ~ quote from an asparagus
I am not a robot! I am a unicorn! ~ quote from a robot


Wed Feb 13, 2013 1:55 pm
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3297
Location: Rotterdam, The Netherlands
Post 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

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Wed Feb 13, 2013 2:08 pm
Profile WWW
Moderator
Moderator

Joined: Thu Jan 03, 2013 5:10 pm
Posts: 198
Location: The plateau north of the Ohio River Valley, also known as Cave Country.
Post 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.

_________________
I'm not a robot! I'm british! ~ quote from an asparagus
I am not a robot! I am a unicorn! ~ quote from a robot


Wed Feb 13, 2013 2:14 pm
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3297
Location: Rotterdam, The Netherlands
Post 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 336 times

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]
Wed Feb 13, 2013 2:55 pm
Profile WWW
Moderator
Moderator

Joined: Thu Jan 03, 2013 5:10 pm
Posts: 198
Location: The plateau north of the Ohio River Valley, also known as Cave Country.
Post 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

_________________
I'm not a robot! I'm british! ~ quote from an asparagus
I am not a robot! I am a unicorn! ~ quote from a robot


Wed Feb 13, 2013 3:15 pm
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3297
Location: Rotterdam, The Netherlands
Post 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

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Wed Feb 13, 2013 3:23 pm
Profile WWW
Moderator
Moderator

Joined: Thu Jan 03, 2013 5:10 pm
Posts: 198
Location: The plateau north of the Ohio River Valley, also known as Cave Country.
Post Re: Pointing Downwards
Thanks for doing all this. If you have any more questions ask and I'll do my best to answer.

_________________
I'm not a robot! I'm british! ~ quote from an asparagus
I am not a robot! I am a unicorn! ~ quote from a robot


Wed Feb 13, 2013 3:57 pm
Profile
Moderator
Moderator

Joined: Thu Jan 03, 2013 5:10 pm
Posts: 198
Location: The plateau north of the Ohio River Valley, also known as Cave Country.
Post 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.

_________________
I'm not a robot! I'm british! ~ quote from an asparagus
I am not a robot! I am a unicorn! ~ quote from a robot


Sat Mar 16, 2013 8:13 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 10 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.