Diff: main.cpp

Revision:

0:a202e1bc38a1

Child:

1:7b0db5ea0ab7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 17 13:41:24 2019 +0000
@@ -0,0 +1,231 @@
+#include "mbed.h"
+#include "ros.h"
+#include "std_msgs/Float32MultiArray.h"
+#include "library/ScrpSlave.hpp"
+#include "library/RotaryInc.hpp"
+#include "library/GY521.hpp"
+
+#define MAXPWM 250
+#define PERIOD 2048
+#define Moter_NUM 4
+
+#define PI2_3 2.0943951023931954923084289221863
+#define PI_3 1.0471975511965977461542144610932
+#define Moter_L 100
+#define Rotary_L 100
+
+#define Kp 0.048
+#define Ki 1.6
+#define Kd 0.00008
+
+const PinName pwm_pin[7][3] = {
+    {PB_1 ,PA_11,PC_6 },
+    {PB_13,PB_14,PB_2 },
+    {PB_4 ,PB_5 ,PB_15},
+    {PC_8 ,PC_9 ,PA_10},
+    {PB_6 ,PB_7 ,PB_12},
+    {PB_8 ,PB_9 ,PC_7 },
+    {PA_0 ,PA_1 ,PB_0 }
+};
+
+const PinName rotary_pin[8][2] = {
+    {PC_10,PC_11},
+    {PC_4 ,PA_13},
+    {PA_14,PA_15},
+    {PC_2 ,PC_3 },
+    {PA_12,PC_5 },
+    {PC_0 ,PC_1 },
+    {PA_6 ,PA_7 },
+    {PA_8 ,PA_9 }
+};
+
+ScrpSlave slave(PC_12,PD_2,PH_1,0x0807ffff);
+
+DigitalOut led(PA_5);
+DigitalIn button(PC_13);
+Timer motertimer;
+
+PwmOut* Moter[Moter_NUM][2];
+DigitalOut* Led[Moter_NUM];
+RotaryInc* Speed[Moter_NUM];
+RotaryInc* Place[Moter_NUM];
+GY521 *gy;
+bool Ok = false;
+double X = 0,Y = 0,T = 0,Yaw = 0;//オドメトリ
+double Vx,Vy,Omega;//速度
+double driveS[Moter_NUM];
+
+ros::NodeHandle nh;
+
+void safe(){
+    Vx = 0;
+    Vy = 0;
+    Omega = 0;
+    for(int i = 0;i < Moter_NUM;i++){
+        Moter[i][0]->write(0);
+        Moter[i][1]->write(0);
+        Led[i]->write(0);
+    }
+}
+
+void Reset(){
+    if(!Ok){
+        Ok = true;
+    }else{
+        led.write(0);
+        safe();
+        gy->reset(0);
+        X = 0;
+        Y = 0;
+        T = 0;
+        led.write(1);
+    }
+}    
+
+inline bool Drive(int id,int pwm){//モーターを回す
+    pwm = min(max(-MAXPWM,pwm),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][1]->write(0);
+        Led[id]->write(1);
+    }else{
+        Moter[id][0]->write(0);
+        Moter[id][1]->write(-pwm/255);
+        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 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;
+#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;
+#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){
+            errer[j] = 0;
+        }
+        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);
+    }
+}
+
+void getData(const std_msgs::Float32MultiArray &msgs){
+    switch((int)msgs.data[0]){
+        case -1:
+            Reset();
+            break;
+        case 0:
+            Vx = msgs.data[1];
+            Vy = msgs.data[2];
+            Omega = msgs.data[3];
+            break;
+        case 1:
+            safe();
+            break;
+    }
+}
+
+std_msgs::Float32MultiArray now;
+ros::Publisher place("place",&now);
+ros::Subscriber<std_msgs::Float32MultiArray> sub("moter",&getData);
+
+int main(){
+    nh.getHardware()->setBaud(115200);
+    nh.initNode();
+    nh.advertise(place);
+    nh.subscribe(sub);
+    now.data_length = 7;
+    now.data = (float*)malloc(sizeof(float)*now.data_length);
+    int i;
+    bool flag = false;
+    double diff[Moter_NUM],Pspeed[Moter_NUM];
+    double nowVx,nowVy,nowVt;
+    double cos_yaw_2,sin_yaw_2;
+    for(i = 0;i < Moter_NUM;i++){
+        Led[i] = new DigitalOut(pwm_pin[i][2]);
+        Moter[i][0] = new PwmOut(pwm_pin[i][0]);
+        Moter[i][1] = new PwmOut(pwm_pin[i][1]);
+        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);
+    }
+    Timer loop;
+    loop.start();
+    while(!button && !Ok){
+        nh.spinOnce();
+        if(loop.read_ms() > 250){
+            led = !led;
+            loop.reset();
+        }
+    }
+    I2C i2c(PB_3,PB_10);
+    GY521 gyro(i2c);
+    gy = &gyro;
+    while(true){
+        nh.spinOnce();
+        gyro.updata();
+        Yaw = gyro.yaw;
+        if(loop.read_ms() > 20){//50msごとに通信する
+            //データ送信
+            now.data[0] = X;
+            now.data[1] = Y;
+            now.data[2] = T;
+            now.data[3] = Yaw;
+            now.data[4] = nowVx;
+            now.data[5] = nowVy;
+            now.data[6] = nowVt;
+            place.publish(&now);
+            loop.reset();
+        }
+        Yaw *= 0.01745329251994329576923690768489;//PI/180
+        for(i = 0;i<Moter_NUM;++i){
+            diff[i] = Place[i]->diff();
+            Pspeed[i] = Speed[i]->getSpeed();
+        }
+        //オドメトリ計算
+#if Moter_NUM == 3
+        X += -2.0/3.0*diff[0]*cos(Yaw) + 2.0/3.0*diff[1]*cos(Yaw-PI_3) + 2.0/3.0*diff[2]*cos(Yaw+PI_3);
+        Y += -2.0/3.0*diff[0]*sin(Yaw) + 2.0/3.0*diff[1]*sin(Yaw-PI_3) + 2.0/3.0*diff[2]*sin(Yaw+PI_3);
+        T +=  diff[0]/Rotary_L/3 + diff[1]/Rotary_L/3 + diff[2]/Rotary_L/3;
+        nowVx = -2.0/3.0*Pspeed[0]*cos(Yaw) + 2.0/3.0*Pspeed[1]*cos(Yaw-PI_3) + 2.0/3.0*Pspeed[2]*cos(Yaw+PI_3);
+        nowVy = -2.0/3.0*Pspeed[0]*sin(Yaw) + 2.0/3.0*Pspeed[1]*sin(Yaw-PI_3) + 2.0/3.0*Pspeed[2]*sin(Yaw+PI_3);
+        nowVt =  Pspeed[0]/Rotary_L/3 + Pspeed[1]/Rotary_L/3 + Pspeed[2]/Rotary_L/3;
+#elif Moter_NUM == 4
+        cos_yaw_2 = cos(Yaw)/2.0;
+        sin_yaw_2 = sin(Yaw)/2.0;
+        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;
+        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){
+            flag = true;
+            Reset();
+        }else if(!button && flag){
+            flag = false;
+        }
+    }
+}
\ No newline at end of file