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
