STM32足回りプログラム
Dependencies: mbed ros_lib_kinetic
main.cpp
- Committer:
- TanakaRobo
- Date:
- 2019-06-17
- Revision:
- 0:a202e1bc38a1
- Child:
- 1:7b0db5ea0ab7
File content as of revision 0:a202e1bc38a1:
#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; } } }