Gesture Control of a Self-Balancing Robot Using TensorFlow

This example uses the on-board IMU to start reading acceleration and gyroscope data from on-board IMU, once enough samples are read, it then uses a TensorFlow Lite (Micro) model to try to classify the movement as a known gesture. Created by Don Coleman, Sandeep Mistry modified by Rolf Kurth

1/*
2  IMU Classifier
3
4  This example uses the on-board IMU to   start reading acceleration and gyroscope
5  data from on-board IMU, once enough   samples are read, it then uses a
6  TensorFlow Lite (Micro) model to try to classify   the movement as a known gesture.
7
8  Note: The direct use of C/C++ pointers,   namespaces, and dynamic memory is generally
9        discouraged in Arduino examples,   and in the future the TensorFlowLite library
10        might change to make the   sketch simpler.
11
12  The circuit:
13  - Arduino Nano 33 BLE or Arduino Nano   33 BLE Sense board.
14  - Button connected to pin 3 and GND.
15
16  Created by   Don Coleman, Sandeep Mistry
17  modified by Rolf Kurth
18
19  This example code   is in the public domain.
20*/
21
22// ------------------------------------------------------------------------
23//   https://www.arduino.cc/en/Reference/HomePage
24// ------------------------------------------------------------------------
25
26#include   <Arduino_LSM9DS1.h>
27
28#include <TensorFlowLite.h>
29#include <tensorflow/lite/experimental/micro/kernels/all_ops_resolver.h>
30#include   <tensorflow/lite/experimental/micro/micro_error_reporter.h>
31#include <tensorflow/lite/experimental/micro/micro_interpreter.h>
32#include   <tensorflow/lite/schema/schema_generated.h>
33#include <tensorflow/lite/version.h>
34#include   "BTooth.h"
35#
36#include "model.h"
37
38
39const int buttonPin = 3;     //   the pin number of the pushbutton pin
40const int numSamples = 59;
41
42int previousButtonState   = HIGH;
43int samplesRead = numSamples;
44#define TestRun
45
46// global variables   used for TensorFlow Lite (Micro)
47tflite::MicroErrorReporter tflErrorReporter;
48
49//   pull in all the TFLM ops, you can remove this line and
50// only pull in the TFLM   ops you need, if would like to reduce
51// the compiled size of the sketch.
52tflite::ops::micro::AllOpsResolver   tflOpsResolver;
53
54
55const tflite::Model* tflModel = nullptr;
56tflite::MicroInterpreter*   tflInterpreter = nullptr;
57TfLiteTensor* tflInputTensor = nullptr;
58TfLiteTensor*   tflOutputTensor = nullptr;
59
60// Create a static memory buffer for TFLM, the   size may need to
61// be adjusted based on the model you are using
62constexpr   int tensorArenaSize = 8 * 1024;
63byte tensorArena[tensorArenaSize];
64
65//   array to map gesture index to a name
66const char* GESTURES[] = {
67  "left",
68   "right",
69  "forward",
70  "backward",
71};
72const int XDirection[]   = { 30, -30, 0, 0 };
73const int YDirection[] = { 0, 0, 30, -30 };
74
75#define   NUM_GESTURES (sizeof(GESTURES) / sizeof(GESTURES[0]))
76
77BTooth BT;  // Bluetooth   Sender
78
79void setup() {
80  Serial.begin(9600);
81  Serial1.begin(9600);
82
83   // initialize the pushbutton pin as an input with pullup:
84  pinMode(buttonPin,   INPUT_PULLUP);
85
86  // initialize the IMU
87  if (!IMU.begin()) {
88    Serial.println("Failed   to initialize IMU!");
89    while (1);
90  }
91
92  // print out the samples   rates of the IMUs
93  Serial.print("Accelerometer sample rate = ");
94  Serial.print(IMU.accelerationSampleRate());
95   Serial.println(" Hz");
96  Serial.print("Gyroscope sample rate = ");
97   Serial.print(IMU.gyroscopeSampleRate());
98  Serial.println(" Hz");
99
100   Serial.println();
101
102  // get the TFL representation of the model byte array
103   tflModel = tflite::GetModel(model);
104  if (tflModel->version() != TFLITE_SCHEMA_VERSION)   {
105    Serial.println("Model schema mismatch!");
106    while (1);
107  }
108
109   // Create an interpreter to run the model
110  tflInterpreter = new tflite::MicroInterpreter(tflModel,   tflOpsResolver, tensorArena, tensorArenaSize, &tflErrorReporter);
111
112  // Allocate   memory for the model's input and output tensors
113  tflInterpreter->AllocateTensors();
114
115   // Get pointers for the model's input and output tensors
116  tflInputTensor =   tflInterpreter->input(0);
117  tflOutputTensor = tflInterpreter->output(0);
118
119   static bool is_initialized = false;
120  if (!is_initialized) {
121    pinMode(LED_BUILTIN,   OUTPUT);
122    // Pins for the built-in RGB LEDs on the Arduino Nano 33 BLE Sense
123     pinMode(LEDR, OUTPUT);
124    pinMode(LEDG, OUTPUT);
125    pinMode(LEDB, OUTPUT);
126     // Ensure the LED is off by default.
127    // Note: The RGB LEDs on the Arduino   Nano 33 BLE
128    // Sense are on when the pin is LOW, off when HIGH.
129    digitalWrite(LEDR,   HIGH);
130    digitalWrite(LEDG, HIGH);
131    digitalWrite(LEDB, HIGH);
132    is_initialized   = true;
133
134  }
135}
136
137void loop() {
138  // read the state of the push   button pin:
139  int buttonState = digitalRead(buttonPin);
140
141  // compare the   button state to the previous state
142  // to see if it has changed
143  if (buttonState   != previousButtonState) {
144    if (buttonState == LOW) {
145      if (samplesRead   == numSamples) {
146        // pressed       }
147        samplesRead = 0;
148        digitalWrite(LED_BUILTIN,   LOW);
149        digitalWrite(LEDR, HIGH);
150        digitalWrite(LEDG, HIGH);
151         //       digitalWrite(LEDB, HIGH);
152      }
153    } else {
154      //   released
155      digitalWrite(LED_BUILTIN, HIGH);
156      digitalWrite(LEDR, HIGH);
157       digitalWrite(LEDG, HIGH);
158    }
159
160    // store the state as the previous   state, for the next loop
161    previousButtonState = buttonState;
162  }
163
164   // check if the all the required samples have been read since
165  // the last   time the button has been pressed
166  if (samplesRead < numSamples) {
167    //   check if new acceleration AND gyroscope data is available
168    if (IMU.accelerationAvailable()   && IMU.gyroscopeAvailable()) {
169      float aX, aY, aZ, gX, gY, gZ;
170
171      //   read the acceleration and gyroscope data
172      IMU.readAcceleration(aX, aY, aZ);
173       IMU.readGyroscope(gX, gY, gZ);
174
175      // normalize the IMU data between   0 to 1 and store in the model's
176      // input tensor
177      tflInputTensor->data.f[samplesRead   * 6 + 0] = (aX + 4.0) / 8.0;
178      tflInputTensor->data.f[samplesRead * 6 + 1]   = (aY + 4.0) / 8.0;
179      tflInputTensor->data.f[samplesRead * 6 + 2] = (aZ +   4.0) / 8.0;
180      tflInputTensor->data.f[samplesRead * 6 + 3] = (gX + 2000.0)   / 4000.0;
181      tflInputTensor->data.f[samplesRead * 6 + 4] = (gY + 2000.0) /   4000.0;
182      tflInputTensor->data.f[samplesRead * 6 + 5] = (gZ + 2000.0) / 4000.0;
183
184       samplesRead++;
185
186      if (samplesRead == numSamples) {
187        //   Run inferencing
188        TfLiteStatus invokeStatus = tflInterpreter->Invoke();
189         if (invokeStatus != kTfLiteOk) {
190          Serial.println("Invoke failed!");
191           while (1);
192          return;
193        }
194
195        int result;
196         result = -1;
197        // Loop through the output tensor values from the   model
198        for (int i = 0; i < NUM_GESTURES; i++) {
199          Serial.print(GESTURES[i]);
200           Serial.print(": ");
201          Serial.print(tflOutputTensor->data.f[i],   6);
202          Serial.print(" / ");
203          if ( tflOutputTensor->data.f[i]   > 0.95) {
204            result = i;
205          }
206        }
207        Serial.println();
208         Serial.print(" Direction : ");
209        if (result >= 0 ) {
210          Serial.println(GESTURES[result]);
211
212           digitalWrite(LEDG, LOW);  // Green for yes
213          BT.XYsend( XDirection[result],   YDirection[result]);   // Bluetooth send X Y
214        }
215        else {
216           digitalWrite(LEDR, LOW);  // red for stop
217          Serial.println("Stop");
218           BT.XYsend( 0, 0); // Bluetooth send X Y
219        }
220      }
221    }
222   }
223}

/*
  IMU Classifier

  This example uses the on-board IMU to   start reading acceleration and gyroscope
  data from on-board IMU, once enough   samples are read, it then uses a
  TensorFlow Lite (Micro) model to try to classify   the movement as a known gesture.

  Note: The direct use of C/C++ pointers,   namespaces, and dynamic memory is generally
        discouraged in Arduino examples,   and in the future the TensorFlowLite library
        might change to make the   sketch simpler.

  The circuit:
  - Arduino Nano 33 BLE or Arduino Nano   33 BLE Sense board.
  - Button connected to pin 3 and GND.

  Created by   Don Coleman, Sandeep Mistry
  modified by Rolf Kurth

  This example code   is in the public domain.
*/

// ------------------------------------------------------------------------
//   https://www.arduino.cc/en/Reference/HomePage
// ------------------------------------------------------------------------

#include   <Arduino_LSM9DS1.h>

#include <TensorFlowLite.h>
#include <tensorflow/lite/experimental/micro/kernels/all_ops_resolver.h>
#include   <tensorflow/lite/experimental/micro/micro_error_reporter.h>
#include <tensorflow/lite/experimental/micro/micro_interpreter.h>
#include   <tensorflow/lite/schema/schema_generated.h>
#include <tensorflow/lite/version.h>
#include   "BTooth.h"
#
#include "model.h"


const int buttonPin = 3;     //   the pin number of the pushbutton pin
const int numSamples = 59;

int previousButtonState   = HIGH;
int samplesRead = numSamples;
#define TestRun

// global variables   used for TensorFlow Lite (Micro)
tflite::MicroErrorReporter tflErrorReporter;

//   pull in all the TFLM ops, you can remove this line and
// only pull in the TFLM   ops you need, if would like to reduce
// the compiled size of the sketch.
tflite::ops::micro::AllOpsResolver   tflOpsResolver;


const tflite::Model* tflModel = nullptr;
tflite::MicroInterpreter*   tflInterpreter = nullptr;
TfLiteTensor* tflInputTensor = nullptr;
TfLiteTensor*   tflOutputTensor = nullptr;

// Create a static memory buffer for TFLM, the   size may need to
// be adjusted based on the model you are using
constexpr   int tensorArenaSize = 8 * 1024;
byte tensorArena[tensorArenaSize];

//   array to map gesture index to a name
const char* GESTURES[] = {
  "left",
   "right",
  "forward",
  "backward",
};
const int XDirection[]   = { 30, -30, 0, 0 };
const int YDirection[] = { 0, 0, 30, -30 };

#define   NUM_GESTURES (sizeof(GESTURES) / sizeof(GESTURES[0]))

BTooth BT;  // Bluetooth   Sender

void setup() {
  Serial.begin(9600);
  Serial1.begin(9600);

   // initialize the pushbutton pin as an input with pullup:
  pinMode(buttonPin,   INPUT_PULLUP);

  // initialize the IMU
  if (!IMU.begin()) {
    Serial.println("Failed   to initialize IMU!");
    while (1);
  }

  // print out the samples   rates of the IMUs
  Serial.print("Accelerometer sample rate = ");
  Serial.print(IMU.accelerationSampleRate());
   Serial.println(" Hz");
  Serial.print("Gyroscope sample rate = ");
   Serial.print(IMU.gyroscopeSampleRate());
  Serial.println(" Hz");

   Serial.println();

  // get the TFL representation of the model byte array
   tflModel = tflite::GetModel(model);
  if (tflModel->version() != TFLITE_SCHEMA_VERSION)   {
    Serial.println("Model schema mismatch!");
    while (1);
  }

   // Create an interpreter to run the model
  tflInterpreter = new tflite::MicroInterpreter(tflModel,   tflOpsResolver, tensorArena, tensorArenaSize, &tflErrorReporter);

  // Allocate   memory for the model's input and output tensors
  tflInterpreter->AllocateTensors();

   // Get pointers for the model's input and output tensors
  tflInputTensor =   tflInterpreter->input(0);
  tflOutputTensor = tflInterpreter->output(0);

   static bool is_initialized = false;
  if (!is_initialized) {
    pinMode(LED_BUILTIN,   OUTPUT);
    // Pins for the built-in RGB LEDs on the Arduino Nano 33 BLE Sense
     pinMode(LEDR, OUTPUT);
    pinMode(LEDG, OUTPUT);
    pinMode(LEDB, OUTPUT);
     // Ensure the LED is off by default.
    // Note: The RGB LEDs on the Arduino   Nano 33 BLE
    // Sense are on when the pin is LOW, off when HIGH.
    digitalWrite(LEDR,   HIGH);
    digitalWrite(LEDG, HIGH);
    digitalWrite(LEDB, HIGH);
    is_initialized   = true;

  }
}

void loop() {
  // read the state of the push   button pin:
  int buttonState = digitalRead(buttonPin);

  // compare the   button state to the previous state
  // to see if it has changed
  if (buttonState   != previousButtonState) {
    if (buttonState == LOW) {
      if (samplesRead   == numSamples) {
        // pressed       }
        samplesRead = 0;
        digitalWrite(LED_BUILTIN,   LOW);
        digitalWrite(LEDR, HIGH);
        digitalWrite(LEDG, HIGH);
         //       digitalWrite(LEDB, HIGH);
      }
    } else {
      //   released
      digitalWrite(LED_BUILTIN, HIGH);
      digitalWrite(LEDR, HIGH);
       digitalWrite(LEDG, HIGH);
    }

    // store the state as the previous   state, for the next loop
    previousButtonState = buttonState;
  }

   // check if the all the required samples have been read since
  // the last   time the button has been pressed
  if (samplesRead < numSamples) {
    //   check if new acceleration AND gyroscope data is available
    if (IMU.accelerationAvailable()   && IMU.gyroscopeAvailable()) {
      float aX, aY, aZ, gX, gY, gZ;

      //   read the acceleration and gyroscope data
      IMU.readAcceleration(aX, aY, aZ);
       IMU.readGyroscope(gX, gY, gZ);

      // normalize the IMU data between   0 to 1 and store in the model's
      // input tensor
      tflInputTensor->data.f[samplesRead   * 6 + 0] = (aX + 4.0) / 8.0;
      tflInputTensor->data.f[samplesRead * 6 + 1]   = (aY + 4.0) / 8.0;
      tflInputTensor->data.f[samplesRead * 6 + 2] = (aZ +   4.0) / 8.0;
      tflInputTensor->data.f[samplesRead * 6 + 3] = (gX + 2000.0)   / 4000.0;
      tflInputTensor->data.f[samplesRead * 6 + 4] = (gY + 2000.0) /   4000.0;
      tflInputTensor->data.f[samplesRead * 6 + 5] = (gZ + 2000.0) / 4000.0;

       samplesRead++;

      if (samplesRead == numSamples) {
        //   Run inferencing
        TfLiteStatus invokeStatus = tflInterpreter->Invoke();
         if (invokeStatus != kTfLiteOk) {
          Serial.println("Invoke failed!");
           while (1);
          return;
        }

        int result;
         result = -1;
        // Loop through the output tensor values from the   model
        for (int i = 0; i < NUM_GESTURES; i++) {
          Serial.print(GESTURES[i]);
           Serial.print(": ");
          Serial.print(tflOutputTensor->data.f[i],   6);
          Serial.print(" / ");
          if ( tflOutputTensor->data.f[i]   > 0.95) {
            result = i;
          }
        }
        Serial.println();
         Serial.print(" Direction : ");
        if (result >= 0 ) {
          Serial.println(GESTURES[result]);

           digitalWrite(LEDG, LOW);  // Green for yes
          BT.XYsend( XDirection[result],   YDirection[result]);   // Bluetooth send X Y
        }
        else {
           digitalWrite(LEDR, LOW);  // red for stop
          Serial.println("Stop");
           BT.XYsend( 0, 0); // Bluetooth send X Y
        }
      }
    }
   }
}

Bluetooth Receiver from self balancing robot

c_cpp

1/* Self balancing Robot via Stepper Motor with microstepping and Digital
2  Motion Processing
3    written by : Rolf Kurth in 2019
4    rolf.kurth@cron-consulting.de
5*/
6
7String
8  inputString = "";         // a String to hold incoming data
9bool   stringComplete
10  = false;  // whether the string is complete
11char   c = ' ';
12
13/*
14    SerialEvent
15  occurs whenever a new data comes in the hardware serial RX. This
16    routine
17  is run between each time loop() runs, so using delay inside loop can
18    delay
19  response. Multiple bytes of data may be available.
20*/
21void serialEvent1()
22  {
23  while (Serial1.available()) {
24    // get the new byte:
25    char inChar
26  = (char)Serial1.read();
27    // add it to the inputString:
28    if (!stringComplete)
29  {
30      inputString += inChar;
31    }
32    // if the incoming character is
33  a newline, set a flag so the main loop can
34    // do something about it:
35
36    if (inChar == '\
37') {
38      stringComplete = true;
39    }
40  }
41}
42
43void
44  BTRead( JStickData &JSData  ) {
45  String       command;
46  unsigned int j;
47
48  long         value;
49
50
51  // print the string when a newline arrives:
52
53  if (stringComplete) {
54    if (inputString.substring(0, 1) != "X")
55    {
56
57      Serial.print("Error reading Bluetooth ");
58      Serial.println(inputString);
59
60    } else {
61      j = 0;
62      for (unsigned int i = 0; i < inputString.length();
63  i++) {
64
65        if (inputString.substring(i, i + 1) == "#") {
66          command
67  = inputString.substring(j, i);
68          //Serial.print("Command: ");
69          //Serial.print(command);
70
71          j = i + 1;
72          for (unsigned int i1 = j; i1 < inputString.length();
73  i1++) {
74            if (inputString.substring(i1, i1 + 1) == "#") {
75              value
76  = inputString.substring(j, i1).toInt();
77              //Serial.print(" Value:
78  ");
79              //Serial.println(value);
80              i = i1;
81              j
82  = i + 1;
83              assignValues(command, value, JSData);
84              break;
85
86            }
87          }
88        }
89      }
90    }
91    inputString
92  = "";
93    stringComplete = false;
94    // Serial.print(" Value: ");
95
96    // Serial.println(JStick.Xvalue);
97  }
98}
99
100void assignValues(String
101  icommand, int ivalue, JStickData &Data  ) {
102
103  if (icommand == "X")  Data.Xvalue
104  = ivalue;
105  if (icommand == "Y")  Data.Yvalue  = ivalue;
106  if (icommand
107  == "B1") Data.JButton = ivalue;
108  if (icommand == "Up") Data.Up      = ivalue;
109
110  if (icommand == "Do") Data.Down    = ivalue;
111  if (icommand == "Ri") Data.Right
112   = ivalue;
113  if (icommand == "Le") Data.Left    = ivalue;
114
115}

/* Self balancing Robot via Stepper Motor with microstepping and Digital
  Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/

String
  inputString = "";         // a String to hold incoming data
bool   stringComplete
  = false;  // whether the string is complete
char   c = ' ';

/*
    SerialEvent
  occurs whenever a new data comes in the hardware serial RX. This
    routine
  is run between each time loop() runs, so using delay inside loop can
    delay
  response. Multiple bytes of data may be available.
*/
void serialEvent1()
  {
  while (Serial1.available()) {
    // get the new byte:
    char inChar
  = (char)Serial1.read();
    // add it to the inputString:
    if (!stringComplete)
  {
      inputString += inChar;
    }
    // if the incoming character is
  a newline, set a flag so the main loop can
    // do something about it:

    if (inChar == '\
') {
      stringComplete = true;
    }
  }
}

void
  BTRead( JStickData &JSData  ) {
  String       command;
  unsigned int j;

  long         value;


  // print the string when a newline arrives:

  if (stringComplete) {
    if (inputString.substring(0, 1) != "X")
    {

      Serial.print("Error reading Bluetooth ");
      Serial.println(inputString);

    } else {
      j = 0;
      for (unsigned int i = 0; i < inputString.length();
  i++) {

        if (inputString.substring(i, i + 1) == "#") {
          command
  = inputString.substring(j, i);
          //Serial.print("Command: ");
          //Serial.print(command);

          j = i + 1;
          for (unsigned int i1 = j; i1 < inputString.length();
  i1++) {
            if (inputString.substring(i1, i1 + 1) == "#") {
              value
  = inputString.substring(j, i1).toInt();
              //Serial.print(" Value:
  ");
              //Serial.println(value);
              i = i1;
              j
  = i + 1;
              assignValues(command, value, JSData);
              break;

            }
          }
        }
      }
    }
    inputString
  = "";
    stringComplete = false;
    // Serial.print(" Value: ");

    // Serial.println(JStick.Xvalue);
  }
}

void assignValues(String
  icommand, int ivalue, JStickData &Data  ) {

  if (icommand == "X")  Data.Xvalue
  = ivalue;
  if (icommand == "Y")  Data.Yvalue  = ivalue;
  if (icommand
  == "B1") Data.JButton = ivalue;
  if (icommand == "Up") Data.Up      = ivalue;

  if (icommand == "Do") Data.Down    = ivalue;
  if (icommand == "Ri") Data.Right
   = ivalue;
  if (icommand == "Le") Data.Left    = ivalue;

}

IMU Classifier

c_cpp

1/*
2  IMU Classifier
3
4  This example uses the on-board IMU to  start reading acceleration and gyroscope
5  data from on-board IMU, once enough  samples are read, it then uses a
6  TensorFlow Lite (Micro) model to try to classify  the movement as a known gesture.
7
8  Note: The direct use of C/C++ pointers,  namespaces, and dynamic memory is generally
9        discouraged in Arduino examples,  and in the future the TensorFlowLite library
10        might change to make the  sketch simpler.
11
12  The circuit:
13  - Arduino Nano 33 BLE or Arduino Nano  33 BLE Sense board.
14  - Button connected to pin 3 and GND.
15
16  Created by  Don Coleman, Sandeep Mistry
17  modified by Rolf Kurth
18
19  This example code  is in the public domain.
20*/
21
22// ------------------------------------------------------------------------
23//  https://www.arduino.cc/en/Reference/HomePage
24// ------------------------------------------------------------------------
25
26#include  <Arduino_LSM9DS1.h>
27
28#include <TensorFlowLite.h>
29#include <tensorflow/lite/experimental/micro/kernels/all_ops_resolver.h>
30#include  <tensorflow/lite/experimental/micro/micro_error_reporter.h>
31#include <tensorflow/lite/experimental/micro/micro_interpreter.h>
32#include  <tensorflow/lite/schema/schema_generated.h>
33#include <tensorflow/lite/version.h>
34#include  "BTooth.h"
35#
36#include "model.h"
37
38
39const int buttonPin = 3;     //  the pin number of the pushbutton pin
40const int numSamples = 59;
41
42int previousButtonState  = HIGH;
43int samplesRead = numSamples;
44#define TestRun
45
46// global variables  used for TensorFlow Lite (Micro)
47tflite::MicroErrorReporter tflErrorReporter;
48
49//  pull in all the TFLM ops, you can remove this line and
50// only pull in the TFLM  ops you need, if would like to reduce
51// the compiled size of the sketch.
52tflite::ops::micro::AllOpsResolver  tflOpsResolver;
53
54
55const tflite::Model* tflModel = nullptr;
56tflite::MicroInterpreter*  tflInterpreter = nullptr;
57TfLiteTensor* tflInputTensor = nullptr;
58TfLiteTensor*  tflOutputTensor = nullptr;
59
60// Create a static memory buffer for TFLM, the  size may need to
61// be adjusted based on the model you are using
62constexpr  int tensorArenaSize = 8 * 1024;
63byte tensorArena[tensorArenaSize];
64
65//  array to map gesture index to a name
66const char* GESTURES[] = {
67  "left",
68  "right",
69  "forward",
70  "backward",
71};
72const int XDirection[]  = { 30, -30, 0, 0 };
73const int YDirection[] = { 0, 0, 30, -30 };
74
75#define  NUM_GESTURES (sizeof(GESTURES) / sizeof(GESTURES[0]))
76
77BTooth BT;  // Bluetooth  Sender
78
79void setup() {
80  Serial.begin(9600);
81  Serial1.begin(9600);
82
83  // initialize the pushbutton pin as an input with pullup:
84  pinMode(buttonPin,  INPUT_PULLUP);
85
86  // initialize the IMU
87  if (!IMU.begin()) {
88    Serial.println("Failed  to initialize IMU!");
89    while (1);
90  }
91
92  // print out the samples  rates of the IMUs
93  Serial.print("Accelerometer sample rate = ");
94  Serial.print(IMU.accelerationSampleRate());
95  Serial.println(" Hz");
96  Serial.print("Gyroscope sample rate = ");
97  Serial.print(IMU.gyroscopeSampleRate());
98  Serial.println(" Hz");
99
100  Serial.println();
101
102  // get the TFL representation of the model byte array
103  tflModel = tflite::GetModel(model);
104  if (tflModel->version() != TFLITE_SCHEMA_VERSION)  {
105    Serial.println("Model schema mismatch!");
106    while (1);
107  }
108
109  // Create an interpreter to run the model
110  tflInterpreter = new tflite::MicroInterpreter(tflModel,  tflOpsResolver, tensorArena, tensorArenaSize, &tflErrorReporter);
111
112  // Allocate  memory for the model's input and output tensors
113  tflInterpreter->AllocateTensors();
114
115  // Get pointers for the model's input and output tensors
116  tflInputTensor =  tflInterpreter->input(0);
117  tflOutputTensor = tflInterpreter->output(0);
118
119  static bool is_initialized = false;
120  if (!is_initialized) {
121    pinMode(LED_BUILTIN,  OUTPUT);
122    // Pins for the built-in RGB LEDs on the Arduino Nano 33 BLE Sense
123    pinMode(LEDR, OUTPUT);
124    pinMode(LEDG, OUTPUT);
125    pinMode(LEDB, OUTPUT);
126    // Ensure the LED is off by default.
127    // Note: The RGB LEDs on the Arduino  Nano 33 BLE
128    // Sense are on when the pin is LOW, off when HIGH.
129    digitalWrite(LEDR,  HIGH);
130    digitalWrite(LEDG, HIGH);
131    digitalWrite(LEDB, HIGH);
132    is_initialized  = true;
133
134  }
135}
136
137void loop() {
138  // read the state of the push  button pin:
139  int buttonState = digitalRead(buttonPin);
140
141  // compare the  button state to the previous state
142  // to see if it has changed
143  if (buttonState  != previousButtonState) {
144    if (buttonState == LOW) {
145      if (samplesRead  == numSamples) {
146        // pressed       }
147        samplesRead = 0;
148        digitalWrite(LED_BUILTIN,  LOW);
149        digitalWrite(LEDR, HIGH);
150        digitalWrite(LEDG, HIGH);
151        //       digitalWrite(LEDB, HIGH);
152      }
153    } else {
154      //  released
155      digitalWrite(LED_BUILTIN, HIGH);
156      digitalWrite(LEDR, HIGH);
157      digitalWrite(LEDG, HIGH);
158    }
159
160    // store the state as the previous  state, for the next loop
161    previousButtonState = buttonState;
162  }
163
164  // check if the all the required samples have been read since
165  // the last  time the button has been pressed
166  if (samplesRead < numSamples) {
167    //  check if new acceleration AND gyroscope data is available
168    if (IMU.accelerationAvailable()  && IMU.gyroscopeAvailable()) {
169      float aX, aY, aZ, gX, gY, gZ;
170
171      //  read the acceleration and gyroscope data
172      IMU.readAcceleration(aX, aY, aZ);
173      IMU.readGyroscope(gX, gY, gZ);
174
175      // normalize the IMU data between  0 to 1 and store in the model's
176      // input tensor
177      tflInputTensor->data.f[samplesRead  * 6 + 0] = (aX + 4.0) / 8.0;
178      tflInputTensor->data.f[samplesRead * 6 + 1]  = (aY + 4.0) / 8.0;
179      tflInputTensor->data.f[samplesRead * 6 + 2] = (aZ +  4.0) / 8.0;
180      tflInputTensor->data.f[samplesRead * 6 + 3] = (gX + 2000.0)  / 4000.0;
181      tflInputTensor->data.f[samplesRead * 6 + 4] = (gY + 2000.0) /  4000.0;
182      tflInputTensor->data.f[samplesRead * 6 + 5] = (gZ + 2000.0) / 4000.0;
183
184      samplesRead++;
185
186      if (samplesRead == numSamples) {
187        //  Run inferencing
188        TfLiteStatus invokeStatus = tflInterpreter->Invoke();
189        if (invokeStatus != kTfLiteOk) {
190          Serial.println("Invoke failed!");
191          while (1);
192          return;
193        }
194
195        int result;
196        result = -1;
197        // Loop through the output tensor values from the  model
198        for (int i = 0; i < NUM_GESTURES; i++) {
199          Serial.print(GESTURES[i]);
200          Serial.print(": ");
201          Serial.print(tflOutputTensor->data.f[i],  6);
202          Serial.print(" / ");
203          if ( tflOutputTensor->data.f[i]  > 0.95) {
204            result = i;
205          }
206        }
207        Serial.println();
208        Serial.print(" Direction : ");
209        if (result >= 0 ) {
210          Serial.println(GESTURES[result]);
211
212          digitalWrite(LEDG, LOW);  // Green for yes
213          BT.XYsend( XDirection[result],  YDirection[result]);   // Bluetooth send X Y
214        }
215        else {
216          digitalWrite(LEDR, LOW);  // red for stop
217          Serial.println("Stop");
218          BT.XYsend( 0, 0); // Bluetooth send X Y
219        }
220      }
221    }
222  }
223}

/*
  IMU Classifier

  This example uses the on-board IMU to  start reading acceleration and gyroscope
  data from on-board IMU, once enough  samples are read, it then uses a
  TensorFlow Lite (Micro) model to try to classify  the movement as a known gesture.

  Note: The direct use of C/C++ pointers,  namespaces, and dynamic memory is generally
        discouraged in Arduino examples,  and in the future the TensorFlowLite library
        might change to make the  sketch simpler.

  The circuit:
  - Arduino Nano 33 BLE or Arduino Nano  33 BLE Sense board.
  - Button connected to pin 3 and GND.

  Created by  Don Coleman, Sandeep Mistry
  modified by Rolf Kurth

  This example code  is in the public domain.
*/

// ------------------------------------------------------------------------
//  https://www.arduino.cc/en/Reference/HomePage
// ------------------------------------------------------------------------

#include  <Arduino_LSM9DS1.h>

#include <TensorFlowLite.h>
#include <tensorflow/lite/experimental/micro/kernels/all_ops_resolver.h>
#include  <tensorflow/lite/experimental/micro/micro_error_reporter.h>
#include <tensorflow/lite/experimental/micro/micro_interpreter.h>
#include  <tensorflow/lite/schema/schema_generated.h>
#include <tensorflow/lite/version.h>
#include  "BTooth.h"
#
#include "model.h"


const int buttonPin = 3;     //  the pin number of the pushbutton pin
const int numSamples = 59;

int previousButtonState  = HIGH;
int samplesRead = numSamples;
#define TestRun

// global variables  used for TensorFlow Lite (Micro)
tflite::MicroErrorReporter tflErrorReporter;

//  pull in all the TFLM ops, you can remove this line and
// only pull in the TFLM  ops you need, if would like to reduce
// the compiled size of the sketch.
tflite::ops::micro::AllOpsResolver  tflOpsResolver;


const tflite::Model* tflModel = nullptr;
tflite::MicroInterpreter*  tflInterpreter = nullptr;
TfLiteTensor* tflInputTensor = nullptr;
TfLiteTensor*  tflOutputTensor = nullptr;

// Create a static memory buffer for TFLM, the  size may need to
// be adjusted based on the model you are using
constexpr  int tensorArenaSize = 8 * 1024;
byte tensorArena[tensorArenaSize];

//  array to map gesture index to a name
const char* GESTURES[] = {
  "left",
  "right",
  "forward",
  "backward",
};
const int XDirection[]  = { 30, -30, 0, 0 };
const int YDirection[] = { 0, 0, 30, -30 };

#define  NUM_GESTURES (sizeof(GESTURES) / sizeof(GESTURES[0]))

BTooth BT;  // Bluetooth  Sender

void setup() {
  Serial.begin(9600);
  Serial1.begin(9600);

  // initialize the pushbutton pin as an input with pullup:
  pinMode(buttonPin,  INPUT_PULLUP);

  // initialize the IMU
  if (!IMU.begin()) {
    Serial.println("Failed  to initialize IMU!");
    while (1);
  }

  // print out the samples  rates of the IMUs
  Serial.print("Accelerometer sample rate = ");
  Serial.print(IMU.accelerationSampleRate());
  Serial.println(" Hz");
  Serial.print("Gyroscope sample rate = ");
  Serial.print(IMU.gyroscopeSampleRate());
  Serial.println(" Hz");

  Serial.println();

  // get the TFL representation of the model byte array
  tflModel = tflite::GetModel(model);
  if (tflModel->version() != TFLITE_SCHEMA_VERSION)  {
    Serial.println("Model schema mismatch!");
    while (1);
  }

  // Create an interpreter to run the model
  tflInterpreter = new tflite::MicroInterpreter(tflModel,  tflOpsResolver, tensorArena, tensorArenaSize, &tflErrorReporter);

  // Allocate  memory for the model's input and output tensors
  tflInterpreter->AllocateTensors();

  // Get pointers for the model's input and output tensors
  tflInputTensor =  tflInterpreter->input(0);
  tflOutputTensor = tflInterpreter->output(0);

  static bool is_initialized = false;
  if (!is_initialized) {
    pinMode(LED_BUILTIN,  OUTPUT);
    // Pins for the built-in RGB LEDs on the Arduino Nano 33 BLE Sense
    pinMode(LEDR, OUTPUT);
    pinMode(LEDG, OUTPUT);
    pinMode(LEDB, OUTPUT);
    // Ensure the LED is off by default.
    // Note: The RGB LEDs on the Arduino  Nano 33 BLE
    // Sense are on when the pin is LOW, off when HIGH.
    digitalWrite(LEDR,  HIGH);
    digitalWrite(LEDG, HIGH);
    digitalWrite(LEDB, HIGH);
    is_initialized  = true;

  }
}

void loop() {
  // read the state of the push  button pin:
  int buttonState = digitalRead(buttonPin);

  // compare the  button state to the previous state
  // to see if it has changed
  if (buttonState  != previousButtonState) {
    if (buttonState == LOW) {
      if (samplesRead  == numSamples) {
        // pressed       }
        samplesRead = 0;
        digitalWrite(LED_BUILTIN,  LOW);
        digitalWrite(LEDR, HIGH);
        digitalWrite(LEDG, HIGH);
        //       digitalWrite(LEDB, HIGH);
      }
    } else {
      //  released
      digitalWrite(LED_BUILTIN, HIGH);
      digitalWrite(LEDR, HIGH);
      digitalWrite(LEDG, HIGH);
    }

    // store the state as the previous  state, for the next loop
    previousButtonState = buttonState;
  }

  // check if the all the required samples have been read since
  // the last  time the button has been pressed
  if (samplesRead < numSamples) {
    //  check if new acceleration AND gyroscope data is available
    if (IMU.accelerationAvailable()  && IMU.gyroscopeAvailable()) {
      float aX, aY, aZ, gX, gY, gZ;

      //  read the acceleration and gyroscope data
      IMU.readAcceleration(aX, aY, aZ);
      IMU.readGyroscope(gX, gY, gZ);

      // normalize the IMU data between  0 to 1 and store in the model's
      // input tensor
      tflInputTensor->data.f[samplesRead  * 6 + 0] = (aX + 4.0) / 8.0;
      tflInputTensor->data.f[samplesRead * 6 + 1]  = (aY + 4.0) / 8.0;
      tflInputTensor->data.f[samplesRead * 6 + 2] = (aZ +  4.0) / 8.0;
      tflInputTensor->data.f[samplesRead * 6 + 3] = (gX + 2000.0)  / 4000.0;
      tflInputTensor->data.f[samplesRead * 6 + 4] = (gY + 2000.0) /  4000.0;
      tflInputTensor->data.f[samplesRead * 6 + 5] = (gZ + 2000.0) / 4000.0;

      samplesRead++;

      if (samplesRead == numSamples) {
        //  Run inferencing
        TfLiteStatus invokeStatus = tflInterpreter->Invoke();
        if (invokeStatus != kTfLiteOk) {
          Serial.println("Invoke failed!");
          while (1);
          return;
        }

        int result;
        result = -1;
        // Loop through the output tensor values from the  model
        for (int i = 0; i < NUM_GESTURES; i++) {
          Serial.print(GESTURES[i]);
          Serial.print(": ");
          Serial.print(tflOutputTensor->data.f[i],  6);
          Serial.print(" / ");
          if ( tflOutputTensor->data.f[i]  > 0.95) {
            result = i;
          }
        }
        Serial.println();
        Serial.print(" Direction : ");
        if (result >= 0 ) {
          Serial.println(GESTURES[result]);

          digitalWrite(LEDG, LOW);  // Green for yes
          BT.XYsend( XDirection[result],  YDirection[result]);   // Bluetooth send X Y
        }
        else {
          digitalWrite(LEDR, LOW);  // red for stop
          Serial.println("Stop");
          BT.XYsend( 0, 0); // Bluetooth send X Y
        }
      }
    }
  }
}

IMU_Capture

c_cpp

This example uses the on-board IMU to start reading acceleration and gyroscope data from on-board IMU and prints it to the Serial Monitor for one second when the button is pressed. Created by Don Coleman, Sandeep Mistry modified by Rolf Kurth

1/*
2  IMU Capture
3
4  This example uses the on-board IMU to start  reading acceleration and gyroscope
5  data from on-board IMU and prints it to  the Serial Monitor for one second
6  when the button is pressed.
7
8  You  can also use the Serial Plotter to graph the data.
9
10  The circuit:
11  -  Arduino Nano 33 BLE or Arduino Nano 33 BLE Sense board.
12  - Button connected  to pin 3 and GND.
13
14  Created by Don Coleman, Sandeep Mistry
15
16  modified  by Rolf Kurth
17
18  This example code is in the public domain.
19*/
20
21#include  <Arduino_LSM9DS1.h>
22
23const int buttonPin = 3;     // the number of the pushbutton  pin
24const int numSamples = 59;
25//const int numSamples = 119;
26
27int previousButtonState  = HIGH;
28int samplesRead = numSamples;
29
30void setup() {
31  Serial.begin(115200);
32  while (!Serial);
33
34  // initialize the pushbutton pin as an input with (internal)  pullup:
35  pinMode(buttonPin, INPUT_PULLUP);
36
37  if (!IMU.begin()) {
38    Serial.println("Failed to initialize IMU!");
39    while (1);
40  }
41  Serial.print("Gyroscope sample rate in degrees/second = ");
42  Serial.print(IMU.gyroscopeSampleRate());
43  Serial.println(" Hz");
44  Serial.println();
45 
46  Serial.print("Accelerometer  sample rate in G's = ");
47  Serial.print(IMU.accelerationSampleRate());
48  Serial.println("  Hz");
49  Serial.println();
50
51  // print the header
52  Serial.println("aX,aY,aZ,gX,gY,gZ");
53}
54
55void  loop() {
56  // read the state of the push button pin:
57  int buttonState = digitalRead(buttonPin);
58  // Serial.println(buttonState);
59  // compare the button state to the previous  state
60  // to see if it has changed
61  if (buttonState != previousButtonState)  {
62    if (buttonState == LOW) {
63      if (samplesRead == numSamples) {
64        // pressed
65        samplesRead = 0;
66      }
67    } else {
68      //  released
69    }
70
71    // store the state as the previous state, for the next  loop
72    previousButtonState = buttonState;
73  }
74
75  // check if the all  the required samples have been read since
76  // the last time the button has been  pressed
77  if (samplesRead < numSamples) {
78    // check if both new acceleration  and gyroscope data is
79    // available
80    if (IMU.accelerationAvailable()  && IMU.gyroscopeAvailable()) {
81      float aX, aY, aZ, gX, gY, gZ;
82
83      //  read the acceleration and gyroscope data
84      IMU.readAcceleration(aX, aY, aZ);
85      IMU.readGyroscope(gX, gY, gZ);
86
87      samplesRead++;
88
89      //  print the data in CSV format
90      Serial.print(aX, 3);
91      Serial.print(',');
92      Serial.print(aY, 3);
93      Serial.print(',');
94      Serial.print(aZ,  3);
95      Serial.print(',');
96      Serial.print(gX, 3);
97      Serial.print(',');
98      Serial.print(gY, 3);
99      Serial.print(',');
100      Serial.print(gZ,  3);
101      Serial.println();
102
103      if (samplesRead == numSamples) {
104        // add an empty line if it's the last sample
105        Serial.println();
106      }
107    }
108  }
109}
110
111// in extra Tab*************************************************
112
113#ifndef  BTooth_h
114#define  BTooth_h
115#include "Arduino.h"
116
117
118
119
120//**********************************************************************  /
121class BTooth
122///**********************************************************************/
123{
124  public:
125        
126    BTooth(); // Constructor
127
128    void XYsend(int  x, int y);
129
130};
131#endif
132// in extra Tab *************************************************
133
134#include  "BTooth.h"
135
136
137/**********************************************************************/
138BTooth::BTooth()
139/**********************************************************************/
140{}
141
142
143void   BTooth::XYsend(int x, int y ) {
144  int  Up, Down, Left , Right, JButton;
145  Serial1.print("X");
146  Serial1.print("#");  // delimiter
147  Serial1.print(x);    // X value
148  Serial1.print("#");
149  Serial1.print("Y");  // Y value
150  Serial1.print("#");
151  Serial1.print(y);
152  Serial1.print("#");
153  Serial1.print("B1");  // the rest compatible to my old Joy Stick solution
154  Serial1.print("#");
155  Serial1.print(JButton);
156  Serial1.print("#");
157  Serial1.print("Up");
158  Serial1.print("#");
159  Serial1.print(Up);
160  Serial1.print("#");
161  Serial1.print("Do");
162  Serial1.print("#");
163  Serial1.print(Down);
164  Serial1.print("#");
165  Serial1.print("Le");
166  Serial1.print("#");
167  Serial1.print(Left);
168  Serial1.print("#");
169  Serial1.print("Ri");
170  Serial1.print("#");
171  Serial1.print(Right);
172  Serial1.print("#");
173  Serial1.print('\n');
174
175}

/*
  IMU Capture

  This example uses the on-board IMU to start  reading acceleration and gyroscope
  data from on-board IMU and prints it to  the Serial Monitor for one second
  when the button is pressed.

  You  can also use the Serial Plotter to graph the data.

  The circuit:
  -  Arduino Nano 33 BLE or Arduino Nano 33 BLE Sense board.
  - Button connected  to pin 3 and GND.

  Created by Don Coleman, Sandeep Mistry

  modified  by Rolf Kurth

  This example code is in the public domain.
*/

#include  <Arduino_LSM9DS1.h>

const int buttonPin = 3;     // the number of the pushbutton  pin
const int numSamples = 59;
//const int numSamples = 119;

int previousButtonState  = HIGH;
int samplesRead = numSamples;

void setup() {
  Serial.begin(115200);
  while (!Serial);

  // initialize the pushbutton pin as an input with (internal)  pullup:
  pinMode(buttonPin, INPUT_PULLUP);

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }
  Serial.print("Gyroscope sample rate in degrees/second = ");
  Serial.print(IMU.gyroscopeSampleRate());
  Serial.println(" Hz");
  Serial.println();
 
  Serial.print("Accelerometer  sample rate in G's = ");
  Serial.print(IMU.accelerationSampleRate());
  Serial.println("  Hz");
  Serial.println();

  // print the header
  Serial.println("aX,aY,aZ,gX,gY,gZ");
}

void  loop() {
  // read the state of the push button pin:
  int buttonState = digitalRead(buttonPin);
  // Serial.println(buttonState);
  // compare the button state to the previous  state
  // to see if it has changed
  if (buttonState != previousButtonState)  {
    if (buttonState == LOW) {
      if (samplesRead == numSamples) {
        // pressed
        samplesRead = 0;
      }
    } else {
      //  released
    }

    // store the state as the previous state, for the next  loop
    previousButtonState = buttonState;
  }

  // check if the all  the required samples have been read since
  // the last time the button has been  pressed
  if (samplesRead < numSamples) {
    // check if both new acceleration  and gyroscope data is
    // available
    if (IMU.accelerationAvailable()  && IMU.gyroscopeAvailable()) {
      float aX, aY, aZ, gX, gY, gZ;

      //  read the acceleration and gyroscope data
      IMU.readAcceleration(aX, aY, aZ);
      IMU.readGyroscope(gX, gY, gZ);

      samplesRead++;

      //  print the data in CSV format
      Serial.print(aX, 3);
      Serial.print(',');
      Serial.print(aY, 3);
      Serial.print(',');
      Serial.print(aZ,  3);
      Serial.print(',');
      Serial.print(gX, 3);
      Serial.print(',');
      Serial.print(gY, 3);
      Serial.print(',');
      Serial.print(gZ,  3);
      Serial.println();

      if (samplesRead == numSamples) {
        // add an empty line if it's the last sample
        Serial.println();
      }
    }
  }
}

// in extra Tab*************************************************

#ifndef  BTooth_h
#define  BTooth_h
#include "Arduino.h"




//**********************************************************************  /
class BTooth
///**********************************************************************/
{
  public:
        
    BTooth(); // Constructor

    void XYsend(int  x, int y);

};
#endif
// in extra Tab *************************************************

#include  "BTooth.h"


/**********************************************************************/
BTooth::BTooth()
/**********************************************************************/
{}


void   BTooth::XYsend(int x, int y ) {
  int  Up, Down, Left , Right, JButton;
  Serial1.print("X");
  Serial1.print("#");  // delimiter
  Serial1.print(x);    // X value
  Serial1.print("#");
  Serial1.print("Y");  // Y value
  Serial1.print("#");
  Serial1.print(y);
  Serial1.print("#");
  Serial1.print("B1");  // the rest compatible to my old Joy Stick solution
  Serial1.print("#");
  Serial1.print(JButton);
  Serial1.print("#");
  Serial1.print("Up");
  Serial1.print("#");
  Serial1.print(Up);
  Serial1.print("#");
  Serial1.print("Do");
  Serial1.print("#");
  Serial1.print(Down);
  Serial1.print("#");
  Serial1.print("Le");
  Serial1.print("#");
  Serial1.print(Left);
  Serial1.print("#");
  Serial1.print("Ri");
  Serial1.print("#");
  Serial1.print(Right);
  Serial1.print("#");
  Serial1.print('\n');

}

Bluetooth Receiver from self balancing robot

c_cpp

1/* Self balancing Robot via Stepper Motor with microstepping and Digital  Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4*/
5
6String  inputString = "";         // a String to hold incoming data
7bool   stringComplete  = false;  // whether the string is complete
8char   c = ' ';
9
10/*
11    SerialEvent  occurs whenever a new data comes in the hardware serial RX. This
12    routine  is run between each time loop() runs, so using delay inside loop can
13    delay  response. Multiple bytes of data may be available.
14*/
15void serialEvent1()  {
16  while (Serial1.available()) {
17    // get the new byte:
18    char inChar  = (char)Serial1.read();
19    // add it to the inputString:
20    if (!stringComplete)  {
21      inputString += inChar;
22    }
23    // if the incoming character is  a newline, set a flag so the main loop can
24    // do something about it:
25    if (inChar == '\n') {
26      stringComplete = true;
27    }
28  }
29}
30
31void  BTRead( JStickData &JSData  ) {
32  String       command;
33  unsigned int j;
34  long         value;
35
36
37  // print the string when a newline arrives:
38  if (stringComplete) {
39    if (inputString.substring(0, 1) != "X")
40    {
41      Serial.print("Error reading Bluetooth ");
42      Serial.println(inputString);
43    } else {
44      j = 0;
45      for (unsigned int i = 0; i < inputString.length();  i++) {
46
47        if (inputString.substring(i, i + 1) == "#") {
48          command  = inputString.substring(j, i);
49          //Serial.print("Command: ");
50          //Serial.print(command);
51          j = i + 1;
52          for (unsigned int i1 = j; i1 < inputString.length();  i1++) {
53            if (inputString.substring(i1, i1 + 1) == "#") {
54              value  = inputString.substring(j, i1).toInt();
55              //Serial.print(" Value:  ");
56              //Serial.println(value);
57              i = i1;
58              j  = i + 1;
59              assignValues(command, value, JSData);
60              break;
61            }
62          }
63        }
64      }
65    }
66    inputString  = "";
67    stringComplete = false;
68    // Serial.print(" Value: ");
69    // Serial.println(JStick.Xvalue);
70  }
71}
72
73void assignValues(String  icommand, int ivalue, JStickData &Data  ) {
74
75  if (icommand == "X")  Data.Xvalue  = ivalue;
76  if (icommand == "Y")  Data.Yvalue  = ivalue;
77  if (icommand  == "B1") Data.JButton = ivalue;
78  if (icommand == "Up") Data.Up      = ivalue;
79  if (icommand == "Do") Data.Down    = ivalue;
80  if (icommand == "Ri") Data.Right   = ivalue;
81  if (icommand == "Le") Data.Left    = ivalue;
82
83}

/* Self balancing Robot via Stepper Motor with microstepping and Digital  Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/

String  inputString = "";         // a String to hold incoming data
bool   stringComplete  = false;  // whether the string is complete
char   c = ' ';

/*
    SerialEvent  occurs whenever a new data comes in the hardware serial RX. This
    routine  is run between each time loop() runs, so using delay inside loop can
    delay  response. Multiple bytes of data may be available.
*/
void serialEvent1()  {
  while (Serial1.available()) {
    // get the new byte:
    char inChar  = (char)Serial1.read();
    // add it to the inputString:
    if (!stringComplete)  {
      inputString += inChar;
    }
    // if the incoming character is  a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}

void  BTRead( JStickData &JSData  ) {
  String       command;
  unsigned int j;
  long         value;


  // print the string when a newline arrives:
  if (stringComplete) {
    if (inputString.substring(0, 1) != "X")
    {
      Serial.print("Error reading Bluetooth ");
      Serial.println(inputString);
    } else {
      j = 0;
      for (unsigned int i = 0; i < inputString.length();  i++) {

        if (inputString.substring(i, i + 1) == "#") {
          command  = inputString.substring(j, i);
          //Serial.print("Command: ");
          //Serial.print(command);
          j = i + 1;
          for (unsigned int i1 = j; i1 < inputString.length();  i1++) {
            if (inputString.substring(i1, i1 + 1) == "#") {
              value  = inputString.substring(j, i1).toInt();
              //Serial.print(" Value:  ");
              //Serial.println(value);
              i = i1;
              j  = i + 1;
              assignValues(command, value, JSData);
              break;
            }
          }
        }
      }
    }
    inputString  = "";
    stringComplete = false;
    // Serial.print(" Value: ");
    // Serial.println(JStick.Xvalue);
  }
}

void assignValues(String  icommand, int ivalue, JStickData &Data  ) {

  if (icommand == "X")  Data.Xvalue  = ivalue;
  if (icommand == "Y")  Data.Yvalue  = ivalue;
  if (icommand  == "B1") Data.JButton = ivalue;
  if (icommand == "Up") Data.Up      = ivalue;
  if (icommand == "Do") Data.Down    = ivalue;
  if (icommand == "Ri") Data.Right   = ivalue;
  if (icommand == "Le") Data.Left    = ivalue;

}

Downloadable files

sbrobotv03export_CNOGijFIPd.fzz

Comments

Only logged in users can leave comments

RolfK

0 Followers

•

0 Projects

Intro