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:
- 2018-01-30
- Revision:
- 12:8464be95bf76
- Parent:
- 11:80b6c5d77073
File content as of revision 12:8464be95bf76:
//------------------------------------------------------------ // 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 //------------------------------------------------------------ //For Flag if we need sample or not //#define ANGLE_ENABLE #define SERIAL_ENABLE //#define LED_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 MIN_ANGLE -90 #define LOOP_WAITMS 50 #define SPEED_RATIO 0.5 #define UP_THRESHOLD 80 #define DOWN_THRESHOLD 70 #define NUM_LED 3 #define MAX_LED_ANGLE 100 #define MAX_LED_BRI 0.7 #define HSV_SHIFT 0.0 /* #define L_UP .5 #define R_UP .5 #define L_DOWN .5 #define R_DOWN .5 */ #define L_UP 0.7 #define R_UP 0.7 #define L_DOWN 0.65 #define R_DOWN 0.65 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 SERIAL_ENABLE Serial pc(USBTX, USBRX); // tx, rx #endif #ifdef ANGLE_ENABLE IQS62xIO iqs62x; // class for basic IQS62x block read and write #endif DigitalIn button1(USER_BUTTON); #ifdef LED_ENABLE PwmOut LEDPWM[NUM_LED]= {D3,D6,D9}; #endif 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; static int degree_shift=0; float pwmPeriod=0.000005; 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(); void ShowLED(int angle); void HSV2RGB(const float *HSV, float *RGB); 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_Calibration",0,0); //wait(1); //iqs62x.configure(); // configure //wait(1); lcd_.WriteStringXY("IQS_Cconfig done",0,0); iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers initial_deg = ReadDegree(iqs62x.registers); //degree_shift = (360- ReadDegree(iqs62x.registers) ); #ifdef LED_ENABLE for (int i=0; i<NUM_LED ;i=i+1) { LEDPWM[i].period(pwmPeriod); LEDPWM[i]=1; } #endif wait(1); lcd_.WriteStringXY("IQS_Init_done ",0,0); #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(); #ifdef SERIAL_ENABLE pc.printf("%d\r\n", shaft_deg); #endif /* LEDPWM[0]=1; LEDPWM[1]=1; LEDPWM[2]=1; */ 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( button1==1) { 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==0) { bflag_up_cur=0; bflag_down_cur=1; } #else if (button1==1) { bflag_up_cur=1; bflag_down_cur=0; } else { bflag_up_cur=0; bflag_down_cur=1; } #endif int mot_speed; if (shaft_speed==0) { mot_speed=1; } else { mot_speed = shaft_speed; } //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, L_UP * mot_speed * SPEED_RATIO); // mvalR.up_motor_set(cnt, R_UP * mot_speed * SPEED_RATIO); mvalL.up_motor_set(cnt, L_UP); mvalR.up_motor_set(cnt, R_UP); lcd_.WriteStringXY("U",0,1); } else if(bflag_down_pre==0 && bflag_down_cur==1) { // mvalL.down_motor_set(cnt, L_DOWN * mot_speed *SPEED_RATIO); // mvalR.down_motor_set(cnt, R_DOWN * mot_speed *SPEED_RATIO ); mvalL.down_motor_set(cnt, L_DOWN); mvalR.down_motor_set(cnt, R_DOWN); 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); wait_ms(1); motorR.speed(rspeed); //motorL.speed(0.5); //motorR.speed(0.3); lcd_.WriteValueXY("%1.2f ",lspeed*5,3,1); lcd_.WriteValueXY("%1.2f ",rspeed*5,8,1); bflag_up_pre = bflag_up_cur; bflag_down_pre = bflag_down_cur; } #ifdef LED_ENABLE void ShowLED(int angle) { float led_H; float led_V; led_H = (float)angle/(float)MAX_LED_ANGLE+HSV_SHIFT; //led_val = (float)angle/(float)MAX_LED_ANGLE; led_V = (float)angle/(float)MAX_LED_ANGLE; if (led_H>1.0) { led_H = led_H - 1.0; } float RGB[3]; float HSV[3]; HSV[0]=led_H; HSV[1]=1; HSV[2]=MAX_LED_BRI; HSV2RGB(HSV,RGB); LEDPWM[0]= 1.0-RGB[0]*1.1; LEDPWM[1]= 1.0-RGB[1]; LEDPWM[2]= 1.0-RGB[2]; } #endif 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; shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+degree_shift; if (shaft_deg<MIN_ANGLE) { shaft_deg=shaft_deg+360; } else if(shaft_deg>(MIN_ANGLE+360)) { shaft_deg= shaft_deg-360; } /* if(shaft_deg<0) { shaft_deg = shaft_deg+360; // offset 100deg to cancel error } */ #endif #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]); } */ #ifdef LED_ENABLE ShowLED(shaft_deg); #endif 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(" "); } } } void HSV2RGB(const float *HSV, float *RGB) { float h = HSV[0]; float s = HSV[1]; float v = HSV[2]; float r = v; float g = v; float b = v; if (s > 0.0f) { h *= 6.0f; int i = (int) h; float f = h - (float) i; switch (i) { default: case 0: g *= 1 - s * (1 - f); b *= 1 - s; break; case 1: r *= 1 - s * f; b *= 1 - s; break; case 2: r *= 1 - s; b *= 1 - s * (1 - f); break; case 3: r *= 1 - s; g *= 1 - s * f; break; case 4: r *= 1 - s * (1 - f); g *= 1 - s; break; case 5: g *= 1 - s; b *= 1 - s * f; break; } } RGB[0] = r; RGB[1] = g; RGB[2] = b; }