Smart Bike Suspension

1/* Edge Impulse Arduino examples
2 * Copyright (c) 2021 EdgeImpulse Inc.
3 * 
4 * MODIFIED by Jallson Suryo for specific Smart Bike Suspension Project
5 * 
6 * Permission is hereby granted, free of charge, to any person obtaining a copy
7 * of this software and associated documentation files (the "Software"), to deal
8 * in the Software without restriction, including without limitation the rights
9 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10 * copies of the Software, and to permit persons to whom the Software is
11 * furnished to do so, subject to the following conditions:
12 *
13 * The above copyright notice and this permission notice shall be included in
14 * all copies or substantial portions of the Software.
15 *
16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22 * SOFTWARE.
23 */
24
25/* Includes ---------------------------------------------------------------- */
26#include <Smart_Bike_Suspension_inferencing.h>
27#include <Arduino_LSM9DS1.h> //Click here to get the library: http://librarymanager/All#Arduino_LSM9DS1
28#include <Servo.h>
29
30Servo myservo;
31
32/* Constant defines -------------------------------------------------------- */
33#define CONVERT_G_TO_MS2    9.80665f
34#define MAX_ACCEPTED_RANGE  2.0f        // starting 03/2022, models are generated setting range to +-2, but this example use Arudino library which set range to +-4g. If you are using an older model, ignore this value and use 4.0f instead
35
36const int UNLOCK = 0;
37const int LOCK = 180;
38const int MIDDLE = 90;
39
40int selected;
41int pos = 0;
42
43/*
44 ** NOTE: If you run into TFLite arena allocation issue.
45 **
46 ** This may be due to may dynamic memory fragmentation.
47 ** Try defining "-DEI_CLASSIFIER_ALLOCATION_STATIC" in boards.local.txt (create
48 ** if it doesn't exist) and copy this file to
49 ** `<ARDUINO_CORE_INSTALL_PATH>/arduino/hardware/<mbed_core>/<core_version>/`.
50 **
51 ** See
52 ** (https://support.arduino.cc/hc/en-us/articles/360012076960-Where-are-the-installed-cores-located-)
53 ** to find where Arduino installs cores on your machine.
54 **
55 ** If the problem persists then there's not enough memory for this model and application.
56 */
57
58/* Private variables ------------------------------------------------------- */
59static bool debug_nn = false; // Set this to true to see e.g. features generated from the raw signal
60
61/**
62* @brief      Arduino setup function
63*/
64void setup()
65{
66    myservo.attach(9);
67    // put your setup code here, to run once:
68    Serial.begin(115200);
69    // comment out the below line to cancel the wait for USB connection (needed for native USB)
70    //while (!Serial);
71    Serial.println("Edge Impulse Inferencing Demo");
72
73    if (!IMU.begin()) {
74        ei_printf("Failed to initialize IMU!\r\n");
75    }
76    else {
77        ei_printf("IMU initialized\r\n");
78    }
79
80    if (EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME != 3) {
81        ei_printf("ERR: EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME should be equal to 3 (the 3 sensor axes)\n");
82        return;
83    }
84}
85
86/**
87 * @brief Return the sign of the number
88 * 
89 * @param number 
90 * @return int 1 if positive (or 0) -1 if negative
91 */
92float ei_get_sign(float number) {
93    return (number >= 0.0) ? 1.0 : -1.0;
94}
95
96/**
97* @brief      Get data and run inferencing
98*
99* @param[in]  debug  Get debug info if true
100*/
101void loop()
102{
103    //ei_printf("\nStarting inferencing in 0.5 seconds...\n");
104
105    //delay(100);
106
107    ei_printf("Sampling...\n");
108
109    // Allocate a buffer here for the values we'll read from the IMU
110    float buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE] = { 0 };
111
112    for (size_t ix = 0; ix < EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE; ix += 3) {
113        // Determine the next tick (and then sleep later)
114        uint64_t next_tick = micros() + (EI_CLASSIFIER_INTERVAL_MS * 1000);
115
116        IMU.readAcceleration(buffer[ix], buffer[ix + 1], buffer[ix + 2]);
117
118        for (int i = 0; i < 3; i++) {
119            if (fabs(buffer[ix + i]) > MAX_ACCEPTED_RANGE) {
120                buffer[ix + i] = ei_get_sign(buffer[ix + i]) * MAX_ACCEPTED_RANGE;
121            }
122        }
123
124        buffer[ix + 0] *= CONVERT_G_TO_MS2;
125        buffer[ix + 1] *= CONVERT_G_TO_MS2;
126        buffer[ix + 2] *= CONVERT_G_TO_MS2;
127
128        delayMicroseconds(next_tick - micros());
129    }
130
131    // Turn the raw buffer in a signal which we can the classify
132    signal_t signal;
133    int err = numpy::signal_from_buffer(buffer, EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE, &signal);
134    if (err != 0) {
135        ei_printf("Failed to create signal from buffer (%d)\n", err);
136        return;
137    }
138
139    // Run the classifier
140    ei_impulse_result_t result = { 0 };
141
142    err = run_classifier(&signal, &result, debug_nn);
143    if (err != EI_IMPULSE_OK) {
144        ei_printf("ERR: Failed to run classifier (%d)\n", err);
145        return;
146    }
147
148    // print the predictions
149    ei_printf("Predictions ");
150    ei_printf("(DSP: %d ms., Classification: %d ms., Anomaly: %d ms.)",
151        result.timing.dsp, result.timing.classification, result.timing.anomaly);
152    ei_printf(": \n");
153    for (size_t ix = 0; ix < EI_CLASSIFIER_LABEL_COUNT; ix++) {
154        ei_printf("    %s: %.5f\n", result.classification[ix].label, result.classification[ix].value);
155        if (result.classification[ix].value > 0.5){
156          ei_printf("Selected: %s\n", result.classification[ix].label);
157          selected = ix;
158        }
159    }
160    if (selected == 0 || selected == 3){ //GO TO LOCK
161      if (pos <= UNLOCK){
162        for (pos = UNLOCK; pos <= LOCK; pos += 1) { // goes from UNLOCK0 to LOCK180
163          myservo.write(pos);
164          delay(5);
165        }
166      }
167      else if (pos == MIDDLE || pos == MIDDLE+1 || pos == MIDDLE-1){
168        for (pos = MIDDLE; pos <= LOCK; pos += 1) { // goes from MIDDLE90 to LOCK180
169          myservo.write(pos);
170          delay(5);
171        }
172      }
173      else{
174        pos = LOCK;
175      }
176    }
177    
178    else if (selected == 1 || selected == 4){  //GO TO MIDDLE
179      if (pos >= LOCK){
180        for (pos = LOCK; pos >= MIDDLE; pos -= 1) { //goes from LOCK180 to MIDDLE90
181          myservo.write(pos);
182          delay(5);
183        }
184      } 
185      else if (pos <= UNLOCK){
186        for (pos = UNLOCK; pos <= MIDDLE; pos += 1) { // goes from UNLOCK0 to MIDDLE90
187          myservo.write(pos);
188          delay(5);
189        }
190      }
191      else{
192        pos = MIDDLE;
193      }
194    }
195    
196    else{ //GO TO UNLOCK
197      if (pos >= LOCK){
198        for (pos = LOCK; pos >= UNLOCK; pos -= 1) { //goes from LOCK180 to UNLOCK0
199          myservo.write(pos);
200          delay(5);
201        }
202      }
203      else if (pos == MIDDLE || pos == MIDDLE+1 || pos == MIDDLE-1) {
204        for (pos = MIDDLE; pos <= UNLOCK; pos -= 1) { // goes from MIDDLE90 to UNLOCK0
205          myservo.write(pos);
206          delay(5);
207        }
208      }
209      else{
210        pos = UNLOCK;
211      }
212    }
213
214    
215#if EI_CLASSIFIER_HAS_ANOMALY == 1
216    ei_printf("    anomaly score: %.3f\n", result.anomaly);
217#endif
218
219
220}
221
222#if !defined(EI_CLASSIFIER_SENSOR) || EI_CLASSIFIER_SENSOR != EI_CLASSIFIER_SENSOR_ACCELEROMETER
223#error "Invalid model for current sensor"
224#endif