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-10-02
- Revision:
- 9:b58e7d72a91c
- Parent:
- 8:f7ad1d7176ba
- Child:
- 10:ef379cbc0004
File content as of revision 9:b58e7d72a91c:
//------------------------------------------------------------ // 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 //------------------------------------------------------------ //#define ANGLE_ENABLE #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 50 #define DEGREE_SHIFT 100 #define UP_THRESHOLD 10 #define DOWN_THRESHOLD 30 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); MotorMove mvalL; MotorMove mvalR; #ifdef ANGLE_ENABLE IQS62xIO iqs62x; // class for basic IQS62x block read and write #endif DigitalIn 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; 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); 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); #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 //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 #ifdef ANGLE_ENABLE if (shaft_deg> (UP_THRESHOLD+DEGREE_SHIFT) ) { bflag_up_cur=1; } else { bflag_up_cur=0; }; if (shaft_deg< (DOWN_THRESHOLD+DEGREE_SHIFT) ) { bflag_down_cur=1; } else { 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, 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]); 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); 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; #ifdef ANGLE_ENABLE iqs62x.waitForIqsReady(); iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT; #endif if(shaft_deg<0) { 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]); } */ 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; } 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(" "); } } }