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:
- 9:b58e7d72a91c
- Parent:
- 8:f7ad1d7176ba
- Child:
- 10:ef379cbc0004
diff -r f7ad1d7176ba -r b58e7d72a91c main.cpp --- a/main.cpp Sat Sep 30 16:43:51 2017 +0000 +++ b/main.cpp Mon Oct 02 19:12:02 2017 +0000 @@ -4,6 +4,7 @@ // 2016/04/01, Copyright (c) 2016 MIKAMI, Naoki //------------------------------------------------------------ +//#define ANGLE_ENABLE #include "ACM1602NI.hpp" #include "DRV8830.h" #include "IQS62x.h" @@ -17,10 +18,11 @@ #define SWITCH_PERIOD 50 //Cycle time[*50ms] #define TOTAL_TIMES 30000 //total times n #define TIMER_COUNT 0.01 -#define LOOP_WAITMS 30 +#define LOOP_WAITMS 50 +#define DEGREE_SHIFT 100 #define UP_THRESHOLD 10 -#define DOWN_THRESHOLD 90 +#define DOWN_THRESHOLD 30 Ticker timer_; @@ -35,13 +37,13 @@ //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; +#ifdef ANGLE_ENABLE IQS62xIO iqs62x; // class for basic IQS62x block read and write -InterruptIn button1(USER_BUTTON); +#endif +DigitalIn button1(USER_BUTTON); DigitalIn in_switchs[]= @@ -57,6 +59,9 @@ static int initial_deg=0; +DRV8830 motorL(i2c, DRV8830ADDR_NN); //Motor1 +DRV8830 motorR(i2c, DRV8830ADDR_0N); //Motor2 + void ShowLCD(char * buffer, int startbyte, int endbyte); // for wheel output int ReadDegree(char * buffer); @@ -78,21 +83,32 @@ //motor.speed(0); //Initialize Ic2 Device + + motorL.speed(0); motorR.speed(0); + + + #ifdef ANGLE_ENABLE 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); + #else + lcd_.WriteStringXY("No_Sensor",0,0); + initial_deg=0; + #endif + + - lcd_.WriteStringXY("IQS_Init_done",0,0); - wait(1); //read 0deg for initialize - button1.fall(&flip); + //button1.fall(&flip); //TimerIsr(); //timer_.attach(&TimerIsr, TIMER_COUNT); @@ -102,7 +118,7 @@ // } //Read here as Asynchronous when data gets ready -while (true) { +while (true) { int time_current = g_timer; int time_diff = time_current - time_prev; int a= MainIOloop(); @@ -112,11 +128,8 @@ //motorR.speed( (shaft_deg-180.0)/200.0 ); wait_ms(LOOP_WAITMS); MoveMotor(); - cnt ++; - } - - + } } void MoveMotor(){ @@ -128,8 +141,9 @@ float rspeed; -//detect up or donw by thredold - if (shaft_deg>UP_THRESHOLD) +//detect up or donw by thredold + #ifdef ANGLE_ENABLE + if (shaft_deg> (UP_THRESHOLD+DEGREE_SHIFT) ) { bflag_up_cur=1; } @@ -138,7 +152,7 @@ bflag_up_cur=0; }; - if (shaft_deg<DOWN_THRESHOLD) + if (shaft_deg< (DOWN_THRESHOLD+DEGREE_SHIFT) ) { bflag_down_cur=1; } @@ -146,33 +160,45 @@ { bflag_down_cur=0; }; - + #else + if (button1==1) + { + bflag_up_cur=1; + bflag_down_cur=0; + } + else + { + bflag_up_cur=0; + bflag_down_cur=1; + } + #endif //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); + mvalL.up_motor_set(cnt, 0.9); + mvalR.up_motor_set(cnt, 0.9); 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]); + 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); + //motorL.speed(0.5); + //motorR.speed(0.3); lcd_.WriteValueXY("%1.2f",lspeed,3,1); lcd_.WriteValueXY("%1.2f",rspeed,8,1); @@ -198,22 +224,53 @@ int MainIOloop() { static int cnt=0; - //iqs62x.waitForIqsReady(); + #ifdef ANGLE_ENABLE + iqs62x.waitForIqsReady(); iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers - shaft_deg = ReadDegree(iqs62x.registers)-initial_deg; + shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT; + #endif + + if(shaft_deg<0) { - shaft_deg = shaft_deg+360; + shaft_deg = shaft_deg+360; // offset 100deg to cancel error } + + #ifdef ANGLE_ENABLE shaft_speed= ReadSpeed(iqs62x.registers); //lcd_.WriteValueXY("%3d ",k, 0,0); + #endif + sw_in[0]= in_switchs[0]; + sw_in[1]= !in_switchs[1]; + sw_in[2]= in_switchs[2]; + sw_in[3]= ! in_switchs[3]; + +/* for (int i=0;i<4;i=i++){ sw_in[i]=!in_switchs[i]; - //sw_in[i+1]=!in_switchs[i+1]; + sw_in[i+1]=!(in_switchs[i+1]); } + */ + + + cnt++; + + + bool statusL = motorL.status(); + if (statusL & DRV8830_F_FAULT){ + motorL.reset(); + } + + bool statusR = motorR.status(); + if (statusR & DRV8830_F_FAULT){ + motorR.reset(); + } + + + return cnt; } @@ -227,8 +284,6 @@ } - - void flip() { static bool b = false;