Diff: main.cpp

Revision:

1:7b0db5ea0ab7

Parent:

0:a202e1bc38a1

Child:

2:c040883ea536

diff -r a202e1bc38a1 -r 7b0db5ea0ab7 main.cpp
--- a/main.cpp	Mon Jun 17 13:41:24 2019 +0000
+++ b/main.cpp	Tue Jul 09 07:47:43 2019 +0000
@@ -5,18 +5,20 @@
 #include "library/RotaryInc.hpp"
 #include "library/GY521.hpp"
 
-#define MAXPWM 250
+#define MAXPWM 0.98
+#define Moter_NUM 4
 #define PERIOD 2048
-#define Moter_NUM 4
 
 #define PI2_3 2.0943951023931954923084289221863
 #define PI_3 1.0471975511965977461542144610932
-#define Moter_L 100
+#define PI_90 0.034906585
+#define PI_4 0.785398163
+#define Moter_L 100 //駆動輪と計測輪のそれぞれの回転中心とタイヤの距離
 #define Rotary_L 100
 
-#define Kp 0.048
-#define Ki 1.6
-#define Kd 0.00008
+#define kp 0.00002 //モーター
+#define ki 0.003
+#define kd 0.0000007
 
 const PinName pwm_pin[7][3] = {
     {PB_1 ,PA_11,PC_6 },
@@ -39,7 +41,7 @@
     {PA_8 ,PA_9 }
 };
 
-ScrpSlave slave(PC_12,PD_2,PH_1,0x0807ffff);
+//ScrpSlave slave(PC_12,PD_2,PH_1,0x0807ffff);
 
 DigitalOut led(PA_5);
 DigitalIn button(PC_13);
@@ -52,15 +54,20 @@
 GY521 *gy;
 bool Ok = false;
 double X = 0,Y = 0,T = 0,Yaw = 0;//オドメトリ
-double Vx,Vy,Omega;//速度
-double driveS[Moter_NUM];
+double Vx = 0,Vy = 0,Omega = 0;//速度
+double driveS[Moter_NUM],nowV[Moter_NUM];
 
 ros::NodeHandle nh;
 
+inline double constrain(double x,double a,double b){
+    return (x < a ? a : (x > b ? b : x));
+}
+
 void safe(){
     Vx = 0;
     Vy = 0;
     Omega = 0;
+    automove = false;
     for(int i = 0;i < Moter_NUM;i++){
         Moter[i][0]->write(0);
         Moter[i][1]->write(0);
@@ -82,65 +89,95 @@
     }
 }    
 
-inline bool Drive(int id,int pwm){//モーターを回す
-    pwm = min(max(-MAXPWM,pwm),MAXPWM);
+inline bool Drive(int id,double pwm){//モーターを回す
+    pwm = constrain(pwm,-MAXPWM,MAXPWM);
     if(!pwm){
         Moter[id][0]->write(0);
         Moter[id][1]->write(0);
         Led[id]->write(0);
     }else if(0 < pwm){
-        Moter[id][0]->write(pwm/255);
+        Moter[id][0]->write(pwm);
         Moter[id][1]->write(0);
         Led[id]->write(1);
     }else{
         Moter[id][0]->write(0);
-        Moter[id][1]->write(-pwm/255);
+        Moter[id][1]->write(-pwm);
         Led[id]->write(1);
     }
     return true;
 }
 
 inline void move(){
-    static double diff[Moter_NUM],errer[Moter_NUM],nowV[Moter_NUM],diffV[Moter_NUM],lastV[Moter_NUM],now_t;
+    static double diff[Moter_NUM],errer[Moter_NUM],diffV[Moter_NUM],lastV[Moter_NUM],driveV[Moter_NUM],now_t,cos_yaw,sin_yaw;
     static int j;
 #if Moter_NUM == 3
-    driveS[0] = Vx*cos(Yaw)         + Vy*sin(Yaw)         + Omega*Moter_L;
-    driveS[1] = Vx*cos(Yaw + PI2_3) + Vy*sin(Yaw + PI2_3) + Omega*Moter_L;
-    driveS[2] = Vx*cos(Yaw - PI2_3) + Vy*sin(Yaw - PI2_3) + Omega*Moter_L;
+    driveV[0] = Vx*cos(Yaw)         + Vy*sin(Yaw)         + Omega*Moter_L;
+    driveV[1] = Vx*cos(Yaw + PI2_3) + Vy*sin(Yaw + PI2_3) + Omega*Moter_L;
+    driveV[2] = Vx*cos(Yaw - PI2_3) + Vy*sin(Yaw - PI2_3) + Omega*Moter_L;
 #elif Moter_NUM == 4
-    driveS[0] =  Vx*cos(Yaw) + Vy*sin(Yaw) + Omega*Moter_L;
-    driveS[1] = -Vx*sin(Yaw) + Vy*cos(Yaw) + Omega*Moter_L;
-    driveS[2] = -Vx*cos(Yaw) - Vy*sin(Yaw) + Omega*Moter_L;
-    driveS[3] =  Vx*sin(Yaw) - Vy*cos(Yaw) + Omega*Moter_L;
+    cos_yaw = cos(Yaw);
+    sin_yaw = sin(Yaw);
+    driveV[0] =  Vx*cos_yaw + Vy*sin_yaw + Omega*Moter_L;
+    driveV[1] = -Vx*sin_yaw + Vy*cos_yaw + Omega*Moter_L;
+    driveV[2] = -Vx*cos_yaw - Vy*sin_yaw + Omega*Moter_L;
+    driveV[3] =  Vx*sin_yaw - Vy*cos_yaw + Omega*Moter_L;
 #endif
     now_t = motertimer.read();
     motertimer.reset();
     for(j = 0;j < Moter_NUM;j++){
         nowV[j] = Speed[j]->getSpeed();
-        diff[j] = driveS[j] - nowV[j];
-        if(nowV[j] == 0 && driveS[j] == 0 && errer[j] != 0){
+        diff[j] = driveV[j] - nowV[j];
+        if(nowV[j] == 0 && driveV[j] == 0 && errer[j] != 0){
             errer[j] = 0;
+        }else{
+            errer[j] += diff[j] * now_t;
         }
-        errer[j] += diff[j] * now_t;
         diffV[j] = (nowV[j] - lastV[j]) / now_t;
         lastV[j] = nowV[j];
-        Drive(j,0.08 * driveS[j] + diff[j] * Kp + errer[j] * Ki - diffV[j] * Kp);
+        driveS[j] = 0.0003 * driveV[j] + diff[j] * kp + errer[j] * ki - diffV[j] * kd;
+        Drive(j,driveS[j]);
     }
 }
 
 void getData(const std_msgs::Float32MultiArray &msgs){
+    if(!Ok && (int)msgs.data[0] != -1)return;
     switch((int)msgs.data[0]){
         case -1:
+            ablemove = true;
             Reset();
             break;
         case 0:
+            ablemove = true;
             Vx = msgs.data[1];
             Vy = msgs.data[2];
             Omega = msgs.data[3];
             break;
         case 1:
+            ablemove = true;
+            goal_x = msgs.data[1];
+            goal_y = msgs.data[2];
+            goal_yaw = msgs.data[3];
+            automove = true;
+            errer_x = 0;
+            errer_y = 0;
+            errer_omega = 0;
+            autotimer.reset();
+            autotimer.start();
+            break;
+        case 2:
+            ablemove = true;
             safe();
             break;
+        case 3:
+            ablemove = false;
+            Drive(0,msgs.data[1]/255);
+            Drive(1,msgs.data[2]/255);
+            break;
+        case 4:
+            ablemove = false;
+            Drive(2,msgs.data[1]/255);
+            Drive(3,msgs.data[2]/255);
+            break;
     }
 }
 
@@ -160,6 +197,8 @@
     double diff[Moter_NUM],Pspeed[Moter_NUM];
     double nowVx,nowVy,nowVt;
     double cos_yaw_2,sin_yaw_2;
+    double diff_x,diff_y,diff_yaw;
+    double now_t,use_amax;
     for(i = 0;i < Moter_NUM;i++){
         Led[i] = new DigitalOut(pwm_pin[i][2]);
         Moter[i][0] = new PwmOut(pwm_pin[i][0]);
@@ -167,21 +206,32 @@
         Moter[i][0]->period_us(PERIOD);
         Moter[i][1]->period_us(PERIOD);
         Speed[i] = new RotaryInc(rotary_pin[i][0],rotary_pin[i][1],2*50.8*M_PI,256,2);
-        Place[i] = new RotaryInc(rotary_pin[i+Moter_NUM][0],rotary_pin[i+Moter_NUM][1],2*50.8*M_PI,256,2);
+        Place[i] = new RotaryInc(rotary_pin[i+Moter_NUM][0],rotary_pin[i+Moter_NUM][1],2*25.4*M_PI,256,2);
     }
+    I2C i2c(PB_3,PB_10);
     Timer loop;
     loop.start();
-    while(!button && !Ok){
+    while(button && !Ok){
         nh.spinOnce();
         if(loop.read_ms() > 250){
             led = !led;
             loop.reset();
         }
     }
-    I2C i2c(PB_3,PB_10);
+    led.write(0);
+    Ok = true;
     GY521 gyro(i2c);
     gy = &gyro;
+    motertimer.start();
+    led.write(1);
     while(true){
+        if(ablemove){
+            move();
+        }else{
+            for(i = 0;i<4;i++){
+                nowV[i] = Speed[i]->getSpeed();
+            }
+        }
         nh.spinOnce();
         gyro.updata();
         Yaw = gyro.yaw;
@@ -199,8 +249,8 @@
         }
         Yaw *= 0.01745329251994329576923690768489;//PI/180
         for(i = 0;i<Moter_NUM;++i){
-            diff[i] = Place[i]->diff();
-            Pspeed[i] = Speed[i]->getSpeed();
+            diff[i] = Place[i]->diff();//Place
+            Pspeed[i] = Speed[i]->getSpeed();//Place
         }
         //オドメトリ計算
 #if Moter_NUM == 3
@@ -216,15 +266,16 @@
         X += diff[0]*cos_yaw_2 - diff[1]*sin_yaw_2 - diff[2]*cos_yaw_2 + diff[3]*sin_yaw_2;
         Y += diff[0]*sin_yaw_2 + diff[1]*cos_yaw_2 - diff[2]*sin_yaw_2 - diff[3]*cos_yaw_2;
         T += diff[0]/Rotary_L/4 + diff[1]/Rotary_L/4 + diff[2]/Rotary_L/4 + diff[3]/Rotary_L/4;
+        cos_yaw_2 = cos(Yaw+PI_4)/2.0;
+        sin_yaw_2 = sin(Yaw+PI_4)/2.0;
         nowVx = Pspeed[0]*cos_yaw_2 - Pspeed[1]*sin_yaw_2 - Pspeed[2]*cos_yaw_2 + Pspeed[3]*sin_yaw_2;
         nowVy = Pspeed[0]*sin_yaw_2 + Pspeed[1]*cos_yaw_2 - Pspeed[2]*sin_yaw_2 - Pspeed[3]*cos_yaw_2;
         nowVt = Pspeed[0]/Rotary_L/4 + Pspeed[1]/Rotary_L/4 + Pspeed[2]/Rotary_L/4 + Pspeed[3]/Rotary_L/4;
 #endif
-        move();
-        if(button && !flag){
+        if(!button && !flag){
             flag = true;
             Reset();
-        }else if(!button && flag){
+        }else if(button && flag){
             flag = false;
         }
     }