![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Use IQS62X sensor and move motor by detected angle
Dependencies: DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed
Fork of Nucleo_ACM1602_I2C_DC by
Diff: MotorMove.cpp
- Revision:
- 9:b58e7d72a91c
- Parent:
- 8:f7ad1d7176ba
- Child:
- 10:ef379cbc0004
--- a/MotorMove.cpp Sat Sep 30 16:43:51 2017 +0000 +++ b/MotorMove.cpp Mon Oct 02 19:12:02 2017 +0000 @@ -5,32 +5,36 @@ //float vol_decel_ini[DECEL_SIZE] = {1,0.75,0.4,0.15,0.05,0}; float vol_decel_ini[DECEL_SIZE] = {-0.5,0,0,0,0,0}; -float vol_accel_ini[ACCEL_SIZE] = {0,0.5,0.8,1,1,1}; +//float vol_accel_ini[ACCEL_SIZE] = {0,0.2,0.8,1,1,1}; +float vol_accel_ini[ACCEL_SIZE] = {0.2,0.4,0.6,0.8,1.0,1.0}; -void MotorMove::up_motor_set(int time_counter, int speed) +void MotorMove::up_motor_set(int time_counter, float speed) { - bflag_up =1; + bflag_up =1; bflag_down =0; start_time_count=time_counter; + accel_count = 0; + decel_count = 0; for (int i=0;i<ACCEL_SIZE;i++) { - vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO; + vol_accel[i] = -vol_accel_ini[i]*speed/SPEED_RATIO; } - + for (int i=0;i<DECEL_SIZE;i++) { - vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO; + vol_decel[i] = -vol_decel_ini[i]*speed/SPEED_RATIO; } }; - -void MotorMove::down_motor_set(int time_counter, int speed) +void MotorMove::down_motor_set(int time_counter, float speed) { - bflag_up =0; - bflag_down=1; + bflag_up =0; + bflag_down =1; start_time_count=time_counter; - + accel_count = 0; + decel_count = 0; + for (int i=0;i<ACCEL_SIZE;i++) { vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO; @@ -43,87 +47,97 @@ }; -float MotorMove::ReturnMotorVol(int time_counter, int sw_flag1,int sw_flag2) +float MotorMove::ReturnMotorVol(int time_counter,int bflag_downed, int bflag_upped) { float speed; - int time = time_counter - start_time_count; - - - static int sw_flag1_pre=0; - static int sw_flag2_pre=0; - static int stop_time=0; + int time; - static int up_decel_count=0; - static int down_decel_count=0; + static int flag_up_stopping=0; + static int flag_down_stopping=0; - //decel timer check - if(bflag_up==1) - up_decel_count==0; - if(bflag_down==1) - down_decel_count==0; - - + //Check flag for normal mode or brake mode + //accel_count MODE + - if(bflag_up) - { - if (sw_flag2==1 && sw_flag2_pre==0) //State changed - { - stop_time=time_counter; - } - - if(sw_flag2==0) // move motor - { - if(time>ACCEL_SIZE) - { - time = ACCEL_SIZE; - } - speed = vol_accel[time]; - } - - else if (sw_flag2==1) - { - time = time_counter-stop_time; - - if(time>DECEL_SIZE) - { - time = DECEL_SIZE; - bflag_up =0; - } - speed = vol_decel[time]; + if (bflag_up==1 && bflag_upped ==1) + { + flag_up_stopping=1; + //bflag_up==0; + } + else if (bflag_down==1 && bflag_downed ==1) + { + flag_down_stopping=1; + //bflag_down=0; + } + + if (bflag_up==1) + { + if (flag_up_stopping==0) + { + if (accel_count<(ACCEL_SIZE-1)) + { + speed = vol_accel[accel_count]; + accel_count++; + } + else + { + speed = vol_accel[ACCEL_SIZE-1]; + } + } + else if(flag_up_stopping==1) + { + if (decel_count<=(DECEL_SIZE-1)) + { + speed = vol_decel[decel_count]; + decel_count++; + } + else + { + bflag_up=0; + speed=0; + flag_up_stopping=0; + } } } - - else if (bflag_down) + else if (bflag_down==1) + { + if (flag_down_stopping==0) + { + if (accel_count<(ACCEL_SIZE-1)) + { + speed = vol_accel[accel_count]; + accel_count++; + } + else + { + speed = vol_accel[ACCEL_SIZE-1]; + } + } + else if(flag_down_stopping==1) + { + if (decel_count<=(DECEL_SIZE-1)) + { + speed = vol_decel[decel_count]; + decel_count++; + } + else + { + bflag_down=0; + speed=0; + flag_down_stopping=0; + } + } + } + else { - if (sw_flag1==1 && sw_flag1_pre==0) //State changed - { - stop_time=time_counter; - } + speed=0; + } + - if(sw_flag1==0) // move motor - { - if(time>ACCEL_SIZE) - { - time = ACCEL_SIZE; - } - speed = -vol_accel[time]; - } - - else if (sw_flag1==1) - { - time = time_counter-stop_time; - if(time>DECEL_SIZE) - { - time = DECEL_SIZE; - bflag_down =0; - } - speed = -vol_decel[time]; - } - } + //move motor before push stopping switch + return speed; + - sw_flag1_pre=sw_flag1; - sw_flag2_pre=sw_flag2; - return speed; };