Tennis Picker

void gesture_task(void *arg)
{
#if !GESTURE_DATA_COLLECTION_MODE
    /* Regression pointers */
    MTB_ML_DATA_T *input_reference;

#endif

    /* Data processed in floating point */
    float data_feed[SENSOR_BATCH_SIZE][SENSOR_NUM_AXIS];

    (void)arg;

    /* Initialize the butter-worth filter variables */
    int n_order = 3;
    /* Coefficients for 3rd order butter-worth filter */
    const float coeff_b[] = IIR_FILTER_BUTTER_WORTH_COEFF_B;
    const float coeff_a[] = IIR_FILTER_BUTTER_WORTH_COEFF_A;
    iir_filter_struct butter_lp_fil;

    for(;;)
    {
        uint16_t cur = 0;
        int16_t temp_buffer[SENSOR_BATCH_SIZE][SENSOR_NUM_AXIS];

        /* Get sensor data */
        sensor_get_data((void *) temp_buffer);

        /* Cast the data from an int16 to a float for pre-processing */
        cast_int16_to_float(&temp_buffer[0][0], &data_feed[0][0], SENSOR_BATCH_SIZE*SENSOR_NUM_AXIS);

        /* Third order butter-worth filter */
        while(cur < SENSOR_NUM_AXIS)
        {
            /* Initialize and run the filter */
            iir_filter_init(&butter_lp_fil, coeff_b, coeff_a, n_order);
            iir_filter(&butter_lp_fil, &data_feed[0][0], SENSOR_BATCH_SIZE, cur, SENSOR_NUM_AXIS);
            cur++;
        }

        /* A min max normalization to get all data between -1 and 1 */
        normalization_min_max(&data_feed[0][0], SENSOR_BATCH_SIZE, SENSOR_NUM_AXIS, MIN_DATA_SAMPLE, MAX_DATA_SAMPLE);

#ifdef CY_BMI_160_IMU_I2C
        /* Swap axis for BMI_160 so board orientation stays the same */
        column_inverse(&data_feed[0][0], SENSOR_BATCH_SIZE, SENSOR_NUM_AXIS, 2);
        column_swap(&data_feed[0][0], SENSOR_BATCH_SIZE, SENSOR_NUM_AXIS, 0, 1);
        column_inverse(&data_feed[0][0], SENSOR_BATCH_SIZE, SENSOR_NUM_AXIS, 5);
        column_swap(&data_feed[0][0], SENSOR_BATCH_SIZE, SENSOR_NUM_AXIS, 3, 4);
#endif

#if GESTURE_DATA_COLLECTION_MODE
    cur = 0;
    printf("-,-,-,-,-,-\r\n");
    while (cur < SENSOR_BATCH_SIZE)
    {
        printf("%6f,%6f,%6f,%6f,%6f,%6f\r\n", data_feed[cur][0],
                                              data_feed[cur][1],
                                              data_feed[cur][2],
                                              data_feed[cur][3],
                                              data_feed[cur][4],
                                              data_feed[cur][5]);
        cur++;
    }
#else

#if !COMPONENT_ML_FLOAT32
        /* Quantize data before feeding model */
        MTB_ML_DATA_T data_feed_int[SENSOR_BATCH_SIZE][SENSOR_NUM_AXIS];
        mtb_ml_utils_model_quantize(magic_wand_obj, &data_feed[0][0], &data_feed_int[0][0]);

        /* Feed the Model */
        input_reference = (MTB_ML_DATA_T *) data_feed_int;
        mtb_ml_model_run(magic_wand_obj, input_reference);
        control(result_buffer, model_output_size);

#else
        input_reference = (MTB_ML_DATA_T *) data_feed;
        mtb_ml_model_run(magic_wand_obj, input_reference);
        control(result_buffer, model_output_size);
#endif

#endif /* #if GESTURE_DATA_COLLECTION */
    }
}

void control(MTB_ML_DATA_T* result_buffer, int model_output_size)
{
    /* Get the class with the highest confidence */
    int class_index = mtb_ml_utils_find_max(result_buffer, model_output_size);

#if !COMPONENT_ML_FLOAT32
    /* Convert 16bit fixed-point output to floating-point for visualization */
    float *nn_float_buffer = (float *) malloc(model_output_size * sizeof(float));
    mtb_ml_utils_model_dequantize(magic_wand_obj, nn_float_buffer);
#else
    float *nn_float_buffer = result_buffer;
#endif

    /* Clear the screen */
    printf("\x1b[2J\x1b[;H");

    /* Prints the confidence level of each class */
    printf("| Gesture         | Confidence\r\n");
    printf("--------------------------------\r\n");
    printf("| %s:", gesture_one);
    printf("%s %%%-3d\r\n", dash_ges_one, (int)(nn_float_buffer[0]*100 + 0.5));
    printf("--------------------------------\r\n");
    printf("| %s:", gesture_two);
    printf("%s %%%-3d\r\n", dash_ges_two, (int)(nn_float_buffer[1]*100 + 0.5));
    printf("--------------------------------\r\n");
    printf("| %s:", gesture_three);
    printf("%s %%%-3d\r\n", dash_ges_three, (int)(nn_float_buffer[2]*100 + 0.5));
    printf("--------------------------------\r\n");
    printf("| %s:", gesture_four);
    printf("%s %%%-3d\r\n", dash_ges_four, (int)(nn_float_buffer[3]*100 + 0.5));
    printf("--------------------------------\r\n");
    printf("| Detection:        ");

    /* Check the confidence for the selected class */
    if(MIN_CONFIDENCE < nn_float_buffer[class_index])
    {
        /* Switch statement for the selected class */
        switch (class_index)
        {
            case 0:
                printf("%s\r\n", gesture_one);
                //const char gesture_one[] = "Circle";
                cyhal_gpio_write(CYBSP_D9, CYBSP_LED_STATE_ON);
                motor(1);
                break;
            case 1:
                printf("%s\r\n", gesture_two);
                cyhal_gpio_write(CYBSP_D10,  CYBSP_LED_STATE_ON);
                //const char gesture_two[] = "Side-to-Side";
                motor(2);
                break;
            case 2:
                printf("%s\r\n", gesture_three);
                cyhal_gpio_write(CYBSP_D11,  CYBSP_LED_STATE_ON);
                //const char gesture_three[] = "Square";
                motor(3);
                break;
            case 3:
                printf("%s\r\n", gesture_four);
                //cyhal_gpio_write(CYBSP_D3, CYBSP_LED_STATE_ON);
                //const char gesture_four[] = "negative";
                motor(4);
                break;
        }

    }
    /* If the confidence is not high, no gesture detected */
    else
    {
        printf("%s\r\n", gesture_four);
    }
    free(nn_float_buffer);
}