Diff: main.cpp

Revision:

0:2c66000e0791

Child:

1:c54fc6087d0d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 01 09:15:27 2017 +0000
@@ -0,0 +1,301 @@
+/*
+    STM32 - Magnetic light - Updated version to work with ST components. (MCU ,  Magnetic sensor)
+    Arkadiraf@gmail.com  - 25/01/2017
+*/
+
+/*
+ Hardware setup: STM32 - Nucleo-F432KC
+ Led strip 74 leds ws2812b 60 leds / meter
+ 5V  -  5V
+ D11 -  Led Din (through voltage converter 3v3->5V)
+ GND -  GND
+
+ Magnetic sensor LIS
+ i2c
+*/
+
+///////////////
+// Libraries //
+///////////////
+#include "mbed.h"
+#include "PololuLedStrip.h"
+
+//////////////
+// Defines: //
+//////////////
+#define DEBBUG_MSG1
+#define LED_COUNT 74
+#define blackColor {0,0,0}
+#define SAMPLEDELAY 30 // roughly 30 hz // magnetic sensor sample rate / Led update
+////////////////////
+// define Objects //
+////////////////////
+PololuLedStrip ledStrip(D11);
+Timer timer;
+Serial pc(SERIAL_TX, SERIAL_RX,57600);
+///////////////
+// variables //
+///////////////
+
+// LED variables:
+rgb_color colors[LED_COUNT];
+rgb_color setColor= {0,0,0};
+
+// define array of pixel vectors based on the compas:
+float Pixel_Vect_Float[3]= {0,0,0}; // variable to store LED Power
+const int Pixel_Vect[LED_COUNT][3]= {
+    {  -41,  -86,  -31 },
+    {  -54,  -71,  -45 },
+    {  -66,  -47,  -58 },
+    {  -75,  -18,  -63 },
+    {  -76,  17,  -63 },
+    {  -68,  44,  -58 },
+    {  -56,  67,  -48 },
+    {  -41,  86,  -32 },
+    {  -35,  94,  -2 },
+    {  -61,  79,  -4 },
+    {  -82,  57,  -4 },
+    {  -99,  10,  -10 },
+    {  -97,  -20,  -6 },
+    {  -87,  -47,  -6 },
+    {  -68,  -72,  -7 },
+    {  -24,  -97,  -3 },
+    {  -50,  -81,  30 },
+    {  -66,  -63,  41 },
+    {  -80,  -37,  49 },
+    {  -85,  0,  53 },
+    {  -80,  32,  51 },
+    {  -71,  56,  42 },
+    {  -50,  79,  33 },
+    {  -30,  94,  19 },
+    {  -20,  94,  29 },
+    {  -25,  81,  54 },
+    {  -34,  54,  78 },
+    {  -36,  24,  90 },
+    {  -36,  -15,  92 },
+    {  -34,  -48,  81 },
+    {  -25,  -69,  68 },
+    {  -23,  -83,  54 },
+    {  22,  -86,  46 },
+    {  35,  -72,  60 },
+    {  44,  -38,  80 },
+    {  53,  -8,  86 },
+    {  45,  31,  83 },
+    {  35,  62,  70 },
+    {  24,  84,  50 },
+    {  10,  96,  24 },
+    {  42,  91,  12 },
+    {  68,  71,  19 },
+    {  85,  44,  24 },
+    {  95,  8,  28 },
+    {  91,  -32,  28 },
+    {  80,  -55,  25 },
+    {  62,  -75,  20 },
+    {  50,  -84,  21 },
+    {  48,  -86,  -19 },
+    {  67,  -70,  -25 },
+    {  83,  -51,  -26 },
+    {  93,  -21,  -31 },
+    {  94,  13,  -30 },
+    {  85,  43,  -29 },
+    {  67,  70,  -25 },
+    {  42,  90,  -16 },
+    {  19,  91,  -35 },
+    {  38,  72,  -58 },
+    {  48,  45,  -74 },
+    {  55,  20,  -81 },
+    {  55,  -20,  -81 },
+    {  47,  -54,  -70 },
+    {  31,  -76,  -56 },
+    {  20,  -87,  -43 },
+    {  -10,  -88,  -45 },
+    {  -14,  -73,  -67 },
+    {  -13,  -50,  -85 },
+    {  -17,  -14,  -98 },
+    {  -12,  17,  -98 },
+    {  -14,  50,  -86 },
+    {  -13,  72,  -68 },
+    {  -9,  89,  -47 },
+    {  -12,  99,  -10 },
+    {  -12,  99,  -10 }
+};
+
+
+
+// Magnetic sensor:
+float Mag_raw[3]= {1,0,0}; //Magnetometer
+float Mag[3]= {0,0,0};     //Magnetometer
+float Mag_Bias[3]= {0,0,0};     //Magnetometer bias values
+
+float MagOut[3]= {0,0,0}; //Magnetometer
+float LMagOut[3]= {0,0,0}; //Last Magnetometer reading
+float MagIn[3]= {0,0,0}; //Magnetometer
+float LMagIn[3]= {0,0,0}; //Last Magnetometer reading
+float AHPF=0.99;
+
+float Mag_Norm[3]= {0,0,0}; //Magnetometer normalized
+float Mag_ABS=1; // Vector size indication how far the magnet is
+float CosAngle=0; // cosin of the angle between the two vectors
+float LedPower=0; // variable to store LED Power
+uint8_t LedPower_Byte=0; // Byte varible to set led power
+
+// timer variables
+int sampleMillis=0;
+int timeMillis=0;
+
+///////////////
+// Functions //
+///////////////
+// move dot throught the strip
+void dotMove(rgb_color dotColor);
+// update strip colors
+void colorStrip();
+
+////////////////////////
+//  Main Code Setup : //
+////////////////////////
+int main()
+{
+    // init timer
+    timer.start();
+
+    // init magnetometer
+
+
+    //
+
+    ///////////////////////
+    //  Main Code Loop : //
+    ///////////////////////
+    while(1) {
+        timeMillis=timer.read_ms();
+        // check timer overflow // returnes int.
+        if (timeMillis<0) {
+            timer.reset();
+            timeMillis=timer.read_ms();
+            // reset variables
+            sampleMillis=0;
+        }
+
+        // update leds based on magnetometer
+        if (timeMillis-sampleMillis>SAMPLEDELAY) {
+            sampleMillis=timeMillis;
+
+            // Read magnetometer values
+            Mag_raw[0]=0;
+            Mag_raw[1]=1000;
+            Mag_raw[2]=0;
+
+            ////////////////////////////
+            // bias samples and scale //
+            ////////////////////////////
+            Mag[0]=Mag_raw[0]-Mag_Bias[0];
+            Mag[1]=Mag_raw[1]-Mag_Bias[1];
+            Mag[2]=Mag_raw[2]-Mag_Bias[2];
+
+            Mag_ABS=sqrt(Mag[0]*Mag[0]+Mag[1]*Mag[1]+Mag[2]*Mag[2]);
+            Mag_Norm[0]=Mag[0]/Mag_ABS;
+            Mag_Norm[1]=Mag[1]/Mag_ABS;
+            Mag_Norm[2]=Mag[2]/Mag_ABS;
+
+            /////////////////
+            // HPF filter: //
+            /////////////////
+//            LMagIn[0]=MagIn[0];
+//            LMagIn[1]=MagIn[1];
+//            LMagIn[2]=MagIn[2];
+//            LMagOut[0]=MagOut[0];
+//            LMagOut[1]=MagOut[1];
+//            LMagOut[2]=MagOut[2];
+//            // update reading
+//            MagIn[0]=Mag_raw[0];
+//            MagIn[1]=Mag_raw[1];
+//            MagIn[2]=Mag_raw[2];
+//            // update filter
+//            MagOut[0]=AHPF*(LMagOut[0]+MagIn[0]-LMagIn[0]);
+//            MagOut[1]=AHPF*(LMagOut[1]+MagIn[1]-LMagIn[1]);
+//            MagOut[2]=AHPF*(LMagOut[2]+MagIn[2]-LMagIn[2]);
+//
+//            // Normalize vector and calculate ABS value
+//            Mag_ABS=sqrt(MagOut[0]*MagOut[0]+MagOut[1]*MagOut[1]+MagOut[2]*MagOut[2]);
+//            Mag_Norm[0]=MagOut[0]/Mag_ABS;
+//            Mag_Norm[1]=MagOut[1]/Mag_ABS;
+//            Mag_Norm[2]=MagOut[2]/Mag_ABS;
+
+
+            // Calculate angle between magnetic vector and LED vectors
+            for (uint16_t ii=0 ; ii<LED_COUNT ; ii++) {
+                Pixel_Vect_Float[0]=((float)Pixel_Vect[ii][0])/100;
+                Pixel_Vect_Float[1]=((float)Pixel_Vect[ii][1])/100;
+                Pixel_Vect_Float[2]=((float)Pixel_Vect[ii][2])/100;
+                CosAngle=Mag_Norm[0]*Pixel_Vect_Float[0] + Mag_Norm[1]*Pixel_Vect_Float[1] + Mag_Norm[2]*Pixel_Vect_Float[2];
+                //LedPower=Mag_ABS*CosAngle*CosAngle*CosAngle*CosAngle*CosAngle;
+                LedPower=Mag_ABS*((float)pow(CosAngle,5));
+                if (LedPower>=0) {
+                    if (LedPower>255) LedPower=255;
+                    LedPower_Byte=(uint8_t)(LedPower);
+                    colors[ii] = (rgb_color) {
+                        LedPower_Byte, 0, 0
+                    };
+                }
+                if (LedPower<0) {
+                    if (LedPower<-255) LedPower=-255;
+                    LedPower_Byte=(uint8_t)(-LedPower);
+                    colors[ii] = (rgb_color) {
+                        0, 0, LedPower_Byte
+                    };
+                }
+            }
+            // Send the colors to the LED strip.
+            ledStrip.write(colors, LED_COUNT);
+            
+            // debug messages:
+            pc.printf("DBG_1: mill: %d , MAG: %.2f , %.2f , %.2f \r\n",sampleMillis,Mag[0],Mag[1],Mag[2]);    
+            pc.printf("DBG_1: mill: %d , MAG: %.2f , %.2f , %.2f \r\n",sampleMillis,Mag_Norm[0],Mag_Norm[1],Mag_Norm[2]);       
+        }// end update pixels based on mag
+
+
+
+        // pixels test
+        if (0) {
+            setColor=(rgb_color) {
+                125,125,0
+            };
+            dotMove(setColor);
+
+            // Send the colors to the LED strip.
+            ledStrip.write(colors, LED_COUNT);
+            wait_ms(10);
+        }
+    }// end main loop
+}// end main
+
+///////////////
+// Functions //
+///////////////
+// move dot throught the strip
+void dotMove(rgb_color dotColor)
+{
+    static int pixelNum=0;
+    colors[pixelNum]=dotColor;
+    if (pixelNum==0) {
+        colors[LED_COUNT-1]=(rgb_color) blackColor;
+    } else {
+        colors[pixelNum-1]=(rgb_color) blackColor;
+    }
+    pixelNum++;
+    pixelNum=pixelNum%LED_COUNT;
+}
+
+// update strip colors
+void colorStrip()
+{
+    // Update the colors array.
+    uint8_t time = timer.read_ms() >> 3;
+    for(uint32_t i = 0; i < LED_COUNT; i++) {
+        uint8_t x = (time - 8*i)%255;
+        colors[i] = (rgb_color) {
+            x, 255 - x, x
+        };
+    }
+}