![](/media/cache/profiles/P1030212_resized.JPG.50x50_q85.jpg)
STM32足回りプログラム
Dependencies: mbed ros_lib_kinetic
main.cpp
- Committer:
- TanakaRobo
- Date:
- 2019-07-09
- Revision:
- 1:7b0db5ea0ab7
- Parent:
- 0:a202e1bc38a1
- Child:
- 2:c040883ea536
File content as of revision 1:7b0db5ea0ab7:
#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 0.98 #define Moter_NUM 4 #define PERIOD 2048 #define PI2_3 2.0943951023931954923084289221863 #define PI_3 1.0471975511965977461542144610932 #define PI_90 0.034906585 #define PI_4 0.785398163 #define Moter_L 100 //駆動輪と計測輪のそれぞれの回転中心とタイヤの距離 #define Rotary_L 100 #define kp 0.00002 //モーター #define ki 0.003 #define kd 0.0000007 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 = 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); 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,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); Moter[id][1]->write(0); Led[id]->write(1); }else{ Moter[id][0]->write(0); Moter[id][1]->write(-pwm); Led[id]->write(1); } return true; } inline void move(){ 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 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 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] = driveV[j] - nowV[j]; if(nowV[j] == 0 && driveV[j] == 0 && errer[j] != 0){ errer[j] = 0; }else{ errer[j] += diff[j] * now_t; } diffV[j] = (nowV[j] - lastV[j]) / now_t; lastV[j] = nowV[j]; 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; } } 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; 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]); 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*25.4*M_PI,256,2); } I2C i2c(PB_3,PB_10); Timer loop; loop.start(); while(button && !Ok){ nh.spinOnce(); if(loop.read_ms() > 250){ led = !led; loop.reset(); } } 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; 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();//Place Pspeed[i] = Speed[i]->getSpeed();//Place } //オドメトリ計算 #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; 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 if(!button && !flag){ flag = true; Reset(); }else if(button && flag){ flag = false; } } }