MFR16 / Mbed 2 deprecated Servo_6180XA1

Sweep a servo according to Proximity sensor measure

Dependencies:   Servo X_NUCLEO_6180XA1 mbed

Fork of HelloWorld_6180XA1 by ST

Middlewares/ST/STM32_HMI_HandGesture/HmiBBGesture.cpp@14:946e62f44f4f, 2015-10-27 (annotated)

Committer:
gallonm
Date:
Tue Oct 27 15:54:05 2015 +0000
Revision:
14:946e62f44f4f
Updated all files. Added midlewares

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gallonm 14:946e62f44f4f 1 /* ----------------------------------------------------------------------
gallonm 14:946e62f44f4f 2 * Copyright (C) 2014 STMicroelectronics. All rights reserved.
gallonm 14:946e62f44f4f 3 *
gallonm 14:946e62f44f4f 4 * Project: BB gesture recognition
gallonm 14:946e62f44f4f 5 * Title: HmiBBGesture
gallonm 14:946e62f44f4f 6 *
gallonm 14:946e62f44f4f 7 * Description: Gesture recognition - left, right, tap
gallonm 14:946e62f44f4f 8 *
gallonm 14:946e62f44f4f 9 * 20/10/14
gallonm 14:946e62f44f4f 10 * Changed from proximity to range
gallonm 14:946e62f44f4f 11 * Ultra simplified version
gallonm 14:946e62f44f4f 12 *
gallonm 14:946e62f44f4f 13 * -------------------------------------------------------------------- */
gallonm 14:946e62f44f4f 14
gallonm 14:946e62f44f4f 15 //#define PRINT_DEBUG_
gallonm 14:946e62f44f4f 16 #define USE_MICRO_
gallonm 14:946e62f44f4f 17
gallonm 14:946e62f44f4f 18 #include "HmiBBGesture.h"
gallonm 14:946e62f44f4f 19 #include "stm32f4xx_hal.h"
gallonm 14:946e62f44f4f 20 #include <stdint.h>
gallonm 14:946e62f44f4f 21 #include <stdio.h>
gallonm 14:946e62f44f4f 22
gallonm 14:946e62f44f4f 23 #ifdef USE_MICRO_
gallonm 14:946e62f44f4f 24 //#include "vl6180x_shield.h"
gallonm 14:946e62f44f4f 25 //#include "platform.h"
gallonm 14:946e62f44f4f 26 //#define GET_TIME_STAMP() ((int)timer_get_clock_time_msecs()) // for Mbed
gallonm 14:946e62f44f4f 27 #define GET_TIME_STAMP() (int32_t)HAL_GetTick() // for Cube
gallonm 14:946e62f44f4f 28 #else
gallonm 14:946e62f44f4f 29 /* PC config */
gallonm 14:946e62f44f4f 30 #include <windows.h>
gallonm 14:946e62f44f4f 31 #define GET_TIME_STAMP() ((int)timeGetTime())
gallonm 14:946e62f44f4f 32 #endif
gallonm 14:946e62f44f4f 33
gallonm 14:946e62f44f4f 34 #ifdef PRINT_DEBUG_
gallonm 14:946e62f44f4f 35 #ifdef USE_MICRO_
gallonm 14:946e62f44f4f 36 static char str[80];
gallonm 14:946e62f44f4f 37 //extern void serial_print(const char *str);
gallonm 14:946e62f44f4f 38 #define serial_print(str) printf(str)
gallonm 14:946e62f44f4f 39 #endif
gallonm 14:946e62f44f4f 40 #endif
gallonm 14:946e62f44f4f 41
gallonm 14:946e62f44f4f 42 #define HMI_SIGNAL_CODE_DROP_DOWN 1
gallonm 14:946e62f44f4f 43 #define HMI_SIGNAL_CODE_RAISE_UP 2
gallonm 14:946e62f44f4f 44 #define HMI_SIGNAL_CODE_DOWN_STATE 3
gallonm 14:946e62f44f4f 45 #define HMI_SIGNAL_CODE_UP_STATE 4
gallonm 14:946e62f44f4f 46 #define HMI_SIGNAL_CODE_NULL -1
gallonm 14:946e62f44f4f 47
gallonm 14:946e62f44f4f 48 #define HMI_TAP_CODE_SINGLE 0
gallonm 14:946e62f44f4f 49 #define HMI_TAP_CODE_DOUBLE 1
gallonm 14:946e62f44f4f 50 #define HMI_TAP_CODE_HOLD 2
gallonm 14:946e62f44f4f 51 #define HMI_TAP_CODE_NULL -1
gallonm 14:946e62f44f4f 52
gallonm 14:946e62f44f4f 53
gallonm 14:946e62f44f4f 54
gallonm 14:946e62f44f4f 55
gallonm 14:946e62f44f4f 56 /**********************************************************************************
gallonm 14:946e62f44f4f 57 *
gallonm 14:946e62f44f4f 58 *
gallonm 14:946e62f44f4f 59 * Gesture recognition class
gallonm 14:946e62f44f4f 60 *
gallonm 14:946e62f44f4f 61 *
gallonm 14:946e62f44f4f 62 **********************************************************************************/
gallonm 14:946e62f44f4f 63
gallonm 14:946e62f44f4f 64
gallonm 14:946e62f44f4f 65 void HmiBBGesture::Init(int threshold_mm, int tap_threshold_mm, int min_hold_duration_ms, int min_swipe_duration_ms, int max_gesture_duration_ms )
gallonm 14:946e62f44f4f 66 {
gallonm 14:946e62f44f4f 67 m_motion_r.Init( threshold_mm );
gallonm 14:946e62f44f4f 68 m_motion_l.Init( threshold_mm );
gallonm 14:946e62f44f4f 69 m_tap_det.Init( tap_threshold_mm, 150, min_hold_duration_ms, false, 0 );
gallonm 14:946e62f44f4f 70 m_fsm_state = 0;
gallonm 14:946e62f44f4f 71 m_timestamp = GET_TIME_STAMP();
gallonm 14:946e62f44f4f 72 m_min_swipe_ms = min_swipe_duration_ms;
gallonm 14:946e62f44f4f 73 m_max_gesture_ms = max_gesture_duration_ms;
gallonm 14:946e62f44f4f 74 m_gesture_starts_from_right = false;
gallonm 14:946e62f44f4f 75 }
gallonm 14:946e62f44f4f 76
gallonm 14:946e62f44f4f 77 int HmiBBGesture::Update(int range_r, int range_l, int *gesture_duration_ms)
gallonm 14:946e62f44f4f 78 {
gallonm 14:946e62f44f4f 79 int r_code, r_ms;
gallonm 14:946e62f44f4f 80 int l_code, l_ms;
gallonm 14:946e62f44f4f 81 int duration;
gallonm 14:946e62f44f4f 82 int gesture_code = HMI_BB_GESTURE_CODE_NULL;
gallonm 14:946e62f44f4f 83 *gesture_duration_ms = 0;
gallonm 14:946e62f44f4f 84
gallonm 14:946e62f44f4f 85 r_code = m_motion_r.Update( range_r , &r_ms );
gallonm 14:946e62f44f4f 86 l_code = m_motion_l.Update( range_l , &l_ms );
gallonm 14:946e62f44f4f 87 // printf ("r_ms: %d l_ms: %d\n\r",r_ms, l_ms);
gallonm 14:946e62f44f4f 88 switch( m_fsm_state )
gallonm 14:946e62f44f4f 89 {
gallonm 14:946e62f44f4f 90 case 1: // gesture ends
gallonm 14:946e62f44f4f 91
gallonm 14:946e62f44f4f 92 duration = GET_TIME_STAMP() - m_timestamp;
gallonm 14:946e62f44f4f 93 if(duration > m_max_gesture_ms) // gesture is too long - discard it
gallonm 14:946e62f44f4f 94 {
gallonm 14:946e62f44f4f 95 m_fsm_state = 0;
gallonm 14:946e62f44f4f 96 #ifdef PRINT_DEBUG_
gallonm 14:946e62f44f4f 97
gallonm 14:946e62f44f4f 98 #ifdef USE_MICRO_
gallonm 14:946e62f44f4f 99 sprintf(str, "FSM 1 - Discarded %d %d %d mts %d\n",r_code,l_code,duration, m_timestamp);
gallonm 14:946e62f44f4f 100 serial_print(str);
gallonm 14:946e62f44f4f 101 #else
gallonm 14:946e62f44f4f 102 printf("FSM 1 - Discarded %d %d %d \n",r_code,l_code,duration);
gallonm 14:946e62f44f4f 103 #endif
gallonm 14:946e62f44f4f 104
gallonm 14:946e62f44f4f 105 #endif
gallonm 14:946e62f44f4f 106 }
gallonm 14:946e62f44f4f 107 else if( (m_gesture_starts_from_right && l_code == HMI_SIGNAL_CODE_RAISE_UP ) ||
gallonm 14:946e62f44f4f 108 (m_gesture_starts_from_right==false && r_code == HMI_SIGNAL_CODE_RAISE_UP) )
gallonm 14:946e62f44f4f 109 {
gallonm 14:946e62f44f4f 110 m_fsm_state = 0;
gallonm 14:946e62f44f4f 111 if( duration > (m_min_swipe_ms/2) )
gallonm 14:946e62f44f4f 112 {
gallonm 14:946e62f44f4f 113 gesture_code = ( m_gesture_starts_from_right ) ? HMI_BB_GESTURE_CODE_RIGHT : HMI_BB_GESTURE_CODE_LEFT;
gallonm 14:946e62f44f4f 114 *gesture_duration_ms = duration;
gallonm 14:946e62f44f4f 115 #ifdef PRINT_DEBUG_
gallonm 14:946e62f44f4f 116
gallonm 14:946e62f44f4f 117 #ifdef USE_MICRO_
gallonm 14:946e62f44f4f 118 sprintf(str,"FSM 2 - Recognized %d\n",gesture_code);
gallonm 14:946e62f44f4f 119 serial_print(str);
gallonm 14:946e62f44f4f 120 #else
gallonm 14:946e62f44f4f 121 printf("FSM 2 - Recognized %d\n",gesture_code);
gallonm 14:946e62f44f4f 122 #endif
gallonm 14:946e62f44f4f 123
gallonm 14:946e62f44f4f 124 #endif
gallonm 14:946e62f44f4f 125 }
gallonm 14:946e62f44f4f 126 #ifdef PRINT_DEBUG_
gallonm 14:946e62f44f4f 127 else
gallonm 14:946e62f44f4f 128 {
gallonm 14:946e62f44f4f 129 #ifdef USE_MICRO_
gallonm 14:946e62f44f4f 130 sprintf(str,"FSM 1 - Discarded %d %d %d \n",r_code,l_code,duration);
gallonm 14:946e62f44f4f 131 serial_print(str);
gallonm 14:946e62f44f4f 132 #else
gallonm 14:946e62f44f4f 133 printf("FSM 1 - Discarded %d %d %d \n",r_code,l_code,duration);
gallonm 14:946e62f44f4f 134 #endif
gallonm 14:946e62f44f4f 135 }
gallonm 14:946e62f44f4f 136 #endif
gallonm 14:946e62f44f4f 137 }
gallonm 14:946e62f44f4f 138
gallonm 14:946e62f44f4f 139 break;
gallonm 14:946e62f44f4f 140
gallonm 14:946e62f44f4f 141 case 0: // gesture starts
gallonm 14:946e62f44f4f 142 if( l_code == HMI_SIGNAL_CODE_DOWN_STATE && r_code == HMI_SIGNAL_CODE_RAISE_UP && r_ms > m_min_swipe_ms )
gallonm 14:946e62f44f4f 143 {
gallonm 14:946e62f44f4f 144 m_gesture_starts_from_right = true;
gallonm 14:946e62f44f4f 145 m_fsm_state = 1;
gallonm 14:946e62f44f4f 146 m_timestamp = GET_TIME_STAMP();
gallonm 14:946e62f44f4f 147 m_var_max_gesture_ms = r_ms;
gallonm 14:946e62f44f4f 148 #ifdef PRINT_DEBUG_
gallonm 14:946e62f44f4f 149
gallonm 14:946e62f44f4f 150 #ifdef USE_MICRO_
gallonm 14:946e62f44f4f 151 sprintf(str,"FSM 0 - Right started %d\n",r_ms);
gallonm 14:946e62f44f4f 152 serial_print(str);
gallonm 14:946e62f44f4f 153 #else
gallonm 14:946e62f44f4f 154 printf("FSM 0 - Right started %d\n",r_ms);
gallonm 14:946e62f44f4f 155 #endif
gallonm 14:946e62f44f4f 156
gallonm 14:946e62f44f4f 157 #endif
gallonm 14:946e62f44f4f 158 }
gallonm 14:946e62f44f4f 159 else if( l_code == HMI_SIGNAL_CODE_RAISE_UP && r_code == HMI_SIGNAL_CODE_DOWN_STATE && l_ms > m_min_swipe_ms )
gallonm 14:946e62f44f4f 160 {
gallonm 14:946e62f44f4f 161 m_gesture_starts_from_right = false;
gallonm 14:946e62f44f4f 162 m_fsm_state = 1;
gallonm 14:946e62f44f4f 163 m_timestamp = GET_TIME_STAMP();
gallonm 14:946e62f44f4f 164 m_var_max_gesture_ms = l_ms;
gallonm 14:946e62f44f4f 165 #ifdef PRINT_DEBUG_
gallonm 14:946e62f44f4f 166
gallonm 14:946e62f44f4f 167 #ifdef USE_MICRO_
gallonm 14:946e62f44f4f 168 sprintf(str,"FSM 0 - Left started %d\n",l_ms);
gallonm 14:946e62f44f4f 169 serial_print(str);
gallonm 14:946e62f44f4f 170 #else
gallonm 14:946e62f44f4f 171 printf("FSM 0 - Left started %d\n",l_ms);
gallonm 14:946e62f44f4f 172 #endif
gallonm 14:946e62f44f4f 173
gallonm 14:946e62f44f4f 174 #endif
gallonm 14:946e62f44f4f 175 }
gallonm 14:946e62f44f4f 176 break;
gallonm 14:946e62f44f4f 177
gallonm 14:946e62f44f4f 178 default:
gallonm 14:946e62f44f4f 179 break;
gallonm 14:946e62f44f4f 180 };
gallonm 14:946e62f44f4f 181
gallonm 14:946e62f44f4f 182 if( m_tap_det.Update((range_r+range_l)/2,&duration) == HMI_TAP_CODE_HOLD )
gallonm 14:946e62f44f4f 183 {
gallonm 14:946e62f44f4f 184 gesture_code = HMI_BB_GESTURE_CODE_TAP;
gallonm 14:946e62f44f4f 185 *gesture_duration_ms = duration;
gallonm 14:946e62f44f4f 186 m_fsm_state = 0;
gallonm 14:946e62f44f4f 187 }
gallonm 14:946e62f44f4f 188
gallonm 14:946e62f44f4f 189 return gesture_code;
gallonm 14:946e62f44f4f 190 }
gallonm 14:946e62f44f4f 191
gallonm 14:946e62f44f4f 192
gallonm 14:946e62f44f4f 193 /**********************************************************************************
gallonm 14:946e62f44f4f 194 *
gallonm 14:946e62f44f4f 195 *
gallonm 14:946e62f44f4f 196 * MotionDetector class
gallonm 14:946e62f44f4f 197 *
gallonm 14:946e62f44f4f 198 *
gallonm 14:946e62f44f4f 199 **********************************************************************************/
gallonm 14:946e62f44f4f 200
gallonm 14:946e62f44f4f 201 void MotionDetector::Init(int threshold)
gallonm 14:946e62f44f4f 202 {
gallonm 14:946e62f44f4f 203 m_threshold = threshold;
gallonm 14:946e62f44f4f 204 m_runonce = true;
gallonm 14:946e62f44f4f 205 }
gallonm 14:946e62f44f4f 206
gallonm 14:946e62f44f4f 207
gallonm 14:946e62f44f4f 208 int MotionDetector::Update(int sample,int *duration)
gallonm 14:946e62f44f4f 209 {
gallonm 14:946e62f44f4f 210 int return_code;
gallonm 14:946e62f44f4f 211 bool belowCurrent, isContinuous;
gallonm 14:946e62f44f4f 212
gallonm 14:946e62f44f4f 213
gallonm 14:946e62f44f4f 214 // run once
gallonm 14:946e62f44f4f 215 if( m_runonce )
gallonm 14:946e62f44f4f 216 {
gallonm 14:946e62f44f4f 217 m_runonce = false;
gallonm 14:946e62f44f4f 218 m_timestamp = GET_TIME_STAMP();
gallonm 14:946e62f44f4f 219 m_prev_sample = sample;
gallonm 14:946e62f44f4f 220 belowCurrent = (sample < m_threshold);
gallonm 14:946e62f44f4f 221 isContinuous = true;
gallonm 14:946e62f44f4f 222 return_code = ( belowCurrent ) ? HMI_SIGNAL_CODE_DOWN_STATE : HMI_SIGNAL_CODE_UP_STATE;
gallonm 14:946e62f44f4f 223 *duration = 1;
gallonm 14:946e62f44f4f 224 return return_code;
gallonm 14:946e62f44f4f 225 }
gallonm 14:946e62f44f4f 226
gallonm 14:946e62f44f4f 227 *duration = 0;
gallonm 14:946e62f44f4f 228 return_code = HMI_SIGNAL_CODE_NULL;
gallonm 14:946e62f44f4f 229 belowCurrent = (sample < m_threshold);
gallonm 14:946e62f44f4f 230 isContinuous = ( belowCurrent == (m_prev_sample < m_threshold));
gallonm 14:946e62f44f4f 231 m_prev_sample = sample;
gallonm 14:946e62f44f4f 232
gallonm 14:946e62f44f4f 233 // update
gallonm 14:946e62f44f4f 234 *duration = GET_TIME_STAMP() - m_timestamp;
gallonm 14:946e62f44f4f 235 if( isContinuous )
gallonm 14:946e62f44f4f 236 {
gallonm 14:946e62f44f4f 237 return_code = ( belowCurrent ) ? HMI_SIGNAL_CODE_DOWN_STATE : HMI_SIGNAL_CODE_UP_STATE;
gallonm 14:946e62f44f4f 238 }
gallonm 14:946e62f44f4f 239 else
gallonm 14:946e62f44f4f 240 {
gallonm 14:946e62f44f4f 241 return_code = ( belowCurrent ) ? HMI_SIGNAL_CODE_DROP_DOWN : HMI_SIGNAL_CODE_RAISE_UP;
gallonm 14:946e62f44f4f 242 m_timestamp = GET_TIME_STAMP();
gallonm 14:946e62f44f4f 243 }
gallonm 14:946e62f44f4f 244
gallonm 14:946e62f44f4f 245 return return_code;
gallonm 14:946e62f44f4f 246 }
gallonm 14:946e62f44f4f 247
gallonm 14:946e62f44f4f 248
gallonm 14:946e62f44f4f 249
gallonm 14:946e62f44f4f 250 /**********************************************************************************
gallonm 14:946e62f44f4f 251 *
gallonm 14:946e62f44f4f 252 *
gallonm 14:946e62f44f4f 253 * TapDetector
gallonm 14:946e62f44f4f 254 *
gallonm 14:946e62f44f4f 255 *
gallonm 14:946e62f44f4f 256 **********************************************************************************/
gallonm 14:946e62f44f4f 257 void TapDetector::Init( int threshold, int min_down_duration_ms, int min_hold_duration_ms, bool enable_double_tap, int double_tap_min_duration_ms)
gallonm 14:946e62f44f4f 258 {
gallonm 14:946e62f44f4f 259 m_threshold = threshold;
gallonm 14:946e62f44f4f 260 m_motion.Init( threshold );
gallonm 14:946e62f44f4f 261 m_min_down_duration_ms = min_down_duration_ms;
gallonm 14:946e62f44f4f 262 m_min_hold_duration_ms = min_hold_duration_ms;
gallonm 14:946e62f44f4f 263 m_min_dtap_duration_ms = double_tap_min_duration_ms;
gallonm 14:946e62f44f4f 264 m_notify_hold = true;
gallonm 14:946e62f44f4f 265 m_notify_dtap = enable_double_tap;
gallonm 14:946e62f44f4f 266 m_timestamp = GET_TIME_STAMP();
gallonm 14:946e62f44f4f 267 }
gallonm 14:946e62f44f4f 268
gallonm 14:946e62f44f4f 269 int TapDetector::Update( int sample, int *duration )
gallonm 14:946e62f44f4f 270 {
gallonm 14:946e62f44f4f 271 int return_code;
gallonm 14:946e62f44f4f 272 int motion_code,motion_duration;
gallonm 14:946e62f44f4f 273
gallonm 14:946e62f44f4f 274 return_code = HMI_TAP_CODE_NULL;
gallonm 14:946e62f44f4f 275 *duration = 0;
gallonm 14:946e62f44f4f 276 motion_code = m_motion.Update(sample, &motion_duration);
gallonm 14:946e62f44f4f 277
gallonm 14:946e62f44f4f 278 // hold condition - down_duration > hold_time
gallonm 14:946e62f44f4f 279 if( motion_code == HMI_SIGNAL_CODE_DOWN_STATE && motion_duration > m_min_hold_duration_ms )
gallonm 14:946e62f44f4f 280 {
gallonm 14:946e62f44f4f 281 if( m_notify_hold )
gallonm 14:946e62f44f4f 282 {
gallonm 14:946e62f44f4f 283 return_code = HMI_TAP_CODE_HOLD;
gallonm 14:946e62f44f4f 284 m_notify_hold = false; // to avoid continue notification ( anti flood )
gallonm 14:946e62f44f4f 285 *duration = motion_duration;
gallonm 14:946e62f44f4f 286 }
gallonm 14:946e62f44f4f 287 }
gallonm 14:946e62f44f4f 288 else if( motion_code == HMI_SIGNAL_CODE_RAISE_UP )
gallonm 14:946e62f44f4f 289 {
gallonm 14:946e62f44f4f 290 m_notify_hold = true;
gallonm 14:946e62f44f4f 291 /* TODO add TAP single/double code */
gallonm 14:946e62f44f4f 292 }
gallonm 14:946e62f44f4f 293
gallonm 14:946e62f44f4f 294 return return_code;
gallonm 14:946e62f44f4f 295 }
gallonm 14:946e62f44f4f 296