Thinkbed
Use IQS62X sensor and move motor by detected angle
Dependencies: DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed
Fork of
Nucleo_ACM1602_I2C_DC
by 
Thinkbed
Revision 12:8464be95bf76, committed 2018-01-30
- Comitter:
- 8mona
- Date:
- Tue Jan 30 06:55:27 2018 +0000
- Parent:
- 11:80b6c5d77073
- Commit message:
- Up version
Changed in this revision
| MotorMove.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 80b6c5d77073 -r 8464be95bf76 MotorMove.cpp
--- a/MotorMove.cpp Mon Oct 02 19:50:39 2017 +0000
+++ b/MotorMove.cpp Tue Jan 30 06:55:27 2018 +0000
@@ -5,8 +5,8 @@
//float vol_decel_ini[DECEL_SIZE] = {1,0.75,0.4,0.15,0.05,0};
float vol_decel_ini[DECEL_SIZE] = {-0.5,0,0,0,0,0};
-//float vol_accel_ini[ACCEL_SIZE] = {0,0.2,0.8,1,1,1};
-float vol_accel_ini[ACCEL_SIZE] = {0.2,0.4,0.6,0.8,.9,1.0};
+float vol_accel_ini[ACCEL_SIZE] = {1,1,1,1,1,1};
+//float vol_accel_ini[ACCEL_SIZE] = {0.2,0.4,0.6,0.8,.9,1.0};
void MotorMove::up_motor_set(int time_counter, float speed)
{
diff -r 80b6c5d77073 -r 8464be95bf76 main.cpp
--- a/main.cpp Mon Oct 02 19:50:39 2017 +0000
+++ b/main.cpp Tue Jan 30 06:55:27 2018 +0000
@@ -4,7 +4,12 @@
// 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"
@@ -18,18 +23,33 @@
#define SWITCH_PERIOD 50 //Cycle time[*50ms]
#define TOTAL_TIMES 30000 //total times n
#define TIMER_COUNT 0.01
-#define LOOP_WAITMS 50
+#define MIN_ANGLE -90
+
-#define DEGREE_SHIFT 100
-#define UP_THRESHOLD 10
-#define DOWN_THRESHOLD 30
+#define LOOP_WAITMS 50
+#define SPEED_RATIO 0.5
-#define L_UP .95
-#define R_UP .95
-#define L_DOWN 0.85
-#define R_DOWN 0.85
+#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
@@ -48,16 +68,23 @@
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
@@ -66,6 +93,9 @@
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
@@ -80,6 +110,12 @@
int MainIOloop();
void DisplayStatus();
void MoveMotor();
+void ShowLED(int angle);
+void HSV2RGB(const float *HSV, float *RGB);
+
+
+
+
int main()
{
@@ -93,25 +129,40 @@
//Initialize Ic2 Device
- motorL.speed(0);
- motorR.speed(0);
+ //motorL.speed(0);
+ //motorR.speed(0);
#ifdef ANGLE_ENABLE
- lcd_.WriteStringXY("IQS_Init_Start",0,0);
- iqs62x.configure(); // configure
+ 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);
- 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
-
+
@@ -136,6 +187,18 @@
//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 ++;
}
}
@@ -151,23 +214,44 @@
//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( 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
+ if (shaft_deg< (DOWN_THRESHOLD+degree_shift) )
+ {
+ bflag_down_cur=1;
+ }
+ else
+ {
+ bflag_down_cur=0;
+ }
+
+ }
+
+
+
+ else if (button1==0)
{
- bflag_down_cur=0;
- };
+ bflag_up_cur=0;
+ bflag_down_cur=1;
+ }
+
+
+
+
+
#else
if (button1==1)
{
@@ -181,10 +265,26 @@
}
#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);
@@ -192,6 +292,8 @@
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);
@@ -210,14 +312,58 @@
//motorL.speed(0.5);
//motorR.speed(0.3);
- lcd_.WriteValueXY("%1.2f",lspeed,3,1);
- lcd_.WriteValueXY("%1.2f",rspeed,8,1);
+ 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()
@@ -235,18 +381,29 @@
{
static int cnt=0;
#ifdef ANGLE_ENABLE
- iqs62x.waitForIqsReady();
+ //iqs62x.waitForIqsReady();
iqs62x.readIqsRegisters(0,NUMBER_OF_REGISTERS); // read all the registers
- shaft_deg = ReadDegree(iqs62x.registers)-initial_deg+ DEGREE_SHIFT;
- #endif
+ //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);
@@ -264,6 +421,10 @@
}
*/
+
+ #ifdef LED_ENABLE
+ ShowLED(shaft_deg);
+ #endif
cnt++;
@@ -348,8 +509,57 @@
{
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;
}
+
+
+
-
-