Diff: main.cpp

Revision:

0:f66784158dab

Child:

1:d22b2e802de9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 26 02:49:19 2020 +0000
@@ -0,0 +1,243 @@
+#define DEVICE_SERIAL_FC 1
+#include "mbed.h"
+#include "Servo.h"
+#include "LSM9DS1.h"
+
+//Serial pc(USBTX, USBRX);  // to receive the name of participant
+Serial pc(P0_19, P0_18);  // to receive the name of participant
+LSM9DS1 imu(P0_5, P0_4, 0xD6, 0x3C);
+
+AnalogIn sensor_0(P0_11);
+AnalogIn sensor_1(P0_12);
+AnalogIn sensor_2(P0_13);
+
+#define ARRAY_SIZE 10
+int array_0[ARRAY_SIZE];
+int array_1[ARRAY_SIZE];
+int array_2[ARRAY_SIZE];
+
+int diff_0;
+int diff_1;
+int diff_2;
+
+int val_0;
+int val_1;
+int val_2;
+
+bool TH_Vol;
+
+int val;
+
+unsigned short counter_read;
+unsigned short prev_counter_read;
+unsigned short counter_wait;
+unsigned short position;
+
+#define THRESHOLD  800
+#define THRESHOLD_VOL  -100
+
+DigitalOut rled(P0_15);
+DigitalOut sp(P0_15);
+DigitalOut gled(P0_23);
+DigitalOut bled(P0_16);
+DigitalIn sw(P0_1);
+DigitalIn bt(P0_14);
+
+Servo myservo(P0_9);
+bool servoState_c = true;
+bool servoState_p = true;
+bool isAutoActuation = false;
+bool isServoClose = false;
+
+Ticker timer_1;
+
+uint8_t mode;  // 0:auto, 1:manual, 2:BT base ctrl
+uint8_t bt_state;
+bool sw_state = true;
+
+void counter(){
+    counter_read ++;
+    counter_wait ++;
+    if(counter_read >= 60000) counter_read = 0;
+    if(counter_wait >= 60000) counter_wait = 1000;
+}
+    
+void readVal(){
+//    imu.readAccel();
+//    imu.readMag();
+//    imu.readGyro();
+    // read the sensor value and push to the window array for differenciation.
+    val_0 = sensor_0.read_u16();
+    val_1 = sensor_1.read_u16();
+    val_2 = sensor_2.read_u16();
+    
+    if(val_1 < 100 && val_2 < 100) TH_Vol = true;
+    else TH_Vol = false;
+
+    if(val_0 > 36000 && val_0 < 27000)  val_0 = array_0[ARRAY_SIZE - 1];
+    if(val_1 > 36000 && val_1 < 27000)  val_1 = array_1[ARRAY_SIZE - 1];
+    if(val_2 > 36000 && val_2 < 27000)  val_2 = array_2[ARRAY_SIZE - 1];
+    
+    position ++;
+    int tempPos = position % ARRAY_SIZE;
+    
+    array_0[tempPos] = (val_0 + array_0[ARRAY_SIZE - 1] + array_0[ARRAY_SIZE - 2] + array_0[ARRAY_SIZE - 3])/3;
+    array_1[tempPos] = (val_1 + array_1[ARRAY_SIZE - 1] + array_1[ARRAY_SIZE - 2] + array_1[ARRAY_SIZE - 3])/3;
+    array_2[tempPos] = (val_2 + array_2[ARRAY_SIZE - 1] + array_2[ARRAY_SIZE - 2] + array_2[ARRAY_SIZE - 3])/3;
+    
+    // calcurate the differenciation.
+    if(tempPos == ARRAY_SIZE - 1){
+        diff_0 = array_0[tempPos] - array_0[0];
+        diff_1 = array_1[tempPos] - array_1[0];
+        diff_2 = array_2[tempPos] - array_2[0];
+    }else{
+        diff_0 = array_0[tempPos] - array_0[tempPos + 1];
+        diff_1 = array_1[tempPos] - array_1[tempPos + 1];
+        diff_2 = array_2[tempPos] - array_2[tempPos + 1];
+    }
+}
+
+void setup(){
+    timer_1.attach_us(&counter, 1000);
+    myservo.calibrate(0.0005, 45.0);
+    myservo = 0.55;
+    imu.begin();
+    sw.mode(PullUp);
+//    pc.set_flow_control(Serial::RTSCTS, p7, p8);
+}
+
+void ledShow(uint8_t _input){
+    switch(_input){
+        case 0:
+            rled = 0;
+            gled = 0;
+            bled = 0;
+            break;
+        case 1:
+            rled = 1;
+            gled = 0;
+            bled = 0;
+            break;
+        case 2:
+            rled = 0;
+            gled = 1;
+            bled = 0;
+            break;
+        case 3:
+            rled = 0;
+            gled = 0;
+            bled = 1;
+            break;
+        default:
+            rled = 0;
+            gled = 0;
+            bled = 0;
+            break;
+    }
+}
+
+void ledCtrl(){
+    if(bt_state){
+        if(counter_read % 500 >= 250){
+            ledShow(mode + 1);
+        }else{
+            ledShow(0);
+        }
+    }else{
+        ledShow(mode + 1);
+    }
+}
+
+void changeMode(){
+    mode++;
+    if(mode >= 3) mode = 0;
+}
+
+void servoOpen(){
+    myservo = 0.55;
+}
+
+void servoClose(){
+    myservo = 0.0;
+}
+
+void autoActuation(){
+    if(isAutoActuation){
+        if(servoState_c && servoState_p){
+        servoState_c = false;
+        myservo = 0.0;
+        counter_wait = 0;
+        }else if(!servoState_c && servoState_p){
+            if(counter_wait >= 250) servoState_p = false;
+        }else if(!servoState_c && !servoState_p){
+            servoState_c = true;
+            myservo = 0.55;
+            counter_wait = 0;
+        }else if(servoState_c && !servoState_p){
+            if(counter_wait >= 500) servoState_p = true;
+            isAutoActuation = false;
+        }
+    }    
+}
+
+void btnCtrl(){
+    if(!sw){
+        pc.printf("Pushed");
+        if(sw_state){
+            sw_state = false;
+            counter_wait = 0;
+        }
+    }
+    if(sw && !sw_state){
+        if((counter_wait > 50) && (counter_wait < 800)){
+            if(mode == 1){
+                if(isServoClose){
+                     servoOpen();
+                 }else{
+                     servoClose();
+                 }
+                 isServoClose = !isServoClose;
+            }
+        }else if(counter_wait >= 800){
+            changeMode();
+            servoOpen();
+        }
+        sw_state = true;
+        pc.printf("mode is %d\n", mode);
+    }
+}
+
+void detection(){
+    if(TH_Vol){
+        if(diff_0 < THRESHOLD_VOL){
+            if((diff_0 > -10000) || (diff_1 > -10000) || (diff_2 < -10000)){
+                    pc.printf("detected!!");
+                    isAutoActuation = true;
+            }
+        }
+    }
+    //else{
+//        if((diff_0 > THRESHOLD) || (diff_1 > THRESHOLD) || (diff_2 > THRESHOLD)){
+//            if((diff_0 < 3000) || (diff_1 < 3000) || (diff_2 < 3000)){
+//                    pc.printf("detected");
+//                    isAutoActuation = true;
+//            }
+//        }
+//    }    
+}
+
+int main() {
+    setup();
+    while(1) {
+        if(counter_read % 3 == 0){
+            readVal();
+            detection();
+        }
+        if(mode == 0){
+            autoActuation();
+        }
+        btnCtrl();
+        bt_state = bt;
+        ledCtrl();
+    }
+}