File content as of revision 1:d22b2e802de9:

#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  500
#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;
    array_0[tempPos] = val_0;
    array_1[tempPos] = val_1;
    array_2[tempPos] = val_2;
    
    // 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();
    }
}