Use IQS62X sensor and move motor by detected angle
Dependencies: DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed
Fork of Nucleo_ACM1602_I2C_DC by
main.cpp
- Committer:
- 8mona
- Date:
- 2017-09-30
- Revision:
- 8:f7ad1d7176ba
- Parent:
- 7:acb1074eaba6
- Child:
- 9:b58e7d72a91c
File content as of revision 8:f7ad1d7176ba:
//------------------------------------------------------------ // Demo program for LCD ACM1602NI using I2C interface // Pullup resistors for SDA and SCL: 4.7 kΩ // 2016/04/01, Copyright (c) 2016 MIKAMI, Naoki //------------------------------------------------------------ #include "ACM1602NI.hpp" #include "DRV8830.h" #include "IQS62x.h" #include "IQSdisplayTerminal.h" #include "MotorMove.h" //Cycle #define UP_DURATION 14 //On time [*100ms] #define WAIT_DELAY 10 //Delay time [*100ms] #define DOWN_DURATION 8 //Down time [*50ms] #define SWITCH_PERIOD 50 //Cycle time[*50ms] #define TOTAL_TIMES 30000 //total times n #define TIMER_COUNT 0.01 #define LOOP_WAITMS 30 #define UP_THRESHOLD 10 #define DOWN_THRESHOLD 90 Ticker timer_; using namespace Mikami; Acm1602Ni lcd_; // Default, OK //Acm1602Ni lcd_(D14, D15); // OK //Acm1602Ni lcd_(D14, D15, 200000); // OK //Acm1602Ni lcd_(D14, D15, 200000, true, true); // OK //Acm1602Ni lcd_(PB_3, PB_10); // OK //Acm1602Ni lcd_(PC_9, PA_8); // OK //Acm1602Ni lcd_(PB_4, PA_8); // OK I2C i2c(D14, D15); 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); 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; //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; ret = (buffer[0x8E]); return ret; } void Displaylevel (int deg) { int level=deg>>5; lcd_.WriteStringXY("@",0,0); for (int i=0;i<12;i++) { if (i<level) { lcd_.WriteString("-"); } else { lcd_.WriteString(" "); } } }