Arkadi Rafalovich / Mbed 2 deprecated STM32_MagneticLight

MagneticLight - Modified version based on ST Components

Dependencies:   PololuLedStrip X_NUCLEO_IKS01A1 mbed

main.cpp@0:2c66000e0791, 2017-02-01 (annotated)

Committer:
Arkadi
Date:
Wed Feb 01 09:15:27 2017 +0000
Revision:
0:2c66000e0791
Child:
1:c54fc6087d0d
STM32_MagneticLight First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arkadi 0:2c66000e0791 1 /*
Arkadi 0:2c66000e0791 2 STM32 - Magnetic light - Updated version to work with ST components. (MCU , Magnetic sensor)
Arkadi 0:2c66000e0791 3 Arkadiraf@gmail.com - 25/01/2017
Arkadi 0:2c66000e0791 4 */
Arkadi 0:2c66000e0791 5
Arkadi 0:2c66000e0791 6 /*
Arkadi 0:2c66000e0791 7 Hardware setup: STM32 - Nucleo-F432KC
Arkadi 0:2c66000e0791 8 Led strip 74 leds ws2812b 60 leds / meter
Arkadi 0:2c66000e0791 9 5V - 5V
Arkadi 0:2c66000e0791 10 D11 - Led Din (through voltage converter 3v3->5V)
Arkadi 0:2c66000e0791 11 GND - GND
Arkadi 0:2c66000e0791 12
Arkadi 0:2c66000e0791 13 Magnetic sensor LIS
Arkadi 0:2c66000e0791 14 i2c
Arkadi 0:2c66000e0791 15 */
Arkadi 0:2c66000e0791 16
Arkadi 0:2c66000e0791 17 ///////////////
Arkadi 0:2c66000e0791 18 // Libraries //
Arkadi 0:2c66000e0791 19 ///////////////
Arkadi 0:2c66000e0791 20 #include "mbed.h"
Arkadi 0:2c66000e0791 21 #include "PololuLedStrip.h"
Arkadi 0:2c66000e0791 22
Arkadi 0:2c66000e0791 23 //////////////
Arkadi 0:2c66000e0791 24 // Defines: //
Arkadi 0:2c66000e0791 25 //////////////
Arkadi 0:2c66000e0791 26 #define DEBBUG_MSG1
Arkadi 0:2c66000e0791 27 #define LED_COUNT 74
Arkadi 0:2c66000e0791 28 #define blackColor {0,0,0}
Arkadi 0:2c66000e0791 29 #define SAMPLEDELAY 30 // roughly 30 hz // magnetic sensor sample rate / Led update
Arkadi 0:2c66000e0791 30 ////////////////////
Arkadi 0:2c66000e0791 31 // define Objects //
Arkadi 0:2c66000e0791 32 ////////////////////
Arkadi 0:2c66000e0791 33 PololuLedStrip ledStrip(D11);
Arkadi 0:2c66000e0791 34 Timer timer;
Arkadi 0:2c66000e0791 35 Serial pc(SERIAL_TX, SERIAL_RX,57600);
Arkadi 0:2c66000e0791 36 ///////////////
Arkadi 0:2c66000e0791 37 // variables //
Arkadi 0:2c66000e0791 38 ///////////////
Arkadi 0:2c66000e0791 39
Arkadi 0:2c66000e0791 40 // LED variables:
Arkadi 0:2c66000e0791 41 rgb_color colors[LED_COUNT];
Arkadi 0:2c66000e0791 42 rgb_color setColor= {0,0,0};
Arkadi 0:2c66000e0791 43
Arkadi 0:2c66000e0791 44 // define array of pixel vectors based on the compas:
Arkadi 0:2c66000e0791 45 float Pixel_Vect_Float[3]= {0,0,0}; // variable to store LED Power
Arkadi 0:2c66000e0791 46 const int Pixel_Vect[LED_COUNT][3]= {
Arkadi 0:2c66000e0791 47 { -41, -86, -31 },
Arkadi 0:2c66000e0791 48 { -54, -71, -45 },
Arkadi 0:2c66000e0791 49 { -66, -47, -58 },
Arkadi 0:2c66000e0791 50 { -75, -18, -63 },
Arkadi 0:2c66000e0791 51 { -76, 17, -63 },
Arkadi 0:2c66000e0791 52 { -68, 44, -58 },
Arkadi 0:2c66000e0791 53 { -56, 67, -48 },
Arkadi 0:2c66000e0791 54 { -41, 86, -32 },
Arkadi 0:2c66000e0791 55 { -35, 94, -2 },
Arkadi 0:2c66000e0791 56 { -61, 79, -4 },
Arkadi 0:2c66000e0791 57 { -82, 57, -4 },
Arkadi 0:2c66000e0791 58 { -99, 10, -10 },
Arkadi 0:2c66000e0791 59 { -97, -20, -6 },
Arkadi 0:2c66000e0791 60 { -87, -47, -6 },
Arkadi 0:2c66000e0791 61 { -68, -72, -7 },
Arkadi 0:2c66000e0791 62 { -24, -97, -3 },
Arkadi 0:2c66000e0791 63 { -50, -81, 30 },
Arkadi 0:2c66000e0791 64 { -66, -63, 41 },
Arkadi 0:2c66000e0791 65 { -80, -37, 49 },
Arkadi 0:2c66000e0791 66 { -85, 0, 53 },
Arkadi 0:2c66000e0791 67 { -80, 32, 51 },
Arkadi 0:2c66000e0791 68 { -71, 56, 42 },
Arkadi 0:2c66000e0791 69 { -50, 79, 33 },
Arkadi 0:2c66000e0791 70 { -30, 94, 19 },
Arkadi 0:2c66000e0791 71 { -20, 94, 29 },
Arkadi 0:2c66000e0791 72 { -25, 81, 54 },
Arkadi 0:2c66000e0791 73 { -34, 54, 78 },
Arkadi 0:2c66000e0791 74 { -36, 24, 90 },
Arkadi 0:2c66000e0791 75 { -36, -15, 92 },
Arkadi 0:2c66000e0791 76 { -34, -48, 81 },
Arkadi 0:2c66000e0791 77 { -25, -69, 68 },
Arkadi 0:2c66000e0791 78 { -23, -83, 54 },
Arkadi 0:2c66000e0791 79 { 22, -86, 46 },
Arkadi 0:2c66000e0791 80 { 35, -72, 60 },
Arkadi 0:2c66000e0791 81 { 44, -38, 80 },
Arkadi 0:2c66000e0791 82 { 53, -8, 86 },
Arkadi 0:2c66000e0791 83 { 45, 31, 83 },
Arkadi 0:2c66000e0791 84 { 35, 62, 70 },
Arkadi 0:2c66000e0791 85 { 24, 84, 50 },
Arkadi 0:2c66000e0791 86 { 10, 96, 24 },
Arkadi 0:2c66000e0791 87 { 42, 91, 12 },
Arkadi 0:2c66000e0791 88 { 68, 71, 19 },
Arkadi 0:2c66000e0791 89 { 85, 44, 24 },
Arkadi 0:2c66000e0791 90 { 95, 8, 28 },
Arkadi 0:2c66000e0791 91 { 91, -32, 28 },
Arkadi 0:2c66000e0791 92 { 80, -55, 25 },
Arkadi 0:2c66000e0791 93 { 62, -75, 20 },
Arkadi 0:2c66000e0791 94 { 50, -84, 21 },
Arkadi 0:2c66000e0791 95 { 48, -86, -19 },
Arkadi 0:2c66000e0791 96 { 67, -70, -25 },
Arkadi 0:2c66000e0791 97 { 83, -51, -26 },
Arkadi 0:2c66000e0791 98 { 93, -21, -31 },
Arkadi 0:2c66000e0791 99 { 94, 13, -30 },
Arkadi 0:2c66000e0791 100 { 85, 43, -29 },
Arkadi 0:2c66000e0791 101 { 67, 70, -25 },
Arkadi 0:2c66000e0791 102 { 42, 90, -16 },
Arkadi 0:2c66000e0791 103 { 19, 91, -35 },
Arkadi 0:2c66000e0791 104 { 38, 72, -58 },
Arkadi 0:2c66000e0791 105 { 48, 45, -74 },
Arkadi 0:2c66000e0791 106 { 55, 20, -81 },
Arkadi 0:2c66000e0791 107 { 55, -20, -81 },
Arkadi 0:2c66000e0791 108 { 47, -54, -70 },
Arkadi 0:2c66000e0791 109 { 31, -76, -56 },
Arkadi 0:2c66000e0791 110 { 20, -87, -43 },
Arkadi 0:2c66000e0791 111 { -10, -88, -45 },
Arkadi 0:2c66000e0791 112 { -14, -73, -67 },
Arkadi 0:2c66000e0791 113 { -13, -50, -85 },
Arkadi 0:2c66000e0791 114 { -17, -14, -98 },
Arkadi 0:2c66000e0791 115 { -12, 17, -98 },
Arkadi 0:2c66000e0791 116 { -14, 50, -86 },
Arkadi 0:2c66000e0791 117 { -13, 72, -68 },
Arkadi 0:2c66000e0791 118 { -9, 89, -47 },
Arkadi 0:2c66000e0791 119 { -12, 99, -10 },
Arkadi 0:2c66000e0791 120 { -12, 99, -10 }
Arkadi 0:2c66000e0791 121 };
Arkadi 0:2c66000e0791 122
Arkadi 0:2c66000e0791 123
Arkadi 0:2c66000e0791 124
Arkadi 0:2c66000e0791 125 // Magnetic sensor:
Arkadi 0:2c66000e0791 126 float Mag_raw[3]= {1,0,0}; //Magnetometer
Arkadi 0:2c66000e0791 127 float Mag[3]= {0,0,0}; //Magnetometer
Arkadi 0:2c66000e0791 128 float Mag_Bias[3]= {0,0,0}; //Magnetometer bias values
Arkadi 0:2c66000e0791 129
Arkadi 0:2c66000e0791 130 float MagOut[3]= {0,0,0}; //Magnetometer
Arkadi 0:2c66000e0791 131 float LMagOut[3]= {0,0,0}; //Last Magnetometer reading
Arkadi 0:2c66000e0791 132 float MagIn[3]= {0,0,0}; //Magnetometer
Arkadi 0:2c66000e0791 133 float LMagIn[3]= {0,0,0}; //Last Magnetometer reading
Arkadi 0:2c66000e0791 134 float AHPF=0.99;
Arkadi 0:2c66000e0791 135
Arkadi 0:2c66000e0791 136 float Mag_Norm[3]= {0,0,0}; //Magnetometer normalized
Arkadi 0:2c66000e0791 137 float Mag_ABS=1; // Vector size indication how far the magnet is
Arkadi 0:2c66000e0791 138 float CosAngle=0; // cosin of the angle between the two vectors
Arkadi 0:2c66000e0791 139 float LedPower=0; // variable to store LED Power
Arkadi 0:2c66000e0791 140 uint8_t LedPower_Byte=0; // Byte varible to set led power
Arkadi 0:2c66000e0791 141
Arkadi 0:2c66000e0791 142 // timer variables
Arkadi 0:2c66000e0791 143 int sampleMillis=0;
Arkadi 0:2c66000e0791 144 int timeMillis=0;
Arkadi 0:2c66000e0791 145
Arkadi 0:2c66000e0791 146 ///////////////
Arkadi 0:2c66000e0791 147 // Functions //
Arkadi 0:2c66000e0791 148 ///////////////
Arkadi 0:2c66000e0791 149 // move dot throught the strip
Arkadi 0:2c66000e0791 150 void dotMove(rgb_color dotColor);
Arkadi 0:2c66000e0791 151 // update strip colors
Arkadi 0:2c66000e0791 152 void colorStrip();
Arkadi 0:2c66000e0791 153
Arkadi 0:2c66000e0791 154 ////////////////////////
Arkadi 0:2c66000e0791 155 // Main Code Setup : //
Arkadi 0:2c66000e0791 156 ////////////////////////
Arkadi 0:2c66000e0791 157 int main()
Arkadi 0:2c66000e0791 158 {
Arkadi 0:2c66000e0791 159 // init timer
Arkadi 0:2c66000e0791 160 timer.start();
Arkadi 0:2c66000e0791 161
Arkadi 0:2c66000e0791 162 // init magnetometer
Arkadi 0:2c66000e0791 163
Arkadi 0:2c66000e0791 164
Arkadi 0:2c66000e0791 165 //
Arkadi 0:2c66000e0791 166
Arkadi 0:2c66000e0791 167 ///////////////////////
Arkadi 0:2c66000e0791 168 // Main Code Loop : //
Arkadi 0:2c66000e0791 169 ///////////////////////
Arkadi 0:2c66000e0791 170 while(1) {
Arkadi 0:2c66000e0791 171 timeMillis=timer.read_ms();
Arkadi 0:2c66000e0791 172 // check timer overflow // returnes int.
Arkadi 0:2c66000e0791 173 if (timeMillis<0) {
Arkadi 0:2c66000e0791 174 timer.reset();
Arkadi 0:2c66000e0791 175 timeMillis=timer.read_ms();
Arkadi 0:2c66000e0791 176 // reset variables
Arkadi 0:2c66000e0791 177 sampleMillis=0;
Arkadi 0:2c66000e0791 178 }
Arkadi 0:2c66000e0791 179
Arkadi 0:2c66000e0791 180 // update leds based on magnetometer
Arkadi 0:2c66000e0791 181 if (timeMillis-sampleMillis>SAMPLEDELAY) {
Arkadi 0:2c66000e0791 182 sampleMillis=timeMillis;
Arkadi 0:2c66000e0791 183
Arkadi 0:2c66000e0791 184 // Read magnetometer values
Arkadi 0:2c66000e0791 185 Mag_raw[0]=0;
Arkadi 0:2c66000e0791 186 Mag_raw[1]=1000;
Arkadi 0:2c66000e0791 187 Mag_raw[2]=0;
Arkadi 0:2c66000e0791 188
Arkadi 0:2c66000e0791 189 ////////////////////////////
Arkadi 0:2c66000e0791 190 // bias samples and scale //
Arkadi 0:2c66000e0791 191 ////////////////////////////
Arkadi 0:2c66000e0791 192 Mag[0]=Mag_raw[0]-Mag_Bias[0];
Arkadi 0:2c66000e0791 193 Mag[1]=Mag_raw[1]-Mag_Bias[1];
Arkadi 0:2c66000e0791 194 Mag[2]=Mag_raw[2]-Mag_Bias[2];
Arkadi 0:2c66000e0791 195
Arkadi 0:2c66000e0791 196 Mag_ABS=sqrt(Mag[0]*Mag[0]+Mag[1]*Mag[1]+Mag[2]*Mag[2]);
Arkadi 0:2c66000e0791 197 Mag_Norm[0]=Mag[0]/Mag_ABS;
Arkadi 0:2c66000e0791 198 Mag_Norm[1]=Mag[1]/Mag_ABS;
Arkadi 0:2c66000e0791 199 Mag_Norm[2]=Mag[2]/Mag_ABS;
Arkadi 0:2c66000e0791 200
Arkadi 0:2c66000e0791 201 /////////////////
Arkadi 0:2c66000e0791 202 // HPF filter: //
Arkadi 0:2c66000e0791 203 /////////////////
Arkadi 0:2c66000e0791 204 // LMagIn[0]=MagIn[0];
Arkadi 0:2c66000e0791 205 // LMagIn[1]=MagIn[1];
Arkadi 0:2c66000e0791 206 // LMagIn[2]=MagIn[2];
Arkadi 0:2c66000e0791 207 // LMagOut[0]=MagOut[0];
Arkadi 0:2c66000e0791 208 // LMagOut[1]=MagOut[1];
Arkadi 0:2c66000e0791 209 // LMagOut[2]=MagOut[2];
Arkadi 0:2c66000e0791 210 // // update reading
Arkadi 0:2c66000e0791 211 // MagIn[0]=Mag_raw[0];
Arkadi 0:2c66000e0791 212 // MagIn[1]=Mag_raw[1];
Arkadi 0:2c66000e0791 213 // MagIn[2]=Mag_raw[2];
Arkadi 0:2c66000e0791 214 // // update filter
Arkadi 0:2c66000e0791 215 // MagOut[0]=AHPF*(LMagOut[0]+MagIn[0]-LMagIn[0]);
Arkadi 0:2c66000e0791 216 // MagOut[1]=AHPF*(LMagOut[1]+MagIn[1]-LMagIn[1]);
Arkadi 0:2c66000e0791 217 // MagOut[2]=AHPF*(LMagOut[2]+MagIn[2]-LMagIn[2]);
Arkadi 0:2c66000e0791 218 //
Arkadi 0:2c66000e0791 219 // // Normalize vector and calculate ABS value
Arkadi 0:2c66000e0791 220 // Mag_ABS=sqrt(MagOut[0]*MagOut[0]+MagOut[1]*MagOut[1]+MagOut[2]*MagOut[2]);
Arkadi 0:2c66000e0791 221 // Mag_Norm[0]=MagOut[0]/Mag_ABS;
Arkadi 0:2c66000e0791 222 // Mag_Norm[1]=MagOut[1]/Mag_ABS;
Arkadi 0:2c66000e0791 223 // Mag_Norm[2]=MagOut[2]/Mag_ABS;
Arkadi 0:2c66000e0791 224
Arkadi 0:2c66000e0791 225
Arkadi 0:2c66000e0791 226 // Calculate angle between magnetic vector and LED vectors
Arkadi 0:2c66000e0791 227 for (uint16_t ii=0 ; ii<LED_COUNT ; ii++) {
Arkadi 0:2c66000e0791 228 Pixel_Vect_Float[0]=((float)Pixel_Vect[ii][0])/100;
Arkadi 0:2c66000e0791 229 Pixel_Vect_Float[1]=((float)Pixel_Vect[ii][1])/100;
Arkadi 0:2c66000e0791 230 Pixel_Vect_Float[2]=((float)Pixel_Vect[ii][2])/100;
Arkadi 0:2c66000e0791 231 CosAngle=Mag_Norm[0]*Pixel_Vect_Float[0] + Mag_Norm[1]*Pixel_Vect_Float[1] + Mag_Norm[2]*Pixel_Vect_Float[2];
Arkadi 0:2c66000e0791 232 //LedPower=Mag_ABS*CosAngle*CosAngle*CosAngle*CosAngle*CosAngle;
Arkadi 0:2c66000e0791 233 LedPower=Mag_ABS*((float)pow(CosAngle,5));
Arkadi 0:2c66000e0791 234 if (LedPower>=0) {
Arkadi 0:2c66000e0791 235 if (LedPower>255) LedPower=255;
Arkadi 0:2c66000e0791 236 LedPower_Byte=(uint8_t)(LedPower);
Arkadi 0:2c66000e0791 237 colors[ii] = (rgb_color) {
Arkadi 0:2c66000e0791 238 LedPower_Byte, 0, 0
Arkadi 0:2c66000e0791 239 };
Arkadi 0:2c66000e0791 240 }
Arkadi 0:2c66000e0791 241 if (LedPower<0) {
Arkadi 0:2c66000e0791 242 if (LedPower<-255) LedPower=-255;
Arkadi 0:2c66000e0791 243 LedPower_Byte=(uint8_t)(-LedPower);
Arkadi 0:2c66000e0791 244 colors[ii] = (rgb_color) {
Arkadi 0:2c66000e0791 245 0, 0, LedPower_Byte
Arkadi 0:2c66000e0791 246 };
Arkadi 0:2c66000e0791 247 }
Arkadi 0:2c66000e0791 248 }
Arkadi 0:2c66000e0791 249 // Send the colors to the LED strip.
Arkadi 0:2c66000e0791 250 ledStrip.write(colors, LED_COUNT);
Arkadi 0:2c66000e0791 251
Arkadi 0:2c66000e0791 252 // debug messages:
Arkadi 0:2c66000e0791 253 pc.printf("DBG_1: mill: %d , MAG: %.2f , %.2f , %.2f \r\n",sampleMillis,Mag[0],Mag[1],Mag[2]);
Arkadi 0:2c66000e0791 254 pc.printf("DBG_1: mill: %d , MAG: %.2f , %.2f , %.2f \r\n",sampleMillis,Mag_Norm[0],Mag_Norm[1],Mag_Norm[2]);
Arkadi 0:2c66000e0791 255 }// end update pixels based on mag
Arkadi 0:2c66000e0791 256
Arkadi 0:2c66000e0791 257
Arkadi 0:2c66000e0791 258
Arkadi 0:2c66000e0791 259 // pixels test
Arkadi 0:2c66000e0791 260 if (0) {
Arkadi 0:2c66000e0791 261 setColor=(rgb_color) {
Arkadi 0:2c66000e0791 262 125,125,0
Arkadi 0:2c66000e0791 263 };
Arkadi 0:2c66000e0791 264 dotMove(setColor);
Arkadi 0:2c66000e0791 265
Arkadi 0:2c66000e0791 266 // Send the colors to the LED strip.
Arkadi 0:2c66000e0791 267 ledStrip.write(colors, LED_COUNT);
Arkadi 0:2c66000e0791 268 wait_ms(10);
Arkadi 0:2c66000e0791 269 }
Arkadi 0:2c66000e0791 270 }// end main loop
Arkadi 0:2c66000e0791 271 }// end main
Arkadi 0:2c66000e0791 272
Arkadi 0:2c66000e0791 273 ///////////////
Arkadi 0:2c66000e0791 274 // Functions //
Arkadi 0:2c66000e0791 275 ///////////////
Arkadi 0:2c66000e0791 276 // move dot throught the strip
Arkadi 0:2c66000e0791 277 void dotMove(rgb_color dotColor)
Arkadi 0:2c66000e0791 278 {
Arkadi 0:2c66000e0791 279 static int pixelNum=0;
Arkadi 0:2c66000e0791 280 colors[pixelNum]=dotColor;
Arkadi 0:2c66000e0791 281 if (pixelNum==0) {
Arkadi 0:2c66000e0791 282 colors[LED_COUNT-1]=(rgb_color) blackColor;
Arkadi 0:2c66000e0791 283 } else {
Arkadi 0:2c66000e0791 284 colors[pixelNum-1]=(rgb_color) blackColor;
Arkadi 0:2c66000e0791 285 }
Arkadi 0:2c66000e0791 286 pixelNum++;
Arkadi 0:2c66000e0791 287 pixelNum=pixelNum%LED_COUNT;
Arkadi 0:2c66000e0791 288 }
Arkadi 0:2c66000e0791 289
Arkadi 0:2c66000e0791 290 // update strip colors
Arkadi 0:2c66000e0791 291 void colorStrip()
Arkadi 0:2c66000e0791 292 {
Arkadi 0:2c66000e0791 293 // Update the colors array.
Arkadi 0:2c66000e0791 294 uint8_t time = timer.read_ms() >> 3;
Arkadi 0:2c66000e0791 295 for(uint32_t i = 0; i < LED_COUNT; i++) {
Arkadi 0:2c66000e0791 296 uint8_t x = (time - 8*i)%255;
Arkadi 0:2c66000e0791 297 colors[i] = (rgb_color) {
Arkadi 0:2c66000e0791 298 x, 255 - x, x
Arkadi 0:2c66000e0791 299 };
Arkadi 0:2c66000e0791 300 }
Arkadi 0:2c66000e0791 301 }