Yuta Kozaki / Mbed 2 deprecated winkey_1_3_11U24_smd

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Servo LSM9DS1_Library_

main.cpp@0:f66784158dab, 2020-02-26 (annotated)

Committer:
kzkyuta
Date:
Wed Feb 26 02:49:19 2020 +0000
Revision:
0:f66784158dab
Child:
1:d22b2e802de9
This version is for the sensor which detects voluntary eyelid closure.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kzkyuta 0:f66784158dab 1 #define DEVICE_SERIAL_FC 1
kzkyuta 0:f66784158dab 2 #include "mbed.h"
kzkyuta 0:f66784158dab 3 #include "Servo.h"
kzkyuta 0:f66784158dab 4 #include "LSM9DS1.h"
kzkyuta 0:f66784158dab 5
kzkyuta 0:f66784158dab 6 //Serial pc(USBTX, USBRX); // to receive the name of participant
kzkyuta 0:f66784158dab 7 Serial pc(P0_19, P0_18); // to receive the name of participant
kzkyuta 0:f66784158dab 8 LSM9DS1 imu(P0_5, P0_4, 0xD6, 0x3C);
kzkyuta 0:f66784158dab 9
kzkyuta 0:f66784158dab 10 AnalogIn sensor_0(P0_11);
kzkyuta 0:f66784158dab 11 AnalogIn sensor_1(P0_12);
kzkyuta 0:f66784158dab 12 AnalogIn sensor_2(P0_13);
kzkyuta 0:f66784158dab 13
kzkyuta 0:f66784158dab 14 #define ARRAY_SIZE 10
kzkyuta 0:f66784158dab 15 int array_0[ARRAY_SIZE];
kzkyuta 0:f66784158dab 16 int array_1[ARRAY_SIZE];
kzkyuta 0:f66784158dab 17 int array_2[ARRAY_SIZE];
kzkyuta 0:f66784158dab 18
kzkyuta 0:f66784158dab 19 int diff_0;
kzkyuta 0:f66784158dab 20 int diff_1;
kzkyuta 0:f66784158dab 21 int diff_2;
kzkyuta 0:f66784158dab 22
kzkyuta 0:f66784158dab 23 int val_0;
kzkyuta 0:f66784158dab 24 int val_1;
kzkyuta 0:f66784158dab 25 int val_2;
kzkyuta 0:f66784158dab 26
kzkyuta 0:f66784158dab 27 bool TH_Vol;
kzkyuta 0:f66784158dab 28
kzkyuta 0:f66784158dab 29 int val;
kzkyuta 0:f66784158dab 30
kzkyuta 0:f66784158dab 31 unsigned short counter_read;
kzkyuta 0:f66784158dab 32 unsigned short prev_counter_read;
kzkyuta 0:f66784158dab 33 unsigned short counter_wait;
kzkyuta 0:f66784158dab 34 unsigned short position;
kzkyuta 0:f66784158dab 35
kzkyuta 0:f66784158dab 36 #define THRESHOLD 800
kzkyuta 0:f66784158dab 37 #define THRESHOLD_VOL -100
kzkyuta 0:f66784158dab 38
kzkyuta 0:f66784158dab 39 DigitalOut rled(P0_15);
kzkyuta 0:f66784158dab 40 DigitalOut sp(P0_15);
kzkyuta 0:f66784158dab 41 DigitalOut gled(P0_23);
kzkyuta 0:f66784158dab 42 DigitalOut bled(P0_16);
kzkyuta 0:f66784158dab 43 DigitalIn sw(P0_1);
kzkyuta 0:f66784158dab 44 DigitalIn bt(P0_14);
kzkyuta 0:f66784158dab 45
kzkyuta 0:f66784158dab 46 Servo myservo(P0_9);
kzkyuta 0:f66784158dab 47 bool servoState_c = true;
kzkyuta 0:f66784158dab 48 bool servoState_p = true;
kzkyuta 0:f66784158dab 49 bool isAutoActuation = false;
kzkyuta 0:f66784158dab 50 bool isServoClose = false;
kzkyuta 0:f66784158dab 51
kzkyuta 0:f66784158dab 52 Ticker timer_1;
kzkyuta 0:f66784158dab 53
kzkyuta 0:f66784158dab 54 uint8_t mode; // 0:auto, 1:manual, 2:BT base ctrl
kzkyuta 0:f66784158dab 55 uint8_t bt_state;
kzkyuta 0:f66784158dab 56 bool sw_state = true;
kzkyuta 0:f66784158dab 57
kzkyuta 0:f66784158dab 58 void counter(){
kzkyuta 0:f66784158dab 59 counter_read ++;
kzkyuta 0:f66784158dab 60 counter_wait ++;
kzkyuta 0:f66784158dab 61 if(counter_read >= 60000) counter_read = 0;
kzkyuta 0:f66784158dab 62 if(counter_wait >= 60000) counter_wait = 1000;
kzkyuta 0:f66784158dab 63 }
kzkyuta 0:f66784158dab 64
kzkyuta 0:f66784158dab 65 void readVal(){
kzkyuta 0:f66784158dab 66 // imu.readAccel();
kzkyuta 0:f66784158dab 67 // imu.readMag();
kzkyuta 0:f66784158dab 68 // imu.readGyro();
kzkyuta 0:f66784158dab 69 // read the sensor value and push to the window array for differenciation.
kzkyuta 0:f66784158dab 70 val_0 = sensor_0.read_u16();
kzkyuta 0:f66784158dab 71 val_1 = sensor_1.read_u16();
kzkyuta 0:f66784158dab 72 val_2 = sensor_2.read_u16();
kzkyuta 0:f66784158dab 73
kzkyuta 0:f66784158dab 74 if(val_1 < 100 && val_2 < 100) TH_Vol = true;
kzkyuta 0:f66784158dab 75 else TH_Vol = false;
kzkyuta 0:f66784158dab 76
kzkyuta 0:f66784158dab 77 if(val_0 > 36000 && val_0 < 27000) val_0 = array_0[ARRAY_SIZE - 1];
kzkyuta 0:f66784158dab 78 if(val_1 > 36000 && val_1 < 27000) val_1 = array_1[ARRAY_SIZE - 1];
kzkyuta 0:f66784158dab 79 if(val_2 > 36000 && val_2 < 27000) val_2 = array_2[ARRAY_SIZE - 1];
kzkyuta 0:f66784158dab 80
kzkyuta 0:f66784158dab 81 position ++;
kzkyuta 0:f66784158dab 82 int tempPos = position % ARRAY_SIZE;
kzkyuta 0:f66784158dab 83
kzkyuta 0:f66784158dab 84 array_0[tempPos] = (val_0 + array_0[ARRAY_SIZE - 1] + array_0[ARRAY_SIZE - 2] + array_0[ARRAY_SIZE - 3])/3;
kzkyuta 0:f66784158dab 85 array_1[tempPos] = (val_1 + array_1[ARRAY_SIZE - 1] + array_1[ARRAY_SIZE - 2] + array_1[ARRAY_SIZE - 3])/3;
kzkyuta 0:f66784158dab 86 array_2[tempPos] = (val_2 + array_2[ARRAY_SIZE - 1] + array_2[ARRAY_SIZE - 2] + array_2[ARRAY_SIZE - 3])/3;
kzkyuta 0:f66784158dab 87
kzkyuta 0:f66784158dab 88 // calcurate the differenciation.
kzkyuta 0:f66784158dab 89 if(tempPos == ARRAY_SIZE - 1){
kzkyuta 0:f66784158dab 90 diff_0 = array_0[tempPos] - array_0[0];
kzkyuta 0:f66784158dab 91 diff_1 = array_1[tempPos] - array_1[0];
kzkyuta 0:f66784158dab 92 diff_2 = array_2[tempPos] - array_2[0];
kzkyuta 0:f66784158dab 93 }else{
kzkyuta 0:f66784158dab 94 diff_0 = array_0[tempPos] - array_0[tempPos + 1];
kzkyuta 0:f66784158dab 95 diff_1 = array_1[tempPos] - array_1[tempPos + 1];
kzkyuta 0:f66784158dab 96 diff_2 = array_2[tempPos] - array_2[tempPos + 1];
kzkyuta 0:f66784158dab 97 }
kzkyuta 0:f66784158dab 98 }
kzkyuta 0:f66784158dab 99
kzkyuta 0:f66784158dab 100 void setup(){
kzkyuta 0:f66784158dab 101 timer_1.attach_us(&counter, 1000);
kzkyuta 0:f66784158dab 102 myservo.calibrate(0.0005, 45.0);
kzkyuta 0:f66784158dab 103 myservo = 0.55;
kzkyuta 0:f66784158dab 104 imu.begin();
kzkyuta 0:f66784158dab 105 sw.mode(PullUp);
kzkyuta 0:f66784158dab 106 // pc.set_flow_control(Serial::RTSCTS, p7, p8);
kzkyuta 0:f66784158dab 107 }
kzkyuta 0:f66784158dab 108
kzkyuta 0:f66784158dab 109 void ledShow(uint8_t _input){
kzkyuta 0:f66784158dab 110 switch(_input){
kzkyuta 0:f66784158dab 111 case 0:
kzkyuta 0:f66784158dab 112 rled = 0;
kzkyuta 0:f66784158dab 113 gled = 0;
kzkyuta 0:f66784158dab 114 bled = 0;
kzkyuta 0:f66784158dab 115 break;
kzkyuta 0:f66784158dab 116 case 1:
kzkyuta 0:f66784158dab 117 rled = 1;
kzkyuta 0:f66784158dab 118 gled = 0;
kzkyuta 0:f66784158dab 119 bled = 0;
kzkyuta 0:f66784158dab 120 break;
kzkyuta 0:f66784158dab 121 case 2:
kzkyuta 0:f66784158dab 122 rled = 0;
kzkyuta 0:f66784158dab 123 gled = 1;
kzkyuta 0:f66784158dab 124 bled = 0;
kzkyuta 0:f66784158dab 125 break;
kzkyuta 0:f66784158dab 126 case 3:
kzkyuta 0:f66784158dab 127 rled = 0;
kzkyuta 0:f66784158dab 128 gled = 0;
kzkyuta 0:f66784158dab 129 bled = 1;
kzkyuta 0:f66784158dab 130 break;
kzkyuta 0:f66784158dab 131 default:
kzkyuta 0:f66784158dab 132 rled = 0;
kzkyuta 0:f66784158dab 133 gled = 0;
kzkyuta 0:f66784158dab 134 bled = 0;
kzkyuta 0:f66784158dab 135 break;
kzkyuta 0:f66784158dab 136 }
kzkyuta 0:f66784158dab 137 }
kzkyuta 0:f66784158dab 138
kzkyuta 0:f66784158dab 139 void ledCtrl(){
kzkyuta 0:f66784158dab 140 if(bt_state){
kzkyuta 0:f66784158dab 141 if(counter_read % 500 >= 250){
kzkyuta 0:f66784158dab 142 ledShow(mode + 1);
kzkyuta 0:f66784158dab 143 }else{
kzkyuta 0:f66784158dab 144 ledShow(0);
kzkyuta 0:f66784158dab 145 }
kzkyuta 0:f66784158dab 146 }else{
kzkyuta 0:f66784158dab 147 ledShow(mode + 1);
kzkyuta 0:f66784158dab 148 }
kzkyuta 0:f66784158dab 149 }
kzkyuta 0:f66784158dab 150
kzkyuta 0:f66784158dab 151 void changeMode(){
kzkyuta 0:f66784158dab 152 mode++;
kzkyuta 0:f66784158dab 153 if(mode >= 3) mode = 0;
kzkyuta 0:f66784158dab 154 }
kzkyuta 0:f66784158dab 155
kzkyuta 0:f66784158dab 156 void servoOpen(){
kzkyuta 0:f66784158dab 157 myservo = 0.55;
kzkyuta 0:f66784158dab 158 }
kzkyuta 0:f66784158dab 159
kzkyuta 0:f66784158dab 160 void servoClose(){
kzkyuta 0:f66784158dab 161 myservo = 0.0;
kzkyuta 0:f66784158dab 162 }
kzkyuta 0:f66784158dab 163
kzkyuta 0:f66784158dab 164 void autoActuation(){
kzkyuta 0:f66784158dab 165 if(isAutoActuation){
kzkyuta 0:f66784158dab 166 if(servoState_c && servoState_p){
kzkyuta 0:f66784158dab 167 servoState_c = false;
kzkyuta 0:f66784158dab 168 myservo = 0.0;
kzkyuta 0:f66784158dab 169 counter_wait = 0;
kzkyuta 0:f66784158dab 170 }else if(!servoState_c && servoState_p){
kzkyuta 0:f66784158dab 171 if(counter_wait >= 250) servoState_p = false;
kzkyuta 0:f66784158dab 172 }else if(!servoState_c && !servoState_p){
kzkyuta 0:f66784158dab 173 servoState_c = true;
kzkyuta 0:f66784158dab 174 myservo = 0.55;
kzkyuta 0:f66784158dab 175 counter_wait = 0;
kzkyuta 0:f66784158dab 176 }else if(servoState_c && !servoState_p){
kzkyuta 0:f66784158dab 177 if(counter_wait >= 500) servoState_p = true;
kzkyuta 0:f66784158dab 178 isAutoActuation = false;
kzkyuta 0:f66784158dab 179 }
kzkyuta 0:f66784158dab 180 }
kzkyuta 0:f66784158dab 181 }
kzkyuta 0:f66784158dab 182
kzkyuta 0:f66784158dab 183 void btnCtrl(){
kzkyuta 0:f66784158dab 184 if(!sw){
kzkyuta 0:f66784158dab 185 pc.printf("Pushed");
kzkyuta 0:f66784158dab 186 if(sw_state){
kzkyuta 0:f66784158dab 187 sw_state = false;
kzkyuta 0:f66784158dab 188 counter_wait = 0;
kzkyuta 0:f66784158dab 189 }
kzkyuta 0:f66784158dab 190 }
kzkyuta 0:f66784158dab 191 if(sw && !sw_state){
kzkyuta 0:f66784158dab 192 if((counter_wait > 50) && (counter_wait < 800)){
kzkyuta 0:f66784158dab 193 if(mode == 1){
kzkyuta 0:f66784158dab 194 if(isServoClose){
kzkyuta 0:f66784158dab 195 servoOpen();
kzkyuta 0:f66784158dab 196 }else{
kzkyuta 0:f66784158dab 197 servoClose();
kzkyuta 0:f66784158dab 198 }
kzkyuta 0:f66784158dab 199 isServoClose = !isServoClose;
kzkyuta 0:f66784158dab 200 }
kzkyuta 0:f66784158dab 201 }else if(counter_wait >= 800){
kzkyuta 0:f66784158dab 202 changeMode();
kzkyuta 0:f66784158dab 203 servoOpen();
kzkyuta 0:f66784158dab 204 }
kzkyuta 0:f66784158dab 205 sw_state = true;
kzkyuta 0:f66784158dab 206 pc.printf("mode is %d\n", mode);
kzkyuta 0:f66784158dab 207 }
kzkyuta 0:f66784158dab 208 }
kzkyuta 0:f66784158dab 209
kzkyuta 0:f66784158dab 210 void detection(){
kzkyuta 0:f66784158dab 211 if(TH_Vol){
kzkyuta 0:f66784158dab 212 if(diff_0 < THRESHOLD_VOL){
kzkyuta 0:f66784158dab 213 if((diff_0 > -10000) || (diff_1 > -10000) || (diff_2 < -10000)){
kzkyuta 0:f66784158dab 214 pc.printf("detected!!");
kzkyuta 0:f66784158dab 215 isAutoActuation = true;
kzkyuta 0:f66784158dab 216 }
kzkyuta 0:f66784158dab 217 }
kzkyuta 0:f66784158dab 218 }
kzkyuta 0:f66784158dab 219 //else{
kzkyuta 0:f66784158dab 220 // if((diff_0 > THRESHOLD) || (diff_1 > THRESHOLD) || (diff_2 > THRESHOLD)){
kzkyuta 0:f66784158dab 221 // if((diff_0 < 3000) || (diff_1 < 3000) || (diff_2 < 3000)){
kzkyuta 0:f66784158dab 222 // pc.printf("detected");
kzkyuta 0:f66784158dab 223 // isAutoActuation = true;
kzkyuta 0:f66784158dab 224 // }
kzkyuta 0:f66784158dab 225 // }
kzkyuta 0:f66784158dab 226 // }
kzkyuta 0:f66784158dab 227 }
kzkyuta 0:f66784158dab 228
kzkyuta 0:f66784158dab 229 int main() {
kzkyuta 0:f66784158dab 230 setup();
kzkyuta 0:f66784158dab 231 while(1) {
kzkyuta 0:f66784158dab 232 if(counter_read % 3 == 0){
kzkyuta 0:f66784158dab 233 readVal();
kzkyuta 0:f66784158dab 234 detection();
kzkyuta 0:f66784158dab 235 }
kzkyuta 0:f66784158dab 236 if(mode == 0){
kzkyuta 0:f66784158dab 237 autoActuation();
kzkyuta 0:f66784158dab 238 }
kzkyuta 0:f66784158dab 239 btnCtrl();
kzkyuta 0:f66784158dab 240 bt_state = bt;
kzkyuta 0:f66784158dab 241 ledCtrl();
kzkyuta 0:f66784158dab 242 }
kzkyuta 0:f66784158dab 243 }