Use IQS62X sensor and move motor by detected angle
Dependencies: DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed
Fork of Nucleo_ACM1602_I2C_DC by
Diff: main.cpp
- Revision:
- 8:f7ad1d7176ba
- Parent:
- 7:acb1074eaba6
- Child:
- 9:b58e7d72a91c
diff -r acb1074eaba6 -r f7ad1d7176ba main.cpp --- a/main.cpp Fri Sep 22 16:27:18 2017 +0000 +++ b/main.cpp Sat Sep 30 16:43:51 2017 +0000 @@ -8,10 +8,7 @@ #include "DRV8830.h" #include "IQS62x.h" #include "IQSdisplayTerminal.h" - - -//IQS62xDisplay terminal; // class to display IQS62x registers on a terminal -IQS62xIO iqs62x; // class for basic IQS62x block read and write +#include "MotorMove.h" //Cycle #define UP_DURATION 14 //On time [*100ms] @@ -19,27 +16,17 @@ #define DOWN_DURATION 8 //Down time [*50ms] #define SWITCH_PERIOD 50 //Cycle time[*50ms] #define TOTAL_TIMES 30000 //total times n -#define ACCEL_SIZE 6 -#define DECEL_SIZE 6 +#define TIMER_COUNT 0.01 +#define LOOP_WAITMS 30 +#define UP_THRESHOLD 10 +#define DOWN_THRESHOLD 90 -const float spd_table[] = {1.0,0.9,0.8}; -//const float vol_accel[ACCEL_SIZE] = {0,0.3,0.6,0.8,0.95,1}; -const float vol_decel[DECEL_SIZE] = {1,0.75,0.4,0.15,0.05,0}; -const float vol_accel[ACCEL_SIZE] = {0,0.5,0.8,1,1,1}; -//const float vol_decel[DECEL_SIZE] = {1,0.75,0.15,-1,-0.2,0}; - -const float donw_ratio=0.85; - -static int sp_index=0; - +Ticker timer_; using namespace Mikami; -void ShowLCD(char * buffer, int startbyte, int endbyte); // for wheel output - Acm1602Ni lcd_; // Default, OK - //Acm1602Ni lcd_(D14, D15); // OK //Acm1602Ni lcd_(D14, D15, 200000); // OK //Acm1602Ni lcd_(D14, D15, 200000, true, true); // OK @@ -47,27 +34,233 @@ //Acm1602Ni lcd_(PC_9, PA_8); // OK //Acm1602Ni lcd_(PB_4, PA_8); // OK -Ticker timer_; I2C i2c(D14, D15); -DRV8830 motor(i2c, DRV8830ADDR_NN); +DRV8830 motorL(i2c, DRV8830ADDR_NN); //Motor1 +DRV8830 motorR(i2c, DRV8830ADDR_0N); //Motor2 +MotorMove mvalL; +MotorMove mvalR; + +IQS62xIO iqs62x; // class for basic IQS62x block read and write InterruptIn button1(USER_BUTTON); -static float motor_speed; + + +DigitalIn in_switchs[]= + { DigitalIn(A0), DigitalIn(A1), DigitalIn(A2), DigitalIn(A3)}; + + +static int shaft_deg=0; +static int shaft_speed=0; +static int g_timer=0; //gloabl timer +static int cnt; //total timer of loop +static int sw_in[4]={0,0,0,0}; //swithc flag bit +static int sp_index=0; //Movement mode 0-3 + +static int initial_deg=0; + + +void ShowLCD(char * buffer, int startbyte, int endbyte); // for wheel output +int ReadDegree(char * buffer); +int ReadSpeed(char * buffer); +void Displaylevel (int deg); +void TimerIsr(); +void flip(); +int MainIOloop(); +void DisplayStatus(); +void MoveMotor(); + +int main() +{ + //initialize system + static int time_prev; + for (int i=0;i<4;i=i++){ + in_switchs[i].mode(PullUp); + } + + //motor.speed(0); + //Initialize Ic2 Device + motorL.speed(0); + motorR.speed(0); + + lcd_.WriteStringXY("IQS_Init_Start",0,0); + iqs62x.configure(); // configure + wait(1); + iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers + initial_deg = ReadDegree(iqs62x.registers); + + + lcd_.WriteStringXY("IQS_Init_done",0,0); + wait(1); + + //read 0deg for initialize + button1.fall(&flip); + +//TimerIsr(); +//timer_.attach(&TimerIsr, TIMER_COUNT); +// bool status = motor.status(); +// if (status & DRV8830_F_FAULT){ +// motor.reset(); +// } + +//Read here as Asynchronous when data gets ready +while (true) { + int time_current = g_timer; + int time_diff = time_current - time_prev; + int a= MainIOloop(); + DisplayStatus(); + //display_info + time_prev = time_current; + //motorR.speed( (shaft_deg-180.0)/200.0 ); + wait_ms(LOOP_WAITMS); + MoveMotor(); + + cnt ++; + } + + +} + +void MoveMotor(){ + static int bflag_up_pre=0; + static int bflag_down_pre=0; + int bflag_up_cur =0; + int bflag_down_cur =0; + float lspeed; + float rspeed; + - -// Display elapsed time in minutes and seconds +//detect up or donw by thredold + if (shaft_deg>UP_THRESHOLD) + { + bflag_up_cur=1; + } + else + { + bflag_up_cur=0; + }; + + if (shaft_deg<DOWN_THRESHOLD) + { + bflag_down_cur=1; + } + else + { + bflag_down_cur=0; + }; + + +//send down or up command when status had changed + if(bflag_up_pre==0&& bflag_up_cur==1) + { + //shaft_speed + mvalL.up_motor_set(cnt, 1.0); + mvalR.up_motor_set(cnt, 1.0); + lcd_.WriteStringXY("U",0,1); + } + else if(bflag_down_pre==0 && bflag_down_cur==1) + { + mvalL.down_motor_set(cnt, 1.0); + mvalR.down_motor_set(cnt, 1.0); + lcd_.WriteStringXY("D",1,1); + } + + else{ + lcd_.WriteStringXY("__",0,1); + } + + + lspeed=mvalL.ReturnMotorVol(cnt, sw_in[0],sw_in[1]); + rspeed=mvalR.ReturnMotorVol(cnt, sw_in[2],sw_in[3]); + + motorL.speed(lspeed); + motorR.speed(rspeed); + + lcd_.WriteValueXY("%1.2f",lspeed,3,1); + lcd_.WriteValueXY("%1.2f",rspeed,8,1); + + + bflag_up_pre = bflag_up_cur; + bflag_down_pre = bflag_down_cur; + } + + +void DisplayStatus() + { + lcd_.WriteValueXY("T%3d ",shaft_deg, 0,0); + lcd_.WriteValue("V%2d",shaft_speed); + + lcd_.WriteStringXY("F",9,0); + for (int i=0;i<4;i++){ + lcd_.WriteValue("%d",sw_in[i]); + } + } + +int MainIOloop() +{ + static int cnt=0; + //iqs62x.waitForIqsReady(); + iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers + shaft_deg = ReadDegree(iqs62x.registers)-initial_deg; + if(shaft_deg<0) + { + shaft_deg = shaft_deg+360; + } + + shaft_speed= ReadSpeed(iqs62x.registers); + //lcd_.WriteValueXY("%3d ",k, 0,0); + + for (int i=0;i<4;i=i++){ + sw_in[i]=!in_switchs[i]; + //sw_in[i+1]=!in_switchs[i+1]; + } + cnt++; + return cnt; +} + + +void TimerIsr() +{ + //For LED Time-Sec display + //wait_ms(5); + g_timer++; + //Displaylevel(val); +} + + + + +void flip() { + static bool b = false; + + if(b==false) + { + timer_.attach(&TimerIsr, TIMER_COUNT); + } + + else + { + timer_.detach(); + //Relay1=0; + sp_index++; + if (sp_index == 3) + { + sp_index = 0; + } + } + b=!b; + +} int ReadDegree(char * buffer) { int ret=0; - //(High bit + Low bit) * 360/65536 //ret = ((buffer[0x80]<<8 + buffer[0x81])*0.00549316406 ; ret = (buffer[0x80]<<8 +buffer[0x81])/65536.0*360.0 ; return ret; } - + int ReadSpeed(char * buffer) { int ret=0; @@ -90,197 +283,8 @@ { lcd_.WriteString(" "); } - } - - - -} - - -void TimerIsr() -{ - - //For LED Time-Sec display - static int k = 0; - static char ctext[4]="---"; - div_t d_Cycle = div (k, SWITCH_PERIOD); - - //for Current time - div_t d_sec = div(k,600*2); //60s * 10n - int t_min = d_sec.quot; - div_t d_min = div(t_min,60); //1min=60s - int t_hr = d_min.quot; - - //for Current time - div_t df_sec = div(TOTAL_TIMES*SWITCH_PERIOD,600); //60s * 10n - int tf_min = df_sec.quot; - div_t df_min = div(tf_min,60); //1min=60s - int tf_hr = df_min.quot; - - - //Motor activation - - //Up Movement - if(WAIT_DELAY <= d_Cycle.rem && d_Cycle.rem < (WAIT_DELAY+UP_DURATION) ) - { - int accel_index = d_Cycle.rem - WAIT_DELAY; - if (accel_index < ACCEL_SIZE) - { - motor_speed=vol_accel[accel_index] * spd_table[sp_index]; - } - else - { - motor_speed=spd_table[sp_index]; - } - strcpy(ctext," CW"); - //ctext="CW"; - - } - - //UP..stop - else if ( (WAIT_DELAY+UP_DURATION)<= d_Cycle.rem && d_Cycle.rem <(2*WAIT_DELAY+UP_DURATION) ) - { - //wait_ms(20); - int accel_index = d_Cycle.rem - (WAIT_DELAY+UP_DURATION); - if (accel_index< DECEL_SIZE) - { - motor_speed=vol_decel[accel_index] * spd_table[sp_index]; - } - else - { - motor_speed=0; - } - strcpy(ctext,"OFF"); - } - - - //down..Start - else if ( (2*WAIT_DELAY+UP_DURATION) <= d_Cycle.rem && d_Cycle.rem <(2*WAIT_DELAY+UP_DURATION+DOWN_DURATION) ) - { - int accel_index = d_Cycle.rem - (2*WAIT_DELAY+UP_DURATION); - if (accel_index < ACCEL_SIZE) - { - motor_speed= - vol_accel[accel_index] * spd_table[sp_index]*donw_ratio; - } - else - { - motor_speed= -spd_table[sp_index]*donw_ratio; - } - strcpy(ctext," CW"); - //ctext="CW"; - } - - - //down..stop - else if ( (2*WAIT_DELAY+UP_DURATION+DOWN_DURATION) <= d_Cycle.rem ) - { - //wait_ms(20); - int accel_index = d_Cycle.rem - ((2*WAIT_DELAY+UP_DURATION+DOWN_DURATION)); - if (accel_index< DECEL_SIZE) - { - motor_speed= -vol_decel[accel_index] * spd_table[sp_index]*donw_ratio; - } - else - { - motor_speed=0; - } - strcpy(ctext,"OFF"); - } - - - if(d_Cycle.quot==TOTAL_TIMES) - { - timer_.detach(); - } - - - -/* - char str[20]; - sprintf(str, "%d'%2d\"", msec.quot, msec.rem); - lcd_.WriteStringXY(str, 0, 1); -*/ - - //1 Row - //lcd_.WriteStringXY("#",0,0); - - - - lcd_.WriteValueXY("%5d/", d_Cycle.quot,0,0); - lcd_.WriteValue("%0dM",TOTAL_TIMES); - lcd_.WriteValue("%1.2f", motor_speed); - //lcd_.WriteValue("V%0d",motor_speed); - - - //2 Row - lcd_.WriteValueXY("%03dh", t_hr, 0, 1); - lcd_.WriteValue("%02dm", d_min.rem); - lcd_.WriteValue("%03ds/", d_sec.rem); - lcd_.WriteValue("%03dh", tf_hr); - lcd_.WriteValue("%02dm", df_min.rem); - - k++; + } } -void flip() { - static bool b = false; - - if(b==false) - { - timer_.attach(&TimerIsr, 0.05); - } - - else - { - timer_.detach(); - //Relay1=0; - sp_index++; - if (sp_index == 3) - { - sp_index = 0; - } - } - b=!b; - -} - - -int main() -{ - - //motor.speed(0); - // Check error and reset - - //LCD_cont=0; - //if (lcd_.IsConnected()) printf("\r\nConnected"); - //else printf("\r\nDisconnected"); - - //TimerIsr(); - //timer_.attach(&TimerIsr, 0.1); - //button1.fall(&flip); - //lcd_.WriteStringXY("0",0,0); - lcd_.WriteStringXY("Boot1",0,0); - iqs62x.configure(); // configure the ICD - lcd_.WriteStringXY("Boot2",0,0); - -// bool status = motor.status(); -// if (status & DRV8830_F_FAULT){ -// motor.reset(); -// } - - while (true) { - iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers - int val = ReadDegree(iqs62x.registers); - int speed= ReadSpeed(iqs62x.registers); - - lcd_.WriteValueXY("T:%3d ",val, 0,1); - lcd_.WriteValue("V:%3d",speed); - Displaylevel(val); - wait_ms(5); - - } - -} -