STM32足回りプログラム
Dependencies: mbed ros_lib_kinetic
main.cpp@1:7b0db5ea0ab7, 2019-07-09 (annotated)
- Committer:
- TanakaRobo
- Date:
- Tue Jul 09 07:47:43 2019 +0000
- Revision:
- 1:7b0db5ea0ab7
- Parent:
- 0:a202e1bc38a1
- Child:
- 2:c040883ea536
first commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TanakaRobo | 0:a202e1bc38a1 | 1 | #include "mbed.h" |
TanakaRobo | 0:a202e1bc38a1 | 2 | #include "ros.h" |
TanakaRobo | 0:a202e1bc38a1 | 3 | #include "std_msgs/Float32MultiArray.h" |
TanakaRobo | 0:a202e1bc38a1 | 4 | #include "library/ScrpSlave.hpp" |
TanakaRobo | 0:a202e1bc38a1 | 5 | #include "library/RotaryInc.hpp" |
TanakaRobo | 0:a202e1bc38a1 | 6 | #include "library/GY521.hpp" |
TanakaRobo | 0:a202e1bc38a1 | 7 | |
TanakaRobo | 1:7b0db5ea0ab7 | 8 | #define MAXPWM 0.98 |
TanakaRobo | 1:7b0db5ea0ab7 | 9 | #define Moter_NUM 4 |
TanakaRobo | 0:a202e1bc38a1 | 10 | #define PERIOD 2048 |
TanakaRobo | 0:a202e1bc38a1 | 11 | |
TanakaRobo | 0:a202e1bc38a1 | 12 | #define PI2_3 2.0943951023931954923084289221863 |
TanakaRobo | 0:a202e1bc38a1 | 13 | #define PI_3 1.0471975511965977461542144610932 |
TanakaRobo | 1:7b0db5ea0ab7 | 14 | #define PI_90 0.034906585 |
TanakaRobo | 1:7b0db5ea0ab7 | 15 | #define PI_4 0.785398163 |
TanakaRobo | 1:7b0db5ea0ab7 | 16 | #define Moter_L 100 //駆動輪と計測輪のそれぞれの回転中心とタイヤの距離 |
TanakaRobo | 0:a202e1bc38a1 | 17 | #define Rotary_L 100 |
TanakaRobo | 0:a202e1bc38a1 | 18 | |
TanakaRobo | 1:7b0db5ea0ab7 | 19 | #define kp 0.00002 //モーター |
TanakaRobo | 1:7b0db5ea0ab7 | 20 | #define ki 0.003 |
TanakaRobo | 1:7b0db5ea0ab7 | 21 | #define kd 0.0000007 |
TanakaRobo | 0:a202e1bc38a1 | 22 | |
TanakaRobo | 0:a202e1bc38a1 | 23 | const PinName pwm_pin[7][3] = { |
TanakaRobo | 0:a202e1bc38a1 | 24 | {PB_1 ,PA_11,PC_6 }, |
TanakaRobo | 0:a202e1bc38a1 | 25 | {PB_13,PB_14,PB_2 }, |
TanakaRobo | 0:a202e1bc38a1 | 26 | {PB_4 ,PB_5 ,PB_15}, |
TanakaRobo | 0:a202e1bc38a1 | 27 | {PC_8 ,PC_9 ,PA_10}, |
TanakaRobo | 0:a202e1bc38a1 | 28 | {PB_6 ,PB_7 ,PB_12}, |
TanakaRobo | 0:a202e1bc38a1 | 29 | {PB_8 ,PB_9 ,PC_7 }, |
TanakaRobo | 0:a202e1bc38a1 | 30 | {PA_0 ,PA_1 ,PB_0 } |
TanakaRobo | 0:a202e1bc38a1 | 31 | }; |
TanakaRobo | 0:a202e1bc38a1 | 32 | |
TanakaRobo | 0:a202e1bc38a1 | 33 | const PinName rotary_pin[8][2] = { |
TanakaRobo | 0:a202e1bc38a1 | 34 | {PC_10,PC_11}, |
TanakaRobo | 0:a202e1bc38a1 | 35 | {PC_4 ,PA_13}, |
TanakaRobo | 0:a202e1bc38a1 | 36 | {PA_14,PA_15}, |
TanakaRobo | 0:a202e1bc38a1 | 37 | {PC_2 ,PC_3 }, |
TanakaRobo | 0:a202e1bc38a1 | 38 | {PA_12,PC_5 }, |
TanakaRobo | 0:a202e1bc38a1 | 39 | {PC_0 ,PC_1 }, |
TanakaRobo | 0:a202e1bc38a1 | 40 | {PA_6 ,PA_7 }, |
TanakaRobo | 0:a202e1bc38a1 | 41 | {PA_8 ,PA_9 } |
TanakaRobo | 0:a202e1bc38a1 | 42 | }; |
TanakaRobo | 0:a202e1bc38a1 | 43 | |
TanakaRobo | 1:7b0db5ea0ab7 | 44 | //ScrpSlave slave(PC_12,PD_2,PH_1,0x0807ffff); |
TanakaRobo | 0:a202e1bc38a1 | 45 | |
TanakaRobo | 0:a202e1bc38a1 | 46 | DigitalOut led(PA_5); |
TanakaRobo | 0:a202e1bc38a1 | 47 | DigitalIn button(PC_13); |
TanakaRobo | 0:a202e1bc38a1 | 48 | Timer motertimer; |
TanakaRobo | 0:a202e1bc38a1 | 49 | |
TanakaRobo | 0:a202e1bc38a1 | 50 | PwmOut* Moter[Moter_NUM][2]; |
TanakaRobo | 0:a202e1bc38a1 | 51 | DigitalOut* Led[Moter_NUM]; |
TanakaRobo | 0:a202e1bc38a1 | 52 | RotaryInc* Speed[Moter_NUM]; |
TanakaRobo | 0:a202e1bc38a1 | 53 | RotaryInc* Place[Moter_NUM]; |
TanakaRobo | 0:a202e1bc38a1 | 54 | GY521 *gy; |
TanakaRobo | 0:a202e1bc38a1 | 55 | bool Ok = false; |
TanakaRobo | 0:a202e1bc38a1 | 56 | double X = 0,Y = 0,T = 0,Yaw = 0;//オドメトリ |
TanakaRobo | 1:7b0db5ea0ab7 | 57 | double Vx = 0,Vy = 0,Omega = 0;//速度 |
TanakaRobo | 1:7b0db5ea0ab7 | 58 | double driveS[Moter_NUM],nowV[Moter_NUM]; |
TanakaRobo | 0:a202e1bc38a1 | 59 | |
TanakaRobo | 0:a202e1bc38a1 | 60 | ros::NodeHandle nh; |
TanakaRobo | 0:a202e1bc38a1 | 61 | |
TanakaRobo | 1:7b0db5ea0ab7 | 62 | inline double constrain(double x,double a,double b){ |
TanakaRobo | 1:7b0db5ea0ab7 | 63 | return (x < a ? a : (x > b ? b : x)); |
TanakaRobo | 1:7b0db5ea0ab7 | 64 | } |
TanakaRobo | 1:7b0db5ea0ab7 | 65 | |
TanakaRobo | 0:a202e1bc38a1 | 66 | void safe(){ |
TanakaRobo | 0:a202e1bc38a1 | 67 | Vx = 0; |
TanakaRobo | 0:a202e1bc38a1 | 68 | Vy = 0; |
TanakaRobo | 0:a202e1bc38a1 | 69 | Omega = 0; |
TanakaRobo | 1:7b0db5ea0ab7 | 70 | automove = false; |
TanakaRobo | 0:a202e1bc38a1 | 71 | for(int i = 0;i < Moter_NUM;i++){ |
TanakaRobo | 0:a202e1bc38a1 | 72 | Moter[i][0]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 73 | Moter[i][1]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 74 | Led[i]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 75 | } |
TanakaRobo | 0:a202e1bc38a1 | 76 | } |
TanakaRobo | 0:a202e1bc38a1 | 77 | |
TanakaRobo | 0:a202e1bc38a1 | 78 | void Reset(){ |
TanakaRobo | 0:a202e1bc38a1 | 79 | if(!Ok){ |
TanakaRobo | 0:a202e1bc38a1 | 80 | Ok = true; |
TanakaRobo | 0:a202e1bc38a1 | 81 | }else{ |
TanakaRobo | 0:a202e1bc38a1 | 82 | led.write(0); |
TanakaRobo | 0:a202e1bc38a1 | 83 | safe(); |
TanakaRobo | 0:a202e1bc38a1 | 84 | gy->reset(0); |
TanakaRobo | 0:a202e1bc38a1 | 85 | X = 0; |
TanakaRobo | 0:a202e1bc38a1 | 86 | Y = 0; |
TanakaRobo | 0:a202e1bc38a1 | 87 | T = 0; |
TanakaRobo | 0:a202e1bc38a1 | 88 | led.write(1); |
TanakaRobo | 0:a202e1bc38a1 | 89 | } |
TanakaRobo | 0:a202e1bc38a1 | 90 | } |
TanakaRobo | 0:a202e1bc38a1 | 91 | |
TanakaRobo | 1:7b0db5ea0ab7 | 92 | inline bool Drive(int id,double pwm){//モーターを回す |
TanakaRobo | 1:7b0db5ea0ab7 | 93 | pwm = constrain(pwm,-MAXPWM,MAXPWM); |
TanakaRobo | 0:a202e1bc38a1 | 94 | if(!pwm){ |
TanakaRobo | 0:a202e1bc38a1 | 95 | Moter[id][0]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 96 | Moter[id][1]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 97 | Led[id]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 98 | }else if(0 < pwm){ |
TanakaRobo | 1:7b0db5ea0ab7 | 99 | Moter[id][0]->write(pwm); |
TanakaRobo | 0:a202e1bc38a1 | 100 | Moter[id][1]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 101 | Led[id]->write(1); |
TanakaRobo | 0:a202e1bc38a1 | 102 | }else{ |
TanakaRobo | 0:a202e1bc38a1 | 103 | Moter[id][0]->write(0); |
TanakaRobo | 1:7b0db5ea0ab7 | 104 | Moter[id][1]->write(-pwm); |
TanakaRobo | 0:a202e1bc38a1 | 105 | Led[id]->write(1); |
TanakaRobo | 0:a202e1bc38a1 | 106 | } |
TanakaRobo | 0:a202e1bc38a1 | 107 | return true; |
TanakaRobo | 0:a202e1bc38a1 | 108 | } |
TanakaRobo | 0:a202e1bc38a1 | 109 | |
TanakaRobo | 0:a202e1bc38a1 | 110 | inline void move(){ |
TanakaRobo | 1:7b0db5ea0ab7 | 111 | static double diff[Moter_NUM],errer[Moter_NUM],diffV[Moter_NUM],lastV[Moter_NUM],driveV[Moter_NUM],now_t,cos_yaw,sin_yaw; |
TanakaRobo | 0:a202e1bc38a1 | 112 | static int j; |
TanakaRobo | 0:a202e1bc38a1 | 113 | #if Moter_NUM == 3 |
TanakaRobo | 1:7b0db5ea0ab7 | 114 | driveV[0] = Vx*cos(Yaw) + Vy*sin(Yaw) + Omega*Moter_L; |
TanakaRobo | 1:7b0db5ea0ab7 | 115 | driveV[1] = Vx*cos(Yaw + PI2_3) + Vy*sin(Yaw + PI2_3) + Omega*Moter_L; |
TanakaRobo | 1:7b0db5ea0ab7 | 116 | driveV[2] = Vx*cos(Yaw - PI2_3) + Vy*sin(Yaw - PI2_3) + Omega*Moter_L; |
TanakaRobo | 0:a202e1bc38a1 | 117 | #elif Moter_NUM == 4 |
TanakaRobo | 1:7b0db5ea0ab7 | 118 | cos_yaw = cos(Yaw); |
TanakaRobo | 1:7b0db5ea0ab7 | 119 | sin_yaw = sin(Yaw); |
TanakaRobo | 1:7b0db5ea0ab7 | 120 | driveV[0] = Vx*cos_yaw + Vy*sin_yaw + Omega*Moter_L; |
TanakaRobo | 1:7b0db5ea0ab7 | 121 | driveV[1] = -Vx*sin_yaw + Vy*cos_yaw + Omega*Moter_L; |
TanakaRobo | 1:7b0db5ea0ab7 | 122 | driveV[2] = -Vx*cos_yaw - Vy*sin_yaw + Omega*Moter_L; |
TanakaRobo | 1:7b0db5ea0ab7 | 123 | driveV[3] = Vx*sin_yaw - Vy*cos_yaw + Omega*Moter_L; |
TanakaRobo | 0:a202e1bc38a1 | 124 | #endif |
TanakaRobo | 0:a202e1bc38a1 | 125 | now_t = motertimer.read(); |
TanakaRobo | 0:a202e1bc38a1 | 126 | motertimer.reset(); |
TanakaRobo | 0:a202e1bc38a1 | 127 | for(j = 0;j < Moter_NUM;j++){ |
TanakaRobo | 0:a202e1bc38a1 | 128 | nowV[j] = Speed[j]->getSpeed(); |
TanakaRobo | 1:7b0db5ea0ab7 | 129 | diff[j] = driveV[j] - nowV[j]; |
TanakaRobo | 1:7b0db5ea0ab7 | 130 | if(nowV[j] == 0 && driveV[j] == 0 && errer[j] != 0){ |
TanakaRobo | 0:a202e1bc38a1 | 131 | errer[j] = 0; |
TanakaRobo | 1:7b0db5ea0ab7 | 132 | }else{ |
TanakaRobo | 1:7b0db5ea0ab7 | 133 | errer[j] += diff[j] * now_t; |
TanakaRobo | 0:a202e1bc38a1 | 134 | } |
TanakaRobo | 0:a202e1bc38a1 | 135 | diffV[j] = (nowV[j] - lastV[j]) / now_t; |
TanakaRobo | 0:a202e1bc38a1 | 136 | lastV[j] = nowV[j]; |
TanakaRobo | 1:7b0db5ea0ab7 | 137 | driveS[j] = 0.0003 * driveV[j] + diff[j] * kp + errer[j] * ki - diffV[j] * kd; |
TanakaRobo | 1:7b0db5ea0ab7 | 138 | Drive(j,driveS[j]); |
TanakaRobo | 0:a202e1bc38a1 | 139 | } |
TanakaRobo | 0:a202e1bc38a1 | 140 | } |
TanakaRobo | 0:a202e1bc38a1 | 141 | |
TanakaRobo | 0:a202e1bc38a1 | 142 | void getData(const std_msgs::Float32MultiArray &msgs){ |
TanakaRobo | 1:7b0db5ea0ab7 | 143 | if(!Ok && (int)msgs.data[0] != -1)return; |
TanakaRobo | 0:a202e1bc38a1 | 144 | switch((int)msgs.data[0]){ |
TanakaRobo | 0:a202e1bc38a1 | 145 | case -1: |
TanakaRobo | 1:7b0db5ea0ab7 | 146 | ablemove = true; |
TanakaRobo | 0:a202e1bc38a1 | 147 | Reset(); |
TanakaRobo | 0:a202e1bc38a1 | 148 | break; |
TanakaRobo | 0:a202e1bc38a1 | 149 | case 0: |
TanakaRobo | 1:7b0db5ea0ab7 | 150 | ablemove = true; |
TanakaRobo | 0:a202e1bc38a1 | 151 | Vx = msgs.data[1]; |
TanakaRobo | 0:a202e1bc38a1 | 152 | Vy = msgs.data[2]; |
TanakaRobo | 0:a202e1bc38a1 | 153 | Omega = msgs.data[3]; |
TanakaRobo | 0:a202e1bc38a1 | 154 | break; |
TanakaRobo | 0:a202e1bc38a1 | 155 | case 1: |
TanakaRobo | 1:7b0db5ea0ab7 | 156 | ablemove = true; |
TanakaRobo | 1:7b0db5ea0ab7 | 157 | goal_x = msgs.data[1]; |
TanakaRobo | 1:7b0db5ea0ab7 | 158 | goal_y = msgs.data[2]; |
TanakaRobo | 1:7b0db5ea0ab7 | 159 | goal_yaw = msgs.data[3]; |
TanakaRobo | 1:7b0db5ea0ab7 | 160 | automove = true; |
TanakaRobo | 1:7b0db5ea0ab7 | 161 | errer_x = 0; |
TanakaRobo | 1:7b0db5ea0ab7 | 162 | errer_y = 0; |
TanakaRobo | 1:7b0db5ea0ab7 | 163 | errer_omega = 0; |
TanakaRobo | 1:7b0db5ea0ab7 | 164 | autotimer.reset(); |
TanakaRobo | 1:7b0db5ea0ab7 | 165 | autotimer.start(); |
TanakaRobo | 1:7b0db5ea0ab7 | 166 | break; |
TanakaRobo | 1:7b0db5ea0ab7 | 167 | case 2: |
TanakaRobo | 1:7b0db5ea0ab7 | 168 | ablemove = true; |
TanakaRobo | 0:a202e1bc38a1 | 169 | safe(); |
TanakaRobo | 0:a202e1bc38a1 | 170 | break; |
TanakaRobo | 1:7b0db5ea0ab7 | 171 | case 3: |
TanakaRobo | 1:7b0db5ea0ab7 | 172 | ablemove = false; |
TanakaRobo | 1:7b0db5ea0ab7 | 173 | Drive(0,msgs.data[1]/255); |
TanakaRobo | 1:7b0db5ea0ab7 | 174 | Drive(1,msgs.data[2]/255); |
TanakaRobo | 1:7b0db5ea0ab7 | 175 | break; |
TanakaRobo | 1:7b0db5ea0ab7 | 176 | case 4: |
TanakaRobo | 1:7b0db5ea0ab7 | 177 | ablemove = false; |
TanakaRobo | 1:7b0db5ea0ab7 | 178 | Drive(2,msgs.data[1]/255); |
TanakaRobo | 1:7b0db5ea0ab7 | 179 | Drive(3,msgs.data[2]/255); |
TanakaRobo | 1:7b0db5ea0ab7 | 180 | break; |
TanakaRobo | 0:a202e1bc38a1 | 181 | } |
TanakaRobo | 0:a202e1bc38a1 | 182 | } |
TanakaRobo | 0:a202e1bc38a1 | 183 | |
TanakaRobo | 0:a202e1bc38a1 | 184 | std_msgs::Float32MultiArray now; |
TanakaRobo | 0:a202e1bc38a1 | 185 | ros::Publisher place("place",&now); |
TanakaRobo | 0:a202e1bc38a1 | 186 | ros::Subscriber<std_msgs::Float32MultiArray> sub("moter",&getData); |
TanakaRobo | 0:a202e1bc38a1 | 187 | |
TanakaRobo | 0:a202e1bc38a1 | 188 | int main(){ |
TanakaRobo | 0:a202e1bc38a1 | 189 | nh.getHardware()->setBaud(115200); |
TanakaRobo | 0:a202e1bc38a1 | 190 | nh.initNode(); |
TanakaRobo | 0:a202e1bc38a1 | 191 | nh.advertise(place); |
TanakaRobo | 0:a202e1bc38a1 | 192 | nh.subscribe(sub); |
TanakaRobo | 0:a202e1bc38a1 | 193 | now.data_length = 7; |
TanakaRobo | 0:a202e1bc38a1 | 194 | now.data = (float*)malloc(sizeof(float)*now.data_length); |
TanakaRobo | 0:a202e1bc38a1 | 195 | int i; |
TanakaRobo | 0:a202e1bc38a1 | 196 | bool flag = false; |
TanakaRobo | 0:a202e1bc38a1 | 197 | double diff[Moter_NUM],Pspeed[Moter_NUM]; |
TanakaRobo | 0:a202e1bc38a1 | 198 | double nowVx,nowVy,nowVt; |
TanakaRobo | 0:a202e1bc38a1 | 199 | double cos_yaw_2,sin_yaw_2; |
TanakaRobo | 1:7b0db5ea0ab7 | 200 | double diff_x,diff_y,diff_yaw; |
TanakaRobo | 1:7b0db5ea0ab7 | 201 | double now_t,use_amax; |
TanakaRobo | 0:a202e1bc38a1 | 202 | for(i = 0;i < Moter_NUM;i++){ |
TanakaRobo | 0:a202e1bc38a1 | 203 | Led[i] = new DigitalOut(pwm_pin[i][2]); |
TanakaRobo | 0:a202e1bc38a1 | 204 | Moter[i][0] = new PwmOut(pwm_pin[i][0]); |
TanakaRobo | 0:a202e1bc38a1 | 205 | Moter[i][1] = new PwmOut(pwm_pin[i][1]); |
TanakaRobo | 0:a202e1bc38a1 | 206 | Moter[i][0]->period_us(PERIOD); |
TanakaRobo | 0:a202e1bc38a1 | 207 | Moter[i][1]->period_us(PERIOD); |
TanakaRobo | 0:a202e1bc38a1 | 208 | Speed[i] = new RotaryInc(rotary_pin[i][0],rotary_pin[i][1],2*50.8*M_PI,256,2); |
TanakaRobo | 1:7b0db5ea0ab7 | 209 | Place[i] = new RotaryInc(rotary_pin[i+Moter_NUM][0],rotary_pin[i+Moter_NUM][1],2*25.4*M_PI,256,2); |
TanakaRobo | 0:a202e1bc38a1 | 210 | } |
TanakaRobo | 1:7b0db5ea0ab7 | 211 | I2C i2c(PB_3,PB_10); |
TanakaRobo | 0:a202e1bc38a1 | 212 | Timer loop; |
TanakaRobo | 0:a202e1bc38a1 | 213 | loop.start(); |
TanakaRobo | 1:7b0db5ea0ab7 | 214 | while(button && !Ok){ |
TanakaRobo | 0:a202e1bc38a1 | 215 | nh.spinOnce(); |
TanakaRobo | 0:a202e1bc38a1 | 216 | if(loop.read_ms() > 250){ |
TanakaRobo | 0:a202e1bc38a1 | 217 | led = !led; |
TanakaRobo | 0:a202e1bc38a1 | 218 | loop.reset(); |
TanakaRobo | 0:a202e1bc38a1 | 219 | } |
TanakaRobo | 0:a202e1bc38a1 | 220 | } |
TanakaRobo | 1:7b0db5ea0ab7 | 221 | led.write(0); |
TanakaRobo | 1:7b0db5ea0ab7 | 222 | Ok = true; |
TanakaRobo | 0:a202e1bc38a1 | 223 | GY521 gyro(i2c); |
TanakaRobo | 0:a202e1bc38a1 | 224 | gy = &gyro; |
TanakaRobo | 1:7b0db5ea0ab7 | 225 | motertimer.start(); |
TanakaRobo | 1:7b0db5ea0ab7 | 226 | led.write(1); |
TanakaRobo | 0:a202e1bc38a1 | 227 | while(true){ |
TanakaRobo | 1:7b0db5ea0ab7 | 228 | if(ablemove){ |
TanakaRobo | 1:7b0db5ea0ab7 | 229 | move(); |
TanakaRobo | 1:7b0db5ea0ab7 | 230 | }else{ |
TanakaRobo | 1:7b0db5ea0ab7 | 231 | for(i = 0;i<4;i++){ |
TanakaRobo | 1:7b0db5ea0ab7 | 232 | nowV[i] = Speed[i]->getSpeed(); |
TanakaRobo | 1:7b0db5ea0ab7 | 233 | } |
TanakaRobo | 1:7b0db5ea0ab7 | 234 | } |
TanakaRobo | 0:a202e1bc38a1 | 235 | nh.spinOnce(); |
TanakaRobo | 0:a202e1bc38a1 | 236 | gyro.updata(); |
TanakaRobo | 0:a202e1bc38a1 | 237 | Yaw = gyro.yaw; |
TanakaRobo | 0:a202e1bc38a1 | 238 | if(loop.read_ms() > 20){//50msごとに通信する |
TanakaRobo | 0:a202e1bc38a1 | 239 | //データ送信 |
TanakaRobo | 0:a202e1bc38a1 | 240 | now.data[0] = X; |
TanakaRobo | 0:a202e1bc38a1 | 241 | now.data[1] = Y; |
TanakaRobo | 0:a202e1bc38a1 | 242 | now.data[2] = T; |
TanakaRobo | 0:a202e1bc38a1 | 243 | now.data[3] = Yaw; |
TanakaRobo | 0:a202e1bc38a1 | 244 | now.data[4] = nowVx; |
TanakaRobo | 0:a202e1bc38a1 | 245 | now.data[5] = nowVy; |
TanakaRobo | 0:a202e1bc38a1 | 246 | now.data[6] = nowVt; |
TanakaRobo | 0:a202e1bc38a1 | 247 | place.publish(&now); |
TanakaRobo | 0:a202e1bc38a1 | 248 | loop.reset(); |
TanakaRobo | 0:a202e1bc38a1 | 249 | } |
TanakaRobo | 0:a202e1bc38a1 | 250 | Yaw *= 0.01745329251994329576923690768489;//PI/180 |
TanakaRobo | 0:a202e1bc38a1 | 251 | for(i = 0;i<Moter_NUM;++i){ |
TanakaRobo | 1:7b0db5ea0ab7 | 252 | diff[i] = Place[i]->diff();//Place |
TanakaRobo | 1:7b0db5ea0ab7 | 253 | Pspeed[i] = Speed[i]->getSpeed();//Place |
TanakaRobo | 0:a202e1bc38a1 | 254 | } |
TanakaRobo | 0:a202e1bc38a1 | 255 | //オドメトリ計算 |
TanakaRobo | 0:a202e1bc38a1 | 256 | #if Moter_NUM == 3 |
TanakaRobo | 0:a202e1bc38a1 | 257 | 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); |
TanakaRobo | 0:a202e1bc38a1 | 258 | 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); |
TanakaRobo | 0:a202e1bc38a1 | 259 | T += diff[0]/Rotary_L/3 + diff[1]/Rotary_L/3 + diff[2]/Rotary_L/3; |
TanakaRobo | 0:a202e1bc38a1 | 260 | 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); |
TanakaRobo | 0:a202e1bc38a1 | 261 | 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); |
TanakaRobo | 0:a202e1bc38a1 | 262 | nowVt = Pspeed[0]/Rotary_L/3 + Pspeed[1]/Rotary_L/3 + Pspeed[2]/Rotary_L/3; |
TanakaRobo | 0:a202e1bc38a1 | 263 | #elif Moter_NUM == 4 |
TanakaRobo | 0:a202e1bc38a1 | 264 | cos_yaw_2 = cos(Yaw)/2.0; |
TanakaRobo | 0:a202e1bc38a1 | 265 | sin_yaw_2 = sin(Yaw)/2.0; |
TanakaRobo | 0:a202e1bc38a1 | 266 | X += diff[0]*cos_yaw_2 - diff[1]*sin_yaw_2 - diff[2]*cos_yaw_2 + diff[3]*sin_yaw_2; |
TanakaRobo | 0:a202e1bc38a1 | 267 | Y += diff[0]*sin_yaw_2 + diff[1]*cos_yaw_2 - diff[2]*sin_yaw_2 - diff[3]*cos_yaw_2; |
TanakaRobo | 0:a202e1bc38a1 | 268 | T += diff[0]/Rotary_L/4 + diff[1]/Rotary_L/4 + diff[2]/Rotary_L/4 + diff[3]/Rotary_L/4; |
TanakaRobo | 1:7b0db5ea0ab7 | 269 | cos_yaw_2 = cos(Yaw+PI_4)/2.0; |
TanakaRobo | 1:7b0db5ea0ab7 | 270 | sin_yaw_2 = sin(Yaw+PI_4)/2.0; |
TanakaRobo | 0:a202e1bc38a1 | 271 | nowVx = Pspeed[0]*cos_yaw_2 - Pspeed[1]*sin_yaw_2 - Pspeed[2]*cos_yaw_2 + Pspeed[3]*sin_yaw_2; |
TanakaRobo | 0:a202e1bc38a1 | 272 | nowVy = Pspeed[0]*sin_yaw_2 + Pspeed[1]*cos_yaw_2 - Pspeed[2]*sin_yaw_2 - Pspeed[3]*cos_yaw_2; |
TanakaRobo | 0:a202e1bc38a1 | 273 | nowVt = Pspeed[0]/Rotary_L/4 + Pspeed[1]/Rotary_L/4 + Pspeed[2]/Rotary_L/4 + Pspeed[3]/Rotary_L/4; |
TanakaRobo | 0:a202e1bc38a1 | 274 | #endif |
TanakaRobo | 1:7b0db5ea0ab7 | 275 | if(!button && !flag){ |
TanakaRobo | 0:a202e1bc38a1 | 276 | flag = true; |
TanakaRobo | 0:a202e1bc38a1 | 277 | Reset(); |
TanakaRobo | 1:7b0db5ea0ab7 | 278 | }else if(button && flag){ |
TanakaRobo | 0:a202e1bc38a1 | 279 | flag = false; |
TanakaRobo | 0:a202e1bc38a1 | 280 | } |
TanakaRobo | 0:a202e1bc38a1 | 281 | } |
TanakaRobo | 0:a202e1bc38a1 | 282 | } |