STM32足回りプログラム
Dependencies: mbed ros_lib_kinetic
Diff: main.cpp
- Revision:
- 0:a202e1bc38a1
- Child:
- 1:7b0db5ea0ab7
diff -r 000000000000 -r a202e1bc38a1 main.cpp --- /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