View unanswered posts | View active topics It is currently Tue Feb 09, 2016 6:03 am

 Page 1 of 1 [ 10 posts ]
 Print view Previous topic | Next topic
Pointing Downwards
Author Message
Moderator

Joined: Thu Jan 03, 2013 5:10 pm
Posts: 202
Location: The plateau north of the Ohio River Valley, also known as Cave Country.
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 ofthe code is written in radians. This is mainly because I understand degrees better, and it also demonstrates aconversion, 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
Site Admin

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

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

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

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

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

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

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

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

Joined: Thu Jan 03, 2013 5:10 pm
Posts: 202
Location: The plateau north of the Ohio River Valley, also known as Cave Country.
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
Display posts from previous:  Sort by
 Page 1 of 1 [ 10 posts ]

#### Who is online

Users browsing this forum: No registered users and 1 guest

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

Search for:
 Jump to:  Select a forum ------------------ ROBOTC Applications    ROBOTC for LEGO MINDSTORMS       Third-party sensors    ROBOTC for CORTEX & PIC    ROBOTC for VEX IQ    ROBOTC for Arduino    Robot Virtual Worlds    Multi-Robot Communications    Issues and Bugs Competitions & Partners    Mini Urban Challenge    2014 Robotics Academy Summer of Learning    CS2N Robot Virtual Worlds Competitions       VEX Skyrise Competition 2014-2015       VEX Toss Up 2013-2014       FTC Block Party! 2013-2014    Competitions using VEX - BEST, TSA, VEX, and RoboFest!    FTC Programming    RoboCup Junior and Other ROBOT Competitions Virtual Brick Robotics Discussions    General Discussions    Project Discussions Off-Topic ROBOTC Forum & ROBOTC.net Suggestions/Feedback    ROBOTC Forums Suggestions/Comments    ROBOTC.net Suggestions/Comments       NXT Programming: Tips for Beginning with ROBOTC       VEX Programming: Tips for Beginning with ROBOTC    2013 Robotics Summer Of Learning       VEX Toss Up Programming Challenge       FTC Ring It Up! Programming Challenge    International Forums       Spanish Forums          ROBOTC for MINDSTORMS          ROBOTC for VEX       French Forums          ROBOTC pour Mindstorms          ROBOTC pour IFI VEX       Japanese Forums （日本語のフォーラム）       German Forums    2015 Spring Carnival Event    PLTW (Project Lead The Way)    Robotics Merit Badge

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