Advertisement
Guest User

Untitled

a guest
Feb 27th, 2018
216
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 12.09 KB | None | 0 0
  1. #include "config.h"
  2.  
  3. #include <Wire.h>
  4. #include <Adafruit_Sensor.h>
  5. #include <Adafruit_ADXL345_U.h>
  6.  
  7. #include "CircularEase.h"
  8. #include "QuadraticEase.h"
  9. #include "QuinticEase.h"
  10. #include "BounceEase.h"
  11. #include "CubicEase.h"
  12. #include "ExponentialEase.h"
  13. #include "QuarticEase.h"
  14. #include "SineEase.h"
  15. #include "LinearEase.h"
  16.  
  17. CircularEase circular;
  18. QuadraticEase quadratic;
  19. QuinticEase quintic;
  20. BounceEase bounce;
  21. CubicEase cubic;
  22. ExponentialEase exponential;
  23. QuarticEase quartic;
  24. SineEase sine;
  25. LinearEase linear;
  26.  
  27. EasingBase *currentEase;
  28. enum Ease { CircularIn, CircularOut,
  29.             QuadraticIn, QuadraticOut,
  30.             QuinticIn, QuinticOut,
  31.             BounceIn, BounceOut,
  32.             CubicIn, CubicOut,
  33.             ExponentialIn, ExponentialOut,
  34.             QuarticIn, QuarticOut,
  35.             SineIn, SineOut,
  36.             Linear
  37.           };
  38.  
  39. #define RX_WAITING 0
  40. #define RX_UP 1
  41. #define RX_DOWN -1
  42.  
  43. #define FADE_WAITING 0
  44. #define FADE_FADE_TOP 1
  45. #define FADE_FADE_BOTTOM -1
  46.  
  47. #define ACCEL_WAITING 0
  48. #define ACCEL_TRIGGERED 1
  49.  
  50. /* --- Button in lieu of RX --- */
  51. const int rxButton = 2;
  52. int buttonState;
  53. int lastButtonState = HIGH;
  54. unsigned long lastDebounceTime = 0;
  55. unsigned long debounceDelay  = 50;
  56.  
  57. /* --- Adafruit.io --- */
  58. String foot = "l";
  59. AdafruitIO_Feed *pub;
  60. AdafruitIO_Feed *sub;
  61. int rxValue = 0;
  62.  
  63. /* --- Accelerometer CONFIG --- */
  64. const int accelThreshold      = 2;
  65. const int accelDefaultDelay   = 200;
  66.  
  67. /* --- Accelerometer --- */
  68. Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);
  69. int accelCheckDelay = accelDefaultDelay;
  70. int accelPauseDelay = 0;
  71. float xVal = 0.0;
  72. unsigned long previousAccelMillis;
  73. int accelState = ACCEL_WAITING;
  74.  
  75. /* --- Vibration --- */
  76. int fadeState = FADE_WAITING;
  77. int currentIndex = 0;
  78. bool isFadeIn = true;
  79. unsigned long previousFadeMillis;
  80.  
  81. /* --- Vibration CONFIG --- */
  82. int fadeDuration  = 400;
  83. int minPWM = 500;
  84. int maxPWM = 1023;
  85.  
  86. // Specify pins twice, once for in and once for out.
  87. int pinsTop[] = {
  88.   16, 16, 15, 15, 13, 13
  89. };
  90. int pinsBottom[] = {
  91.   12, 12, 14, 14
  92. };
  93.  
  94. // Specifiy type of fade for each pin (in and out)
  95. Ease pinsTopSeq[] = {
  96.   QuinticOut, SineIn, QuinticOut, SineIn, QuinticOut, SineIn
  97. };
  98. Ease pinsBottomSeq[] = {
  99.   QuinticOut, SineIn, QuinticOut, SineIn, QuinticOut, SineIn
  100. };
  101.  
  102. void setup() {
  103.   Serial.begin(115200);
  104.   while (!Serial)   {
  105.     ;
  106.   }
  107.  
  108.   // Start connect to io.adafruit.com´
  109.   Serial.print("Connecting to Adafruit IO");
  110.   io.connect();
  111.  
  112.   if ( foot == "r" ) {
  113.     pub = io.feed("01-user2-r");
  114.     sub = io.feed("01-user1-l");
  115.   } else if (foot == "l" ) {
  116.     pub = io.feed("01-user1-l");
  117.     sub = io.feed("01-user2-r");
  118.   }
  119.  
  120.   // Message RX handler.
  121.   sub->onMessage(handleMessage);
  122.  
  123.   // wait for a connection
  124.   while (io.status() < AIO_CONNECTED) {
  125.     Serial.print(".");
  126.     delay(500);
  127.   }
  128.   // Connected to io.
  129.   Serial.println(io.statusText());
  130.  
  131.   // Start acceleromoter
  132.   if (!accel.begin()) {
  133.     /* There was a problem detecting the ADXL345 ... check your connections */
  134.     Serial.println("No ADXL345 detected ... Check wiring.");
  135.     while (1);
  136.   }
  137.  
  138.   // Set all vibration motor pins to output
  139.   int numPinsT = sizeof(pinsTop) / sizeof(int);
  140.   for (int i = 0; i < numPinsT; i++) {
  141.     pinMode(pinsTop[i], OUTPUT);
  142.   }
  143.   int numPinsB = sizeof(pinsBottom) / sizeof(int);
  144.   for (int i = 0; i < numPinsB; i++) {
  145.     pinMode(pinsBottom[i], OUTPUT);
  146.   }
  147.  
  148.   pinMode(0, OUTPUT);
  149.   digitalWrite(0, HIGH);
  150.   // Dev - Button pin.
  151. //  pinMode(rxButton, INPUT_PULLUP);
  152.  
  153.   pub->save(99);
  154.  
  155. }
  156.  
  157. void loop() {
  158.   checkButton();
  159.  
  160.   // Adafruit io needs this!
  161.   io.run();
  162.  
  163.   // Checks if rx value has changed and starts vibration seq.
  164.   rxVibration();
  165.   // Checks if accel movement is above threshold and send event.
  166.   accelerometer();
  167.  
  168.   delay(10);
  169. }
  170.  
  171. void accelerometer() {
  172.  
  173.   unsigned long currentMillis = millis();
  174.   int currentDelay = currentMillis - previousAccelMillis;
  175.   float xChange;
  176.  
  177.   // Accelerometer Readings
  178.   sensors_event_t event;
  179.   accel.getEvent(&event);
  180.  
  181.   if (accelPauseDelay > 0 && currentDelay >= accelPauseDelay) {
  182.     accelPauseDelay = 0;
  183.   } else if (accelPauseDelay > 0) {
  184.     xVal = event.acceleration.x;
  185.     Serial.println("paused");
  186.     return;
  187.   }
  188.  
  189.   // ACCELEROMETER reading...every {accelCheckDelay}ms
  190.   if ( currentDelay >= accelCheckDelay ) {
  191.  
  192.     // How much has x val changed since last loop.
  193.     xChange = event.acceleration.x - xVal;
  194.  
  195.     if (accelState != ACCEL_WAITING) {
  196.       Serial.println("Accel waiting.");
  197.       pub->save(0);
  198.       accelState = ACCEL_WAITING;
  199.     }
  200.  
  201.     // If change is greater than threshold.
  202.     if (accelState == ACCEL_WAITING && xChange > accelThreshold) {
  203.       Serial.println("Accel movement down. Sending: 1.");
  204.       pub->save(1);
  205.     }
  206.  
  207.     // If change is less than threshold
  208.     if (accelState == ACCEL_WAITING && xChange < 0 - accelThreshold) {
  209.       pub->save(-1);
  210.     }
  211.  
  212.     if ( accelState == ACCEL_WAITING && abs(xChange) > accelThreshold ) {
  213.       accelState = ACCEL_TRIGGERED;
  214.       Serial.println("pause");
  215.       // Increase delay for the next loop. Debouncing.
  216.       accelPauseDelay = 2000;
  217.     }
  218.  
  219.     xVal = event.acceleration.x;
  220.  
  221.     /* Display the results (acceleration is measured in m/s^2) */
  222.     Serial.print("X: "); Serial.print(event.acceleration.x); Serial.println("  ");
  223.     //  Serial.print("Y: "); Serial.print(event.acceleration.y); Serial.print("  ");
  224.     //  Serial.print("Z: "); Serial.print(event.acceleration.z); Serial.print("  "); Serial.println("m/s^2 ");
  225.  
  226.     previousAccelMillis = currentMillis;
  227.   }
  228.  
  229. }
  230.  
  231. void rxVibration() {
  232.  
  233.   double easedPos;
  234.   boolean singleFadeFinished;
  235.   int currentFadeMs;
  236.  
  237.   if (rxValue == RX_WAITING && fadeState == FADE_WAITING) {
  238.     resetPins();
  239.   }
  240.  
  241.   // RX_DOWN=FADE_TOP
  242.   if (rxValue == RX_DOWN && fadeState == FADE_WAITING) {
  243.     Serial.println("START SEQ TOP");
  244.     fadeState = FADE_FADE_TOP;
  245.     configureEase(pinsBottomSeq[currentIndex]);
  246.   }
  247.  
  248.   // RX_UP=FADE_BOTTOM
  249.   if (rxValue == RX_UP && fadeState == FADE_WAITING) {
  250.     Serial.println("START SEQ BOTTOM");
  251.     fadeState = FADE_FADE_BOTTOM;
  252.     configureEase(pinsBottomSeq[currentIndex]);
  253.   }
  254.  
  255.   // What stage in fade are we at & has it finished?
  256.   //  currentFadeMs = millis() - previousFadeMillis
  257.   currentFadeMs = min(int(millis() - previousFadeMillis), fadeDuration);
  258.   singleFadeFinished = (currentFadeMs >= fadeDuration);
  259.  
  260.   int currentPin;
  261.   Ease currentEaseSeq;
  262.   if ( fadeState == FADE_FADE_TOP ) {
  263.     currentPin = pinsTop[currentIndex];
  264.     currentEaseSeq = pinsTopSeq[currentIndex];
  265.   }
  266.  
  267.   if ( fadeState == FADE_FADE_BOTTOM ) {
  268.     currentPin = pinsBottom[currentIndex];
  269.     currentEaseSeq = pinsBottomSeq[currentIndex];
  270.   }
  271.  
  272.   if ( fadeState == FADE_FADE_BOTTOM || fadeState == FADE_FADE_TOP ) {
  273.     easedPos = calculateEase(inOrOut(currentEaseSeq), isFadeIn, currentFadeMs);
  274.     int mappedPos;
  275.     if (isFadeIn == false) {
  276.       mappedPos = map(easedPos, 0, maxPWM, minPWM + 250, maxPWM);
  277.     } else {
  278.       mappedPos = map(easedPos, 0, maxPWM, minPWM, maxPWM);
  279.     }
  280.     analogWrite(currentPin, mappedPos);
  281.     Serial.print("["); Serial.print(currentPin); Serial.print("] "); if (isFadeIn == true) {
  282.       Serial.print(currentFadeMs);
  283.     } else {
  284.       Serial.print(fadeDuration - currentFadeMs);
  285.     };  Serial.print(" - "); Serial.println(mappedPos);
  286.   }
  287.  
  288.   // Fade has finished. Check whether we're through a full sequence yet.
  289.   if ( fadeState != FADE_WAITING && singleFadeFinished == true) {
  290.     int numPins = 0;
  291.     isFadeIn = !isFadeIn;
  292.     if (fadeState == FADE_FADE_TOP) {
  293.       numPins = sizeof(pinsTop) / sizeof(int);
  294.     } else {
  295.       numPins = sizeof(pinsBottom) / sizeof(int);
  296.     }
  297.  
  298.     if ( currentIndex >= numPins - 1 ) {
  299.       currentIndex = 0;
  300.  
  301.       if ( rxValue == RX_WAITING ) {
  302.         fadeState = FADE_WAITING;
  303.         resetPins();
  304.         Serial.println("FADE WAITING");
  305.       }
  306.     } else {
  307.       currentIndex++;
  308.       Serial.println("FINISHED PIN.");
  309.       if (isFadeIn == true) {
  310.         Serial.print("NEXT (in): ");
  311.       } else {
  312.         Serial.print("NEXT (out): ");
  313.       }
  314.       Serial.println(currentIndex);
  315.     }
  316.  
  317.     if (fadeState == FADE_FADE_TOP) {
  318.       configureEase(pinsTopSeq[currentIndex]);
  319.     } else {
  320.       configureEase(pinsBottomSeq[currentIndex]);
  321.     }
  322.  
  323.   }
  324.  
  325. }
  326.  
  327. void resetPins() {
  328.  
  329.   int numPinsT = sizeof(pinsTop) / sizeof(int);
  330.   for (int i = 0; i < numPinsT; i++) {
  331.     analogWrite(pinsTop[i], 0);
  332.   }
  333.   int numPinsB = sizeof(pinsBottom) / sizeof(int);
  334.   for (int i = 0; i < numPinsB; i++) {
  335.     analogWrite(pinsBottom[i], 0);
  336.   }
  337.  
  338.   isFadeIn = true;
  339.  
  340. }
  341.  
  342. double calculateEase(int easeDirection, bool isIn, int t) {
  343.   int _t = (isIn == true) ? t : fadeDuration - t;
  344.  
  345.   if (easeDirection == 1) {
  346.     return calculateEaseDirection(currentEase, &EasingBase::easeIn, _t);
  347.   } else {
  348.     return calculateEaseDirection(currentEase, &EasingBase::easeOut, _t);
  349.   }
  350.  
  351. }
  352.  
  353. double calculateEaseDirection(
  354.   EasingBase* easingMethod_, double(EasingBase::*easer_)(NUMBER) const,  int t) {
  355.   //  Serial.println(t);
  356.   return (easingMethod_->*easer_)(t);
  357. }
  358.  
  359.  
  360. int inOrOut(Ease ease) {
  361.   switch (ease) {
  362.     case CircularIn:
  363.     case QuadraticIn:
  364.     case QuinticIn:
  365.     case BounceIn:
  366.     case CubicIn:
  367.     case ExponentialIn:
  368.     case QuarticIn:
  369.     case SineIn:
  370.     case Linear:
  371.       return 1;
  372.       break;
  373.     case CircularOut:
  374.     case QuadraticOut:
  375.     case QuinticOut:
  376.     case BounceOut:
  377.     case CubicOut:
  378.     case ExponentialOut:
  379.     case QuarticOut:
  380.     case SineOut:
  381.       return 0;
  382.       break;
  383.     default:
  384.       return 1;
  385.       break;
  386.   }
  387. }
  388.  
  389.  
  390. void configureEase(Ease ease) {
  391.  
  392.   switch (ease) {
  393.     case CircularIn:
  394.     case CircularOut:
  395.       Serial.println("circular");
  396.       currentEase = &circular;
  397.       break;
  398.     case QuadraticIn:
  399.     case QuadraticOut:
  400.       Serial.println("quadratic");
  401.       currentEase = &quadratic;
  402.       break;
  403.     case QuinticIn:
  404.     case QuinticOut:
  405.       Serial.println("quintic");
  406.       currentEase = &quintic;
  407.       break;
  408.     case BounceIn:
  409.     case BounceOut:
  410.       Serial.println("bounce");
  411.       currentEase = &bounce;
  412.       break;
  413.     case CubicIn:
  414.     case CubicOut:
  415.       Serial.println("cubic");
  416.       currentEase = &cubic;
  417.       break;
  418.     case ExponentialIn:
  419.     case ExponentialOut:
  420.       Serial.println("Exponential");
  421.       currentEase = &exponential;
  422.       break;
  423.     case QuarticIn:
  424.     case QuarticOut:
  425.       Serial.println("Quartic");
  426.       currentEase = &quartic;
  427.       break;
  428.     case SineIn:
  429.     case SineOut:
  430.       Serial.println("sine");
  431.       currentEase = &sine;
  432.       break;
  433.     case Linear:
  434.       Serial.println("linear");
  435.       currentEase = &linear;
  436.       break;
  437.     default:
  438.       Serial.println("default");
  439.       currentEase = &linear;
  440.       break;
  441.   }
  442.  
  443.   currentEase->setDuration(fadeDuration);
  444.   currentEase->setTotalChangeInPosition(maxPWM);
  445.  
  446.   previousFadeMillis = millis();
  447. }
  448.  
  449. void handleMessage(AdafruitIO_Data *data) {
  450.   rxValue = atoi((char *)data->value());
  451.   Serial.print("RX VALUE: ");
  452.   Serial.println(rxValue);
  453. }
  454.  
  455. void checkButton() {
  456.   int reading = digitalRead(rxButton); // HIGH = off // LOW = on
  457.  
  458.   // If the switch changed, due to noise or pressing:
  459.   if (reading != lastButtonState) {
  460.     // reset the debouncing timer
  461.     lastDebounceTime = millis();
  462.   }
  463.  
  464.   if ((millis() - lastDebounceTime) > debounceDelay) {
  465.     // whatever the reading is at, it's been there for longer than the debounce
  466.     // delay, so take it as the actual current state:
  467.  
  468.     // if the button state has changed:
  469.     if (reading != buttonState) {
  470.       buttonState = reading;
  471.  
  472.       if (buttonState == LOW) {
  473.         if (rxValue == RX_WAITING) {
  474.           rxValue = RX_DOWN;
  475.           Serial.println("RX DOWN");
  476.         } else if (rxValue == RX_DOWN) {
  477.           rxValue = RX_UP;
  478.           Serial.println("RX UP");
  479.         } else {
  480.           rxValue = RX_WAITING;
  481.           Serial.println("RX WAITING");
  482.         }
  483.       }
  484.  
  485.     }
  486.   }
  487.  
  488.   lastButtonState = reading;
  489. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement