Components and supplies
6 DOF Sensor - MPU6050
WS2812 Addressable LED Strip
Arduino Nano R3
Project description
Code
MotionSensor Repository
SweetMaker Core
StrawberryString Repository
SweetMaker Core
StrawberryString Repository
StrarberryString Example
c_cpp
1/******************************************************************************* 2LightHat.ino - A Motion controlled Light Hat using StrawberryString 3 4Copyright(C) 2017-2022 Howard James May 5 6This program is free software : you can redistribute it and / or modify 7it under the terms of the GNU General Public License as published by 8the Free Software Foundation, either version 3 of the License, or 9(at your option) any later version. 10 11This program is distributed in the hope that it will be useful, 12but WITHOUT ANY WARRANTY; without even the implied warranty of 13MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the 14GNU General Public License for more details. 15 16You should have received a copy of the GNU General Public License 17along with this program.If not, see <http://www.gnu.org/licenses/>. 18 19Contact me at sweet.maker@outlook.com 20 21*******************************************************************************/ 22#include <Wire.h> 23#include <EEPROM.h> 24#include <SweetMaker.h> 25#include <MotionSensor.h> 26#include "StrawberryString.h" 27#include "EepromUtility.h" 28#include "SigLib.h" 29 30using namespace SweetMaker; 31 32StrawberryString strStr; 33 34/* 35 * These events are generated by this sketch and also handled by it. 36 */ 37static const uint16_t EVENT_CALLIBRATE = IEventHandler::USER + 0; 38static const uint16_t EVENT_SET_ROT_OFF = IEventHandler::USER + 1; 39static const uint16_t EVENT_PRINT_POSITION = IEventHandler::USER + 2; 40 41void myEventHandler(uint16_t eventId, uint8_t src, uint16_t eventInfo); 42 43/* 44 * Signal generator creating a gentle breating effect 45 */ 46SigGen breatingSigGen; 47 48/* 49 * Hue/Saturation/Value controll of LEDs 50 */ 51ColourHSV myColourHSV[StrawberryString::num_lights]; 52 53/* 54 * Flag to enable some diagnostics 55 */ 56bool diagnosticsOn = false; 57 58/* 59 * Function Prototypes 60 */ 61void processMotionSensorReading(void); 62void handleSerialInput(void); 63 64 65/* 66 * Responsible for configuring and initialising 67 * - strStr - our StrawberryString 68 * - breathingSigGen - our breath Signal Generator 69 * - The Serial interface 70 * - our HSV pixels 71 */ 72void setup() 73{ 74 /* Start Serial at a speed (Baud rate) of 112500 Bytes per second */ 75 Serial.begin(112500); 76 77 breatingSigGen.configSamples(breathingWave, NUM_SAM(breathingWave), 2000, 0); // period of 2000ms 78 breatingSigGen.start(); // start and repeat forever 79 80 strStr.configEventHandlerCallback(myEventHandler); 81 strStr.init(); 82 83 for(int i=0;i<strStr.num_lights;i++) 84 myColourHSV[i].setColour(123, 255, 255); 85 86 /* 87 * Flush Serial and start timer looking for user input in first 5 seconds 88 */ 89 while (Serial.available()) 90 Serial.read(); 91 92 Serial.println("LightCap"); 93 94} 95 96 97/* 98 * Main loop - runs forever. This uses the SweetMaker framework and so 99 * not much happens here other than updating the StrawberryString LEDs to whatever 100 * value the HSV pixels have been set to. 101 * 102 * The SweetMaker framework is called via 'strStr.update()' this will generate events 103 * which are marshalled to 'myEventHandler'. 104 * 105 */ 106void loop() 107{ 108 /* 109 * Performance Monitor records loop time intervals - these are printed out 110 * when requested on the Serial Input. 111 */ 112 PerfMon::getPerfMon()->intervalStop(); 113 PerfMon::getPerfMon()->intervalStart(); 114 115 /* 116 * Convert from HSV to RGB so the driver can update the LEDs 117 */ 118 for (uint8_t i = 0; i < StrawberryString::num_lights; i++) { 119 ColourConverter::ConvertToRGB(myColourHSV + i, strStr.ledStrip + i); 120 } 121 122 /* 123 * We handle some user commands for calibration and setup 124 */ 125 if (Serial.available()) 126 handleSerialInput(); 127 128 /* 129 * This updates the underlying StrawberryString 130 */ 131 strStr.update(); 132} 133 134/* 135 * myEventHandler - this callback function is called by the SweetMaker framework / StrawberryString 136 * and notifys us when various events have occured. We then choose how to handle them. 137 * some events have been generated by our own code in response to Serial input. 138 */ 139void myEventHandler(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo) 140{ 141 switch (eventId) 142 { 143 case EVENT_CALLIBRATE: { 144 /* 145 * A request to callibrate the StrawberryString Motion Sensor. Each individual MPU6050 motion sensor 146 * requires calibration. The StrawberryString stores the callibration values in EEPROM so 147 * calibration is a one time task. 148 * 149 * When running Calibrating the motion sensor should be level and the MPU6050 chip topmost. Don't move 150 * the Strawberry String until calibration is complete .. about 10 seconds. 151 */ 152 Serial.println("Starting Self Cal"); 153 strStr.recalibrateMotionSensor(); 154 Serial.println("Writing to EEPROM"); 155 break; 156 } 157 158 case EVENT_PRINT_POSITION: { 159 /* 160 * Print out the current position / orientation 161 */ 162 Serial.println("Print Position"); 163 int16_t roll = strStr.motionSensor.rotQuat.getSinRotX(); 164 int16_t pitch = strStr.motionSensor.rotQuat.getSinRotY(); 165 int16_t yaw_16384 = strStr.motionSensor.rotQuat.getSinRotZ(); 166 167 Serial.print(roll); Serial.print(" "); 168 Serial.print(pitch); Serial.print(" "); 169 Serial.print(yaw_16384); Serial.println(); 170 strStr.motionSensor.rotQuat.printQ(); 171 172 break; 173 } 174 175 case TimerTickMngt::TIMER_TICK_100MS: // Generated ten times a second 176 if (diagnosticsOn) { 177 // place Diagnostic Code here 178 } 179 break; 180 181 case TimerTickMngt::TIMER_TICK_S: // This event is called every second 182 // myEventHandler(EVENT_PRINT_POSITION, 0, 0); 183 break; 184 185 case TimerTickMngt::TIMER_TICK_10S: // This event is called every ten seconds 186 break; 187 188 case MotionSensor::MOTION_SENSOR_INIT_ERROR: // This sometimes happens ... best restart 189 Serial.println("MOTION_SENSOR_INIT_ERROR: "); 190 break; 191 192 case MotionSensor::MOTION_SENSOR_READY: 193 Serial.println("MOTION_SENSOR_READY: "); 194 break; 195 196 case MotionSensor::MOTION_SENSOR_RUNTIME_ERROR: 197 Serial.println("Motion Sensor Error"); 198 break; 199 200 case MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY: 201 /* 202 * This is where we do the main work 203 */ 204 processMotionSensorReading(); 205 break; 206 207 208 /* 209 * Other events which we aren't using 210 */ 211 case SigGen::SIG_GEN_STOPPED: // A Signal Generator has been stopped 212 case TimerTickMngt::TIMER_TICK_UPDATE: // Generated every time fizzyMint is updated - could be every 500us (micro seconds) e.g. less than a millisecond 213 case TimerTickMngt::TIMER_FREQ_GEN: // Generated a certain number of times a seconds 214 case TimerTickMngt::TIMER_EXPIRED: // A timer has expired - eventInfo from timerId 215 case SigGen::SIG_GEN_STARTED: // A Signal Generator has been started 216 case SigGen::SIG_GEN_FINISHED: // A Signal Generator has finished 217 break; 218 219 } 220} 221 222 223/* 224 * processMotionSensorReading: - This looks at the StrawberryString Motion sensor and 225 * based on the rotation updates the LEDs 226 */ 227void processMotionSensorReading(void) 228{ 229 /* 230 * Start by calculating the angular velocity by looking at the Rotation Quaternion Delta. 231 * The Square of x, y and z gives the square of the square of sin(dtheta/2) where dTheta is the change 232 * in angle over the last 10ms (presuming the motion sensor is running at 100Hz). 233 * We use this as an indication of the angular velocity. 234 */ 235 RotationQuaternion_16384* rqd = &strStr.motionSensor.rotQuatDelta; 236 uint32_t ang_vel = (uint32_t)rqd->x * (uint32_t)rqd->x + (uint32_t)rqd->y * (uint32_t)rqd->y + (uint32_t)rqd->z * (uint32_t)rqd->z; 237 238 /* 239 * Scale this down so it's a uint8_t 240 */ 241 ang_vel = (ang_vel >> 8); 242 if (ang_vel > 255) 243 ang_vel = 255; 244 245 /* 246 * Get the tilt and scale 247 */ 248 int16_t tilt_forward = strStr.motionSensor.rotQuat.getSinRotY(); 249 tilt_forward = tilt_forward >> 8; 250 251 /* 252 * Get the tilt_side, scale and take absolute value 253 */ 254 int16_t tilt_side = strStr.motionSensor.rotQuat.getSinRotX(); 255 tilt_side = tilt_side >> 8; 256 tilt_side = abs(tilt_side); 257 258 /* 259 * Get the updated 'breathing' value 260 * And use it only if tilting to one side 261 */ 262 uint32_t breathing = breatingSigGen.readValue(); 263 breathing = (breathing * tilt_side) >> 6; 264 265 if (breathing > 200) 266 breathing = 200; 267 268 /* 269 * Using ang_vel_sticky to add a slower decay to the ang_vel 270 */ 271 static uint32_t ang_vel_sticky = 0; 272 if (ang_vel > ang_vel_sticky) 273 ang_vel_sticky = ang_vel; 274 else if (ang_vel_sticky > 4) 275 ang_vel_sticky -= 4; 276 else 277 ang_vel_sticky = 0; 278 279 280 /* 281 * Set the hue to 200 (purple) and vary it depending on the tilt forward 282 */ 283 uint8_t hue = 200 + tilt_forward; 284 285 /* 286 * Set saturation to max but reduce when moving and when 'breathing' 287 */ 288 int16_t saturation = 255 - (ang_vel_sticky /2) - breathing; 289 if (saturation < 0) 290 saturation = 0; 291 292 /* 293 * Set the brightness to 156 but increase when moving. 294 */ 295 uint16_t value = ang_vel_sticky + 156; 296 if (value > 255) 297 value = 255; 298 299 /* 300 * Now update the HSV pixels 301 */ 302 for (int i = 0; i < strStr.num_lights; i++) { 303 myColourHSV[i].hue = hue; 304 myColourHSV[i].saturation = (uint8_t)saturation; 305 myColourHSV[i].value = (uint8_t)value; 306 } 307 308} 309 310 311/* 312 * handleSerialInput - Handles user commands on the serial interface 313 */ 314void handleSerialInput(void) 315{ 316 /* 317 * Default offset rotation for the SweetMaker 318 */ 319 static int rz = -26; // rotation about Z axis 320 321 char c = Serial.read(); 322 switch (c) { 323 case 's': 324 /* Print Out Performance Stats and then reset them */ 325 PerfMon::getPerfMon()->intervalStop(); 326 PerfMon::getPerfMon()->print(); 327 PerfMon::getPerfMon()->reset(); 328 PerfMon::getPerfMon()->intervalStart(); 329 break; 330 331 case 'c': 332 /* Run MPU6050 callibration - make sure sensor is flat */ 333 myEventHandler(EVENT_CALLIBRATE, 0, 0); 334 break; 335 336 case 'p': 337 /* Print Current Orientation */ 338 myEventHandler(EVENT_PRINT_POSITION, 0, 0); 339 break; 340 341 case 'd': 342 if(diagnosticsOn) 343 diagnosticsOn = false; 344 else 345 diagnosticsOn = true; 346 break; 347 348 349 case ',': 350 { 351 /* Decrement Rotation Offset about Z axis and auto-level*/ 352 rz--; 353 Serial.println(rz); 354 strStr.configOffsetRotation(rz); 355 myEventHandler(EVENT_PRINT_POSITION, 0, 0); 356 break; 357 } 358 case '.': 359 { 360 /* Increment Rotation Offset about Z axis and auto-level*/ 361 rz++; 362 Serial.println(rz); 363 strStr.configOffsetRotation(rz); 364 myEventHandler(EVENT_PRINT_POSITION, 0, 0); 365 break; 366 } 367 } 368}
MotionSensor Repository
Downloadable files
StarwberryString Schematic
Erratum: the ICSP MOSI pin should read D11 not 15
StarwberryString Schematic
StarwberryString Schematic
Erratum: the ICSP MOSI pin should read D11 not 15
StarwberryString Schematic
Comments
Only logged in users can leave comments
SweetMaker
0 Followers
•0 Projects
Table of contents
Intro
0
0