Predict orientation

  1'''
  2TinyML - predict_orientation.py.
  3
  4This example demonstrates how to detect the acceleration or the rotation of movement from time-series data.
  5
  6The read_and_predict function uses measured acceleration and orientation to predict the movement.
  7Then, It will predict four situation:
  8-which is Roll, Pitch, Yaw and No Predicted Orientation.
  9The output is the Led, which will be RED for Roll, GREEN for Pitch and BLUE for Yaw.
 10
 11For other examples please check:
 12    https://github.com/WPC-Systems-Ltd/WPC_Stand-alone_Python_release/tree/main/examples
 13
 14Copyright (c) 2024 WPC Systems Ltd.
 15All rights reserved.
 16'''
 17
 18## Standard
 19import time
 20from ulab import numpy as np
 21
 22## WPC
 23import pywpc
 24
 25## TinyML
 26from test_tree_acc import WPC_test_acceleration_tree
 27
 28## Parameters
 29DR = np.pi / 180
 30
 31class Queue:
 32    '''First In, First Out - FIFO'''
 33    def __init__(self, n):
 34        self.list = np.zeros(n)
 35        self.size = n
 36        self.head = 0
 37
 38    def add(self, item):
 39        self.list[self.head] = item
 40        self.head = (self.head + 1) % self.size
 41
 42def most_frequent_element(list, return_max_freq = False):
 43    '''Returns most frequent element of a list, and optionally, the frequency of this element'''
 44    if not list:  # For the case of an empty list
 45        return (None, 0) if return_max_freq else None
 46    most_freq_el = list[0]
 47    max_freq = 1
 48    h =  {}
 49    for items in list:
 50        if items not in h:
 51            h[items] = 1
 52        else:
 53            h[items] += 1
 54
 55        if h[items] > max_freq:
 56            max_freq = h[items]
 57            most_freq_el = items
 58    if return_max_freq:
 59        return most_freq_el, max_freq
 60    return most_freq_el
 61
 62def initialization(model):
 63    '''defines all the variables for prediction'''
 64    global data_acc, data_paste_gyr, data_current_gyr, data_diff_gyr, pred_acc, pred_gyr, wide_tab, MODEL, MAP, MAP_LED
 65    data_acc = np.zeros(3)
 66
 67    data_past_gyr = np.zeros(3)
 68    data_current_gyr = np.zeros(3)
 69    data_diff_gyr = data_current_gyr - data_past_gyr
 70
 71    pred_acc = Queue(7)
 72    pred_gyr = Queue(7)
 73
 74    '''wide_tab is the "concatenation" of seven current tabs, 6x7 = 42, hence a such result'''
 75    wide_tab = np.zeros(42)
 76
 77    MODEL = model
 78
 79    MAP_LED = {0: pywpc.LED_reset, 1: pywpc.LED_setGreen, 2: pywpc.LED_setRed, 3: pywpc.LED_setBlue}
 80
 81def read_and_predict(g):
 82    '''it reads the current data, and predicts the movement based on the last 7 data read'''
 83    global data_acc, data_current_gyr, data_diff_gyr, wide_tab
 84    t0 = time.ticks_ms()
 85
 86    '''Read AI data.'''
 87    data_past_gyr = data_current_gyr
 88    data_current_gyr = np.array(pywpc.AHRS_getOrientation())
 89    data_diff_gyr = data_current_gyr - data_past_gyr
 90
 91    '''remove the gravity component'''
 92    data_acc = np.array(pywpc.AHRS_getAcceleration()) - 9.81*np.array([np.sin(DR*data_current_gyr[1]),
 93                                                np.sin(DR*data_current_gyr[0])*np.cos(DR*data_current_gyr[1]),
 94                                                np.cos(DR*data_current_gyr[0])*np.cos(DR*data_current_gyr[1])])
 95
 96    wide_tab = np.concatenate((data_acc, data_diff_gyr, wide_tab[:-6]))
 97
 98    pred = MODEL(wide_tab)
 99
100    return pred
101
102def main():
103    '''predicts movement for 10 iterations, and return the most frequent prediction'''
104    pywpc.AHRS_start()
105    pywpc.LED_reset()
106    initialization(model = WPC_test_orientation_tree)
107    time.sleep(1)
108
109    preds = Queue(10)
110    start = time.ticks_ms()
111    while True:
112        preds.add(read_and_predict(0))
113        time.sleep_ms(45)
114        end = time.ticks_ms()
115        print(time.ticks_diff(end, start))
116        start = time.ticks_ms()
117        '''ROLL: Red ; Pitch: GREEN ; YAW: Blue, No Orientation Detected: No Color'''
118        MAP_LED[most_frequent_element(preds.list)]()
119    pywpc.AHRS_stop()
120print('[Main] Starting script')
121
122main()