main.cpp

00001 /*
00002     STM32 - Magnetic light - Updated version to work with ST components. (MCU ,  Magnetic sensor)
00003     Arkadiraf@gmail.com  - 07/02/2017
00004     
00005     Based on my previous arduino version published at:
00006 */
00007 
00008 /*
00009  Hardware setup: STM32 - Nucleo-F432KC
00010  Led strip 74 leds ws2812b 60 leds / meter
00011  5V  -  5V
00012  D11 -  Led Din (through voltage converter 3v3->5V)
00013  GND -  GND
00014 
00015  Magnetic sensor LIS3MDL I2C : https://www.pololu.com/product/2737
00016  3V3 - Vin
00017  GND - GND
00018  D5  - SCL
00019  D4  - SDA 
00020  
00021 */
00022 
00023 ///////////////
00024 // Libraries //
00025 ///////////////
00026 #include "mbed.h"
00027 #include "PololuLedStrip.h"
00028 #include "x_nucleo_iks01a1.h"
00029 //////////////
00030 // Defines: //
00031 //////////////
00032 #define DEBBUG_MSG1
00033 #define LED_COUNT 74
00034 #define blackColor {0,0,0}
00035 #define SAMPLEDELAY 30 // roughly 30 hz // magnetic sensor sample rate / Led update
00036 ////////////////////
00037 // define Objects //
00038 ////////////////////
00039 PololuLedStrip ledStrip(D11);
00040 Timer timer;
00041 Serial pc(SERIAL_TX, SERIAL_RX,57600);
00042 
00043 // Magnetic sensor:
00044 /* Instantiate the expansion board */
00045 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D4, D5);
00046 static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
00047 
00048 ///////////////
00049 // variables //
00050 ///////////////
00051 
00052 // LED variables:
00053 rgb_color colors[LED_COUNT];
00054 rgb_color setColor= {0,0,0};
00055 
00056 // define array of pixel vectors based on the compas:
00057 float Pixel_Vect_Float[3]= {0,0,0}; // variable to store LED Power
00058 const int Pixel_Vect[LED_COUNT][3]= {
00059     {  -41,  -86,  -31 },
00060     {  -54,  -71,  -45 },
00061     {  -66,  -47,  -58 },
00062     {  -75,  -18,  -63 },
00063     {  -76,  17,  -63 },
00064     {  -68,  44,  -58 },
00065     {  -56,  67,  -48 },
00066     {  -41,  86,  -32 },
00067     {  -35,  94,  -2 },
00068     {  -61,  79,  -4 },
00069     {  -82,  57,  -4 },
00070     {  -99,  10,  -10 },
00071     {  -97,  -20,  -6 },
00072     {  -87,  -47,  -6 },
00073     {  -68,  -72,  -7 },
00074     {  -24,  -97,  -3 },
00075     {  -50,  -81,  30 },
00076     {  -66,  -63,  41 },
00077     {  -80,  -37,  49 },
00078     {  -85,  0,  53 },
00079     {  -80,  32,  51 },
00080     {  -71,  56,  42 },
00081     {  -50,  79,  33 },
00082     {  -30,  94,  19 },
00083     {  -20,  94,  29 },
00084     {  -25,  81,  54 },
00085     {  -34,  54,  78 },
00086     {  -36,  24,  90 },
00087     {  -36,  -15,  92 },
00088     {  -34,  -48,  81 },
00089     {  -25,  -69,  68 },
00090     {  -23,  -83,  54 },
00091     {  22,  -86,  46 },
00092     {  35,  -72,  60 },
00093     {  44,  -38,  80 },
00094     {  53,  -8,  86 },
00095     {  45,  31,  83 },
00096     {  35,  62,  70 },
00097     {  24,  84,  50 },
00098     {  10,  96,  24 },
00099     {  42,  91,  12 },
00100     {  68,  71,  19 },
00101     {  85,  44,  24 },
00102     {  95,  8,  28 },
00103     {  91,  -32,  28 },
00104     {  80,  -55,  25 },
00105     {  62,  -75,  20 },
00106     {  50,  -84,  21 },
00107     {  48,  -86,  -19 },
00108     {  67,  -70,  -25 },
00109     {  83,  -51,  -26 },
00110     {  93,  -21,  -31 },
00111     {  94,  13,  -30 },
00112     {  85,  43,  -29 },
00113     {  67,  70,  -25 },
00114     {  42,  90,  -16 },
00115     {  19,  91,  -35 },
00116     {  38,  72,  -58 },
00117     {  48,  45,  -74 },
00118     {  55,  20,  -81 },
00119     {  55,  -20,  -81 },
00120     {  47,  -54,  -70 },
00121     {  31,  -76,  -56 },
00122     {  20,  -87,  -43 },
00123     {  -10,  -88,  -45 },
00124     {  -14,  -73,  -67 },
00125     {  -13,  -50,  -85 },
00126     {  -17,  -14,  -98 },
00127     {  -12,  17,  -98 },
00128     {  -14,  50,  -86 },
00129     {  -13,  72,  -68 },
00130     {  -9,  89,  -47 },
00131     {  -12,  99,  -10 },
00132     {  -12,  99,  -10 }
00133 };
00134 
00135 
00136 
00137 // Magnetic sensor:
00138 int16_t axesRaw[3]; // raw data from magnetic sensor
00139 float Mag_raw[3]= {1,0,0}; //Magnetometer
00140 float Mag[3]= {0,0,0};     //Magnetometer
00141 float Mag_Bias[3]= {0,0,0};     //Magnetometer bias values
00142 
00143 float MagOut[3]= {0,0,0}; //Magnetometer
00144 float LMagOut[3]= {0,0,0}; //Last Magnetometer reading
00145 float MagIn[3]= {0,0,0}; //Magnetometer
00146 float LMagIn[3]= {0,0,0}; //Last Magnetometer reading
00147 float AHPF=0.99;
00148 
00149 float Mag_Norm[3]= {0,0,0}; //Magnetometer normalized
00150 float Mag_ABS=1; // Vector size indication how far the magnet is
00151 float CosAngle=0; // cosin of the angle between the two vectors
00152 float LedPower=0; // variable to store LED Power
00153 uint8_t LedPower_Byte=0; // Byte varible to set led power
00154 
00155 // timer variables
00156 int sampleMillis=0;
00157 int timeMillis=0;
00158 
00159 ///////////////
00160 // Functions //
00161 ///////////////
00162 // HPF filter:
00163 void HighPassFilter();
00164 // Update Pixel array based on mag sensor
00165 void Update_Pixels_Mag();
00166 // move dot throught the strip
00167 void dotMove(rgb_color dotColor);
00168 // update strip colors
00169 void colorStrip();
00170 
00171 ////////////////////////
00172 //  Main Code Setup : //
00173 ////////////////////////
00174 int main()
00175 {
00176     // init timer
00177     timer.start();
00178 
00179     // init magnetometer - Rewrite library or add options to change magnetic full scale and sample rate
00180     // read magnetic sensor ID
00181     uint8_t id;
00182     magnetometer->ReadID(&id);
00183     pc.printf("LIS3MDL magnetometer              = 0x%X\r\n", id);
00184 
00185     ///////////////////////
00186     //  Main Code Loop : //
00187     ///////////////////////
00188     while(1) {
00189         timeMillis=timer.read_ms();
00190         // check timer overflow // returnes int.
00191         if (timeMillis<0) {
00192             timer.reset();
00193             timeMillis=timer.read_ms();
00194             // reset variables
00195             sampleMillis=0;
00196         }
00197 
00198         // update leds based on magnetometer
00199         if (timeMillis-sampleMillis>SAMPLEDELAY) {
00200             sampleMillis=timeMillis;
00201 
00202             // Read magnetometer values
00203             magnetometer->Get_M_AxesRaw(axesRaw);
00204             // debbug messages
00205             //pc.printf("LIS3MDL [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", axesRaw[0], axesRaw[1], axesRaw[2]);
00206             
00207             // update float ariables
00208             // axis adjustment to fit the previous setup, (visual aligment).
00209             Mag_raw[0]=-axesRaw[1];
00210             Mag_raw[1]=axesRaw[0];
00211             Mag_raw[2]=-axesRaw[2];
00212 
00213             // bias samples and scale
00214             Mag[0]=Mag_raw[0]-Mag_Bias[0];
00215             Mag[1]=Mag_raw[1]-Mag_Bias[1];
00216             Mag[2]=Mag_raw[2]-Mag_Bias[2];
00217 
00218             Mag_ABS=sqrt(Mag[0]*Mag[0]+Mag[1]*Mag[1]+Mag[2]*Mag[2]);
00219             Mag_Norm[0]=Mag[0]/Mag_ABS;
00220             Mag_Norm[1]=Mag[1]/Mag_ABS;
00221             Mag_Norm[2]=Mag[2]/Mag_ABS;
00222 
00223             // HPF filter:
00224             HighPassFilter();
00225 
00226             // Update Pixel array based on mag sensor
00227             Update_Pixels_Mag();
00228             
00229             // Send the colors to the LED strip.
00230             ledStrip.write(colors, LED_COUNT);
00231 
00232             // debug messages:
00233             //pc.printf("DBG_1: mill: %d , MAG: %.2f , %.2f , %.2f \r\n",sampleMillis,Mag[0],Mag[1],Mag[2]);
00234             //pc.printf("DBG_1: mill: %d , MAG: %.2f , %.2f , %.2f \r\n",sampleMillis,Mag_Norm[0],Mag_Norm[1],Mag_Norm[2]);
00235         }// end update pixels based on mag
00236 
00237 
00238 
00239         // pixels test
00240         if (0) {
00241             setColor=(rgb_color) {
00242                 125,125,0
00243             };
00244             dotMove(setColor);
00245 
00246             // Send the colors to the LED strip.
00247             ledStrip.write(colors, LED_COUNT);
00248             wait_ms(10);
00249         }
00250     }// end main loop
00251 }// end main
00252 
00253 ///////////////
00254 // Functions //
00255 ///////////////
00256 // HPF filter:
00257 void HighPassFilter()
00258 {
00259     LMagIn[0]=MagIn[0];
00260     LMagIn[1]=MagIn[1];
00261     LMagIn[2]=MagIn[2];
00262     LMagOut[0]=MagOut[0];
00263     LMagOut[1]=MagOut[1];
00264     LMagOut[2]=MagOut[2];
00265     // update reading
00266     MagIn[0]=Mag_raw[0];
00267     MagIn[1]=Mag_raw[1];
00268     MagIn[2]=Mag_raw[2];
00269     // update filter
00270     MagOut[0]=AHPF*(LMagOut[0]+MagIn[0]-LMagIn[0]);
00271     MagOut[1]=AHPF*(LMagOut[1]+MagIn[1]-LMagIn[1]);
00272     MagOut[2]=AHPF*(LMagOut[2]+MagIn[2]-LMagIn[2]);
00273 
00274     // Normalize vector and calculate ABS value
00275     Mag_ABS=sqrt(MagOut[0]*MagOut[0]+MagOut[1]*MagOut[1]+MagOut[2]*MagOut[2]);
00276     Mag_Norm[0]=MagOut[0]/Mag_ABS;
00277     Mag_Norm[1]=MagOut[1]/Mag_ABS;
00278     Mag_Norm[2]=MagOut[2]/Mag_ABS;
00279 }// end high pass filter
00280 
00281 // Update Pixel array based on mag sensor
00282 void Update_Pixels_Mag()
00283 {
00284     // Calculate angle between magnetic vector and LED vectors
00285     for (uint16_t ii=0 ; ii<LED_COUNT ; ii++) {
00286         Pixel_Vect_Float[0]=((float)Pixel_Vect[ii][0])/100;
00287         Pixel_Vect_Float[1]=((float)Pixel_Vect[ii][1])/100;
00288         Pixel_Vect_Float[2]=((float)Pixel_Vect[ii][2])/100;
00289         CosAngle=Mag_Norm[0]*Pixel_Vect_Float[0] + Mag_Norm[1]*Pixel_Vect_Float[1] + Mag_Norm[2]*Pixel_Vect_Float[2];
00290         //LedPower=Mag_ABS*CosAngle*CosAngle*CosAngle*CosAngle*CosAngle;
00291         LedPower=Mag_ABS*((float)pow(CosAngle,9));
00292         if (LedPower>=0) {
00293             if (LedPower>255) LedPower=255;
00294             LedPower_Byte=(uint8_t)(LedPower);
00295             colors[ii] = (rgb_color) {
00296                 LedPower_Byte, 0, 0
00297             };
00298         }
00299         if (LedPower<0) {
00300             if (LedPower<-255) LedPower=-255;
00301             LedPower_Byte=(uint8_t)(-LedPower);
00302             colors[ii] = (rgb_color) {
00303                 0, 0, LedPower_Byte
00304             };
00305         }
00306     }
00307 }// end pixel update based on magnetic field
00308 
00309 
00310 // move dot throught the strip
00311 void dotMove(rgb_color dotColor)
00312 {
00313     static int pixelNum=0;
00314     colors[pixelNum]=dotColor;
00315     if (pixelNum==0) {
00316         colors[LED_COUNT-1]=(rgb_color) blackColor;
00317     } else {
00318         colors[pixelNum-1]=(rgb_color) blackColor;
00319     }
00320     pixelNum++;
00321     pixelNum=pixelNum%LED_COUNT;
00322 }
00323 
00324 // update strip colors
00325 void colorStrip()
00326 {
00327     // Update the colors array.
00328     uint8_t time = timer.read_ms() >> 3;
00329     for(uint32_t i = 0; i < LED_COUNT; i++) {
00330         uint8_t x = (time - 8*i)%255;
00331         colors[i] = (rgb_color) {
00332             x, 255 - x, x
00333         };
00334     }
00335 }