STM32足回りプログラム

Dependencies:   mbed ros_lib_kinetic

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?

UserRevisionLine numberNew 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 }