/* --------------------------------------------------------------------------- | Filename: compass_single.c | | Author: Klaus Schmitt | | Description: compass demo | ------------------------------------------------------------------------- */ #include "eyebot.h" #include /* The following passage is a duplicate of the routines in vw_inter.c */ /* By the way, these routines should be placed in a file ownmath.c and */ /* made public to be called by user programms */ /* lookuptable for 90 degrees in 360 steps! */ float sintab[90*4+1] = { 0.000000,0.004363,0.008727,0.013090,0.017452,0.021815,0.026177,0.030539, 0.034899,0.039260,0.043619,0.047978,0.052336,0.056693,0.061049,0.065403, 0.069756,0.074108,0.078459,0.082808,0.087156,0.091502,0.095846,0.100188, 0.104528,0.108867,0.113203,0.117537,0.121869,0.126199,0.130526,0.134851, 0.139173,0.143493,0.147809,0.152123,0.156434,0.160743,0.165048,0.169350, 0.173648,0.177944,0.182236,0.186524,0.190809,0.195090,0.199368,0.203642, 0.207912,0.212178,0.216440,0.220697,0.224951,0.229200,0.233445,0.237686, 0.241922,0.246153,0.250380,0.254602,0.258819,0.263031,0.267238,0.271440, 0.275637,0.279829,0.284015,0.288196,0.292372,0.296542,0.300706,0.304864, 0.309017,0.313164,0.317305,0.321439,0.325568,0.329691,0.333807,0.337917, 0.342020,0.346117,0.350207,0.354291,0.358368,0.362438,0.366501,0.370557, 0.374607,0.378649,0.382683,0.386711,0.390731,0.394744,0.398749,0.402747, 0.406737,0.410719,0.414693,0.418660,0.422618,0.426569,0.430511,0.434445, 0.438371,0.442289,0.446198,0.450098,0.453990,0.457874,0.461749,0.465615, 0.469472,0.473320,0.477159,0.480989,0.484810,0.488621,0.492424,0.496217, 0.500000,0.503774,0.507538,0.511293,0.515038,0.518773,0.522499,0.526214, 0.529919,0.533615,0.537300,0.540974,0.544639,0.548293,0.551937,0.555570, 0.559193,0.562805,0.566406,0.569997,0.573576,0.577145,0.580703,0.584250, 0.587785,0.591310,0.594823,0.598325,0.601815,0.605294,0.608761,0.612217, 0.615661,0.619094,0.622515,0.625923,0.629320,0.632705,0.636078,0.639439, 0.642788,0.646124,0.649448,0.652760,0.656059,0.659346,0.662620,0.665882, 0.669131,0.672367,0.675590,0.678801,0.681998,0.685183,0.688355,0.691513, 0.694658,0.697790,0.700909,0.704015,0.707107,0.710185,0.713250,0.716302, 0.719340,0.722364,0.725374,0.728371,0.731354,0.734323,0.737277,0.740218, 0.743145,0.746057,0.748956,0.751840,0.754710,0.757565,0.760406,0.763232, 0.766044,0.768842,0.771625,0.774393,0.777146,0.779884,0.782608,0.785317, 0.788011,0.790690,0.793353,0.796002,0.798636,0.801254,0.803857,0.806445, 0.809017,0.811574,0.814116,0.816642,0.819152,0.821647,0.824126,0.826590, 0.829038,0.831470,0.833886,0.836286,0.838671,0.841039,0.843391,0.845728, 0.848048,0.850352,0.852640,0.854912,0.857167,0.859406,0.861629,0.863836, 0.866025,0.868199,0.870356,0.872496,0.874620,0.876727,0.878817,0.880891, 0.882948,0.884988,0.887011,0.889017,0.891007,0.892979,0.894934,0.896873, 0.898794,0.900698,0.902585,0.904455,0.906308,0.908143,0.909961,0.911762, 0.913545,0.915311,0.917060,0.918791,0.920505,0.922201,0.923880,0.925541, 0.927184,0.928810,0.930418,0.932008,0.933580,0.935135,0.936672,0.938191, 0.939693,0.941176,0.942641,0.944089,0.945519,0.946930,0.948324,0.949699, 0.951057,0.952396,0.953717,0.955020,0.956305,0.957571,0.958820,0.960050, 0.961262,0.962455,0.963630,0.964787,0.965926,0.967046,0.968148,0.969231, 0.970296,0.971342,0.972370,0.973379,0.974370,0.975342,0.976296,0.977231, 0.978148,0.979045,0.979925,0.980785,0.981627,0.982450,0.983255,0.984041, 0.984808,0.985556,0.986286,0.986996,0.987688,0.988362,0.989016,0.989651, 0.990268,0.990866,0.991445,0.992005,0.992546,0.993068,0.993572,0.994056, 0.994522,0.994969,0.995396,0.995805,0.996195,0.996566,0.996917,0.997250, 0.997564,0.997859,0.998135,0.998392,0.998630,0.998848,0.999048,0.999229, 0.999391,0.999534,0.999657,0.999762,0.999848,0.999914,0.999962,0.999990, 1.000000 }; /* approx. sin in the range (-PI <-> PI) */ float own_sin(float x) { int deg; deg = (int)((x*180*4)/M_PI); if(deg<0) { if(deg<-90*4) if(deg<-180*4) return 0; /* out of range */ else return (-sintab[deg+180*4]); else return (-sintab[-deg]); } else { if(deg>90*4) if(deg>180*4) return 0; /* out of range */ else return (sintab[180*4-deg]); else return (sintab[deg]); } } /* approx. cos in the range (-PI <-> PI) */ float own_cos(float x) { int deg; deg = (x*180*4)/M_PI; if(deg<0) { if(deg<-90*4) if(deg<-180*4) return 0; /* out of range */ else return (-sintab[-(deg+90*4)]); else return (sintab[deg+90*4]); } else { if(deg>90*4) if(deg>180*4) return 0; /* out of range */ else return (-sintab[deg-90*4]); else return (sintab[90*4-deg]); } } /* Draws the arrow indicating the direction */ void DrawArrow(int angle, char color) { int p[3][2] = {{-5, 0},{5, 0},{0, 29}}; int pn[3][2]; int i; float mat[2]; float phi; if(angle > 180) angle -= 360; phi = (float)angle*M_PI/-180.0; mat[0] = own_cos(phi); mat[1] = own_sin(phi); for(i=0; i<3; i++) { pn[i][0] = (mat[0]*p[i][0] - mat[1]*p[i][1])*(44.0/32.0) + 61; pn[i][1] = -(mat[1]*p[i][0] + mat[0]*p[i][1]) + 32; } LCDLine(pn[0][0], pn[0][1], pn[1][0], pn[1][1], color); LCDLine(pn[1][0], pn[1][1], pn[2][0], pn[2][1], color); LCDLine(pn[0][0], pn[0][1], pn[2][0], pn[2][1], color); } /* Draws the simple graphics of the compass */ void CompassMenue() { int j; float x,y; LCDClear(); LCDMenu("Cal","","","END"); for(j=-180;j<180;j++) { x=own_cos((float)j*M_PI/180); y=own_sin((float)j*M_PI/180); LCDSetPixel((int)(y*32+32),(int)(x*44+61), 1); if(j%10==0) LCDLine((int)(x*41+61), (int)(y*30+32), (int)(x*44+61), (int)(y*32+32), 1); } LCDSetPos(0,7); LCDPrintf("N"); LCDSetPos(7,7); LCDPrintf("S"); LCDSetPos(4,2); LCDPrintf("W"); LCDSetPos(4,12); LCDPrintf("E"); } /* Calibration Subpart */ void Calibration() { char ch; LCDClear(); LCDPrintf(" Calibration\n -----------\nPlace the Eyebotin 0Deg positionand press Cal!\n"); LCDMenu("","Cal","RST","END"); while(1) { ch = KEYRead(); switch(ch) { case KEY2: /* First Calibration Phase */ COMPASSCalibrate(1); LCDSetPos(2,0); LCDMenu("","Cal"," "," "); LCDPrintf("Now place Eyebotin 180Deg pos. and press Cal! "); ch = 0; do{ ch = KEYRead(); } while(ch != KEY2); /* Second Calibration Phase */ COMPASSCalibrate(1); return; case KEY3: /* Calibration Reset */ COMPASSCalibrate(0); LCDClear(); LCDPrintf(" Calibration\n -----------\nCalibration datais cleared now!"); LCDMenu("","","","END"); ch = 0; do{ ch = KEYRead(); } while(ch != KEY4); return; case KEY4: return; } } } int main() { int j; char ch; COMPASSInit(COMPASS); CompassMenue(); j=0; while(1) { ch = KEYRead(); switch(ch) { case KEY1: Calibration(); CompassMenue(); break; case KEY4: COMPASSRelease(); return 0; default: COMPASSStart(FALSE); while(!COMPASSCheck()); DrawArrow(j, 0); j = COMPASSGet(); DrawArrow(j, 1); LCDSetPos(4,6); LCDPrintf("%.3d",j); break; } } return 0; }