GR-PEACH_producer_meeting
MotorDrive制御
Dependencies: mbed Motor PID QEI2 fusion_Motor
Fork of
Nucleo_Motor
by 
Kiko Ishimoto
Revision 1:ad8acc6cfba7, committed 2015-10-30
- Comitter:
- kikoaac
- Date:
- Fri Oct 30 12:48:08 2015 +0000
- Parent:
- 0:8719ed91e6fd
- Commit message:
- ???
;
Changed in this revision
--- a/main.cpp Wed Sep 23 06:06:49 2015 +0000
+++ b/main.cpp Fri Oct 30 12:48:08 2015 +0000
@@ -11,7 +11,7 @@
#include "fusion_Motor.h"
#include "Pin.h"
#define Lx 0.0001
-#define Ku 0.32200//0.922000
+#define Ku 0.48000//0.922000
#define Pu 0.0125
#define Kp Ku*0.6
#define Ti Pu*0.5
@@ -27,6 +27,7 @@
#define Databit 0
#define TIME 0.0125
Timer Time;
+DigitalOut Elec(D15);
/*
PID p(P,I,D,&Time);
QEI i(QEI1_A,QEI1_B,NC,50,&Time);
@@ -34,7 +35,7 @@
fusion_Motor mecanam3(i,p,d);
*/
-
+float duty_max;
Serial device(D8,D2);
/*
PID *pid;
@@ -43,7 +44,7 @@
*/
PID pid[4] = {PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time)};
QEI wheel[4] = {QEI(QEI1_A,QEI1_B,NC,100,&Time),QEI(QEI2_A,QEI2_B,NC,100,&Time),QEI(QEI3_A,QEI3_B,NC,100,&Time),QEI(QEI4_A,QEI4_B,NC,100,&Time)};
-Motor motor[4] = {Motor(A1,A2,A3,A4,A0),Motor(PD_2,PA_13,PA_14,PA_15,PC_9),Motor(D5,D4,D3,PB_8,PB_9),Motor(D12,D11,D10,D9,D7)};
+Motor motor[4] = {Motor(A1,A2,A3,A4,A0),Motor(PD_2,PA_13,PA_14,PA_15,PC_9),Motor(D7,D6,D5,D4,D3),Motor(D13,D12,D11,D10,D9)};
fusion_Motor mecanam[4] = {
fusion_Motor(wheel[0],pid[0],motor[0]),//front right
fusion_Motor(wheel[1],pid[1],motor[1]),//Back right
@@ -98,7 +99,6 @@
enum STATE {
R,L,F,B
} state;
-PwmOut LED(LED1);
#define PI 3.141592653589793
float DUTY_fill[4]= {0};
float DUTY_inte[4]= {0};
@@ -113,7 +113,7 @@
*data = GAIN_P*dat+GAIN_D**fill+GAIN_I**inte;
*fill = (dat - *prev_dat)*inter;
*prev_dat =dat;
-
+
/*
if(tardata>*data)
*data+=0.01f;
@@ -135,7 +135,16 @@
double mecCot(float angle,STATE RorL)
{
double radian = (double)angle * PI / 180.0;
- return RorL==R ? 1.4142135623730950488016887242097*sin(radian-PI/4-PI/2) : 1.4142135623730950488016887242097*cos(radian-PI/4-PI/2);
+ return RorL==R ? 1.4142135623730950488016887242097*sin(radian-PI/4) : 1.4142135623730950488016887242097*cos(radian-PI/4);
+}
+bool stop_flag()
+{
+ bool flag=1;
+ for(int i=0;i<4;i++)
+ {
+ flag &= (DUTY[i] <duty_max*0.8f && DUTY[i]>-duty_max*0.8f);
+ }
+ return flag;
}
void vector(float angle)
{
@@ -163,7 +172,12 @@
void pid_reset()
{
for(int i=0; i<4; i++)
- pid[i].pid_reset();
+ {
+ DUTY_fill[i]= 0;
+ DUTY_inte[i]= 0;
+ DUTY[i]= 0;
+ DUTY_prev[i]= 0;
+ }
}
void getData()
{
@@ -176,20 +190,33 @@
//printf("0x%x ,0x%x ,0x%x ,0x%x\r\n" ,data,speed,sot,Data2);
//printf("getdata!!\r\n");
}
+int flag=0;
bool MotorState()
{
for(int a=0; a<4; a++) {
if(speed==0) {
- mecanam[a].run(Free,1);
+ mecanam[a].run(Stop,1);
+ pid_reset();
+ //mecanam[a].pid_reset();
} else if(speed==1) {
- mecanam[a].run(Stop,1);
-
+ if(stop_flag())
+ {
+ mecanam[a].run(Stop,1.0);
+ pid_reset();
+ }
+ else
+ {
+ motruns(R,0);
+ motruns(L,0);
+ }
} else if(speed==2) {
- mecanam[a].duty_max(0.3);
- mecanam[a].duty_min(-0.3);
+ duty_max=0.2f;
+ mecanam[a].duty_max(duty_max);
+ mecanam[a].duty_min(-duty_max);
} else if(speed==3) {
- mecanam[a].duty_max(1.0);
- mecanam[a].duty_min(-1.0);
+ duty_max=1.0f;
+ mecanam[a].duty_max(duty_max);
+ mecanam[a].duty_min(-duty_max);
}
}
return speed==0||speed==1;
@@ -207,14 +234,17 @@
}
int main()
{
+ Elec=0;
for(int i=0; i<4; i++) {
mecanam[i](fusion_Motor::Normal,1);
mecanam[i].run(Free,1.0);
+ mecanam[i].setbias(0.0);
+ mecanam[i].setbias_motor(0.0);
}
-for(int a=0;a<4;a++){
- mecanam[a].duty_max(1.0);
- mecanam[a].duty_min(-1.0);}
- led=0.5;
+ for(int a=0; a<4; a++) {
+ mecanam[a].duty_max(1.0);
+ mecanam[a].duty_min(-1.0);
+ }
//M=0x1f;
//wait(3);
//setup();
@@ -225,28 +255,61 @@
bool prev_sot=0;
wait(1);
- Ticker tic;
- tic.attach(&f,3);
+// Ticker tic;
+// tic.attach(&f,3);
//mecanam3=200;
//for(float i=-1;i<1;i+=0.05)printf("%f\r\n",mecanam3.rerpmper(i));
//for(int i = 0 ;i<4;i++)mecanam[i]=100;
//wait(5);
DigitalIn PIN(USER_BUTTON);
+ if(PIN==0) {
+
+ bool flag=0;
+ float duty=1;
+ while(1) {
+ for(int i=0;i<4;i++)
+ mecanam[i].run(Front,1);
+ wait(4);
+ for(int i=0;i<4;i++)
+ mecanam[i].run(Front,0);
+ wait(4);
+ /*if(PIN==0&&flag==1) {
+ flag=0;
+ duty-=0.1;
+ } else if(PIN==1&&flag==0) {
+ flag=1;
+ for(int i=0; i<4; i++)
+ mecanam[i].run(Front,duty);
+ }*/
+ }
+ }
+ flag=PIN;
+ int prevang=0;
+ Timer waittimer;
while(1) {
- //printf("start main\r\n");
-
+ //Elec=!PIN;
+ /*
+ for(int x = 0; 1; x++) {
+ vector(x);
+ for(int i=0; i<4; i++)
+ printf("%f ",mecanam[i].getduty());
+ printf("\r\n");
+ }
+ */
+ //printf("start main\r\n");
for(int x = 0; 1; x++) {
if(PIN==0)GAIN_P+=0.001;
- //printf("start main\r\n");
- /*vector(Y);
- for(int i=0; i<4; i++)
- printf("%f ",DUTY[i]);
- printf("%F \r\n",GAIN_P);*/
+ //printf("start main\r\n");
+ /*
+ vector(Y);
+ for(int i=0; i<4; i++)
+ printf("%f ",DUTY[i]);
+ printf("%F \r\n",GAIN_P);*/
wait(TIME);
//mecanam3=mecCot(i,R);
//wait(0.5);
//printf("%5d, %5.5f , %5.5f, %5.5f\r\n",i,mecanam3.getrpm(),mecanam3.rerpmper(mecCot(i,R)),mecanam3.rpmper(300*mecCot(i,R)));
-
+
getData();
//wait(0.01);
if(MotorState())break;
@@ -255,6 +318,7 @@
if(sot==0) {
prev_sot=sot;
float angle = Data2*11.25;
+ prevang=Data2;
vector(angle);
} else if(sot==1) {
if(Data2==0) {
@@ -262,12 +326,15 @@
mecanam[i]=1;
mecanam[i+2]=-1;
}
- }
- else if(Data2==1) {
+ } else if(Data2==1) {
for(int i=0; i<2; i++) {
mecanam[i]=-1;
mecanam[i+2]=1;
}
+ } else if(Data2==2){
+ Elec=1;
+ } else if(Data2==3){
+ Elec=0;
} else {
for(int i=0; i<4; i++)
mecanam[i].run(Stop,1);
@@ -278,3 +345,4 @@
}
+
--- a/motor_lib/Motor.lib Wed Sep 23 06:06:49 2015 +0000 +++ b/motor_lib/Motor.lib Fri Oct 30 12:48:08 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/kikoaac/code/Motor/#dea2df71cb97 +http://developer.mbed.org/users/kikoaac/code/Motor/#56a1159c881c
--- a/motor_lib/PID.lib Wed Sep 23 06:06:49 2015 +0000 +++ b/motor_lib/PID.lib Fri Oct 30 12:48:08 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/kikoaac/code/PID/#3519920d064d +http://developer.mbed.org/users/kikoaac/code/PID/#775c9421fe3b