Thinkbed
/
Nucleo_ACM1602_I2C_DC_Angle
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed
Fork of
Nucleo_ACM1602_I2C_DC
by 
Thinkbed
Revision 9:b58e7d72a91c, committed 2017-10-02
- Comitter:
- 8mona
- Date:
- Mon Oct 02 19:12:02 2017 +0000
- Parent:
- 8:f7ad1d7176ba
- Child:
- 10:ef379cbc0004
- Commit message:
- Enable non-sensor mode and define curve
Changed in this revision
--- a/MotorMove.cpp Sat Sep 30 16:43:51 2017 +0000
+++ b/MotorMove.cpp Mon Oct 02 19:12:02 2017 +0000
@@ -5,32 +5,36 @@
//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.5,0.8,1,1,1};
+//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,1.0,1.0};
-void MotorMove::up_motor_set(int time_counter, int speed)
+void MotorMove::up_motor_set(int time_counter, float speed)
{
- bflag_up =1;
+ bflag_up =1;
bflag_down =0;
start_time_count=time_counter;
+ accel_count = 0;
+ decel_count = 0;
for (int i=0;i<ACCEL_SIZE;i++)
{
- vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
+ vol_accel[i] = -vol_accel_ini[i]*speed/SPEED_RATIO;
}
-
+
for (int i=0;i<DECEL_SIZE;i++)
{
- vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO;
+ vol_decel[i] = -vol_decel_ini[i]*speed/SPEED_RATIO;
}
};
-
-void MotorMove::down_motor_set(int time_counter, int speed)
+void MotorMove::down_motor_set(int time_counter, float speed)
{
- bflag_up =0;
- bflag_down=1;
+ bflag_up =0;
+ bflag_down =1;
start_time_count=time_counter;
-
+ accel_count = 0;
+ decel_count = 0;
+
for (int i=0;i<ACCEL_SIZE;i++)
{
vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
@@ -43,87 +47,97 @@
};
-float MotorMove::ReturnMotorVol(int time_counter, int sw_flag1,int sw_flag2)
+float MotorMove::ReturnMotorVol(int time_counter,int bflag_downed, int bflag_upped)
{
float speed;
- int time = time_counter - start_time_count;
-
-
- static int sw_flag1_pre=0;
- static int sw_flag2_pre=0;
- static int stop_time=0;
+ int time;
- static int up_decel_count=0;
- static int down_decel_count=0;
+ static int flag_up_stopping=0;
+ static int flag_down_stopping=0;
- //decel timer check
- if(bflag_up==1)
- up_decel_count==0;
- if(bflag_down==1)
- down_decel_count==0;
-
-
+ //Check flag for normal mode or brake mode
+ //accel_count MODE
+
- if(bflag_up)
- {
- if (sw_flag2==1 && sw_flag2_pre==0) //State changed
- {
- stop_time=time_counter;
- }
-
- if(sw_flag2==0) // move motor
- {
- if(time>ACCEL_SIZE)
- {
- time = ACCEL_SIZE;
- }
- speed = vol_accel[time];
- }
-
- else if (sw_flag2==1)
- {
- time = time_counter-stop_time;
-
- if(time>DECEL_SIZE)
- {
- time = DECEL_SIZE;
- bflag_up =0;
- }
- speed = vol_decel[time];
+ if (bflag_up==1 && bflag_upped ==1)
+ {
+ flag_up_stopping=1;
+ //bflag_up==0;
+ }
+ else if (bflag_down==1 && bflag_downed ==1)
+ {
+ flag_down_stopping=1;
+ //bflag_down=0;
+ }
+
+ if (bflag_up==1)
+ {
+ if (flag_up_stopping==0)
+ {
+ if (accel_count<(ACCEL_SIZE-1))
+ {
+ speed = vol_accel[accel_count];
+ accel_count++;
+ }
+ else
+ {
+ speed = vol_accel[ACCEL_SIZE-1];
+ }
+ }
+ else if(flag_up_stopping==1)
+ {
+ if (decel_count<=(DECEL_SIZE-1))
+ {
+ speed = vol_decel[decel_count];
+ decel_count++;
+ }
+ else
+ {
+ bflag_up=0;
+ speed=0;
+ flag_up_stopping=0;
+ }
}
}
-
- else if (bflag_down)
+ else if (bflag_down==1)
+ {
+ if (flag_down_stopping==0)
+ {
+ if (accel_count<(ACCEL_SIZE-1))
+ {
+ speed = vol_accel[accel_count];
+ accel_count++;
+ }
+ else
+ {
+ speed = vol_accel[ACCEL_SIZE-1];
+ }
+ }
+ else if(flag_down_stopping==1)
+ {
+ if (decel_count<=(DECEL_SIZE-1))
+ {
+ speed = vol_decel[decel_count];
+ decel_count++;
+ }
+ else
+ {
+ bflag_down=0;
+ speed=0;
+ flag_down_stopping=0;
+ }
+ }
+ }
+ else
{
- if (sw_flag1==1 && sw_flag1_pre==0) //State changed
- {
- stop_time=time_counter;
- }
+ speed=0;
+ }
+
- if(sw_flag1==0) // move motor
- {
- if(time>ACCEL_SIZE)
- {
- time = ACCEL_SIZE;
- }
- speed = -vol_accel[time];
- }
-
- else if (sw_flag1==1)
- {
- time = time_counter-stop_time;
- if(time>DECEL_SIZE)
- {
- time = DECEL_SIZE;
- bflag_down =0;
- }
- speed = -vol_decel[time];
- }
- }
+ //move motor before push stopping switch
+ return speed;
+
- sw_flag1_pre=sw_flag1;
- sw_flag2_pre=sw_flag2;
- return speed;
};
--- a/MotorMove.h Sat Sep 30 16:43:51 2017 +0000
+++ b/MotorMove.h Mon Oct 02 19:12:02 2017 +0000
@@ -13,19 +13,21 @@
float vol_decel[DECEL_SIZE];
float vol_accel[ACCEL_SIZE];
float down_ratio;
+ int bflag_up;
+ int bflag_down;
- void up_motor_set (int time_counter, int speed);
- void down_motor_set (int time_counter, int speed);
+ void up_motor_set (int time_counter, float speed);
+ void down_motor_set (int time_counter, float speed);
float ReturnMotorVol(int time_counter, int sw_flag1,int sw_flag2);
private:
int start_time_count;
float Voltage;
+ int accel_count;
+ int decel_count;
- int bflag_up;
- int bflag_down;
};
\ No newline at end of file
--- 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;