go-roboB / aa2_another_right_re

o

Dependencies:   arrc_mbed

main.cpp@0:a3056628cb39, 2022-04-08 (annotated)

Committer:
darkumatar
Date:
Fri Apr 08 09:30:33 2022 +0000
Revision:
0:a3056628cb39
i

Who changed what in which revision?

UserRevisionLine numberNew contents of line
darkumatar 0:a3056628cb39 1 #include "mbed.h"
darkumatar 0:a3056628cb39 2 #include"scrp_slave.hpp"
darkumatar 0:a3056628cb39 3 #include "rotary_inc.hpp"
darkumatar 0:a3056628cb39 4 #include"cmath"
darkumatar 0:a3056628cb39 5 #include"gy521.hpp"
darkumatar 0:a3056628cb39 6 #include "neopixel.h"
darkumatar 0:a3056628cb39 7
darkumatar 0:a3056628cb39 8
darkumatar 0:a3056628cb39 9 #define goal_x -17700.0
darkumatar 0:a3056628cb39 10 #define goal_y 28800.0//23800.0
darkumatar 0:a3056628cb39 11
darkumatar 0:a3056628cb39 12
darkumatar 0:a3056628cb39 13 #define goal_x2 -8000000
darkumatar 0:a3056628cb39 14 #define goal_y2 10000000
darkumatar 0:a3056628cb39 15 #define period_r 8000.0
darkumatar 0:a3056628cb39 16 NeoPixelOut npx(PB_4,7);
darkumatar 0:a3056628cb39 17 I2C i2d(PB_3,PB_10);
darkumatar 0:a3056628cb39 18 GY521 gyro(i2d);
darkumatar 0:a3056628cb39 19 ScrpSlave slave(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);//srcslave設定
darkumatar 0:a3056628cb39 20 RotaryInc v[4]{RotaryInc(PC_5,PA_12,1,256,4),RotaryInc(PA_9,PA_8,1,256,4),RotaryInc(PC_3,PC_2,1,256,4),RotaryInc(PC_11,PC_10,1,256,4)};//ロリコンピン
darkumatar 0:a3056628cb39 21 RotaryInc rote[4]{RotaryInc(PA_15,PA_14,1,256,4),RotaryInc(PA_7,PA_6,1,256,4),RotaryInc(PC_1,PC_0,1,256,4),RotaryInc(PA_13,PC_4,1,256,4)};//読み取り用ピン
darkumatar 0:a3056628cb39 22 int target[4];//target変数　この変数の値を目指すように動く
darkumatar 0:a3056628cb39 23 int angler=90;
darkumatar 0:a3056628cb39 24 int speedr=1024;
darkumatar 0:a3056628cb39 25 bool auto_swich = false;
darkumatar 0:a3056628cb39 26 bool limit_switch = true;
darkumatar 0:a3056628cb39 27 int step=0;
darkumatar 0:a3056628cb39 28 int auto_mode=0;
darkumatar 0:a3056628cb39 29
darkumatar 0:a3056628cb39 30 int direct_xx=0;
darkumatar 0:a3056628cb39 31 int direct_yy=0;
darkumatar 0:a3056628cb39 32 int direct_turn=0;
darkumatar 0:a3056628cb39 33
darkumatar 0:a3056628cb39 34 DigitalOut Led1(PB_2);
darkumatar 0:a3056628cb39 35 DigitalOut Led2(PC_6);
darkumatar 0:a3056628cb39 36 DigitalOut Led3(PB_15);
darkumatar 0:a3056628cb39 37 DigitalOut Led4(PA_10);
darkumatar 0:a3056628cb39 38 int check_tepu=0;
darkumatar 0:a3056628cb39 39
darkumatar 0:a3056628cb39 40 bool limi=true;
darkumatar 0:a3056628cb39 41 bool liset_abc=false;
darkumatar 0:a3056628cb39 42
darkumatar 0:a3056628cb39 43
darkumatar 0:a3056628cb39 44 DigitalIn limitB(PB_12,PullUp);
darkumatar 0:a3056628cb39 45
darkumatar 0:a3056628cb39 46
darkumatar 0:a3056628cb39 47
darkumatar 0:a3056628cb39 48 bool add(int id,int ppp){//速度を変えるやつ
darkumatar 0:a3056628cb39 49 Led4.write(0);
darkumatar 0:a3056628cb39 50 if(auto_swich==false&&limi==true){
darkumatar 0:a3056628cb39 51 target[id]=30*ppp;
darkumatar 0:a3056628cb39 52 }
darkumatar 0:a3056628cb39 53 return true;
darkumatar 0:a3056628cb39 54 }
darkumatar 0:a3056628cb39 55
darkumatar 0:a3056628cb39 56 bool anglez(int id,int ppp){//おまけで作ったやつ　アングルを決めてその方向へ走る
darkumatar 0:a3056628cb39 57 angler=ppp;
darkumatar 0:a3056628cb39 58 for(int i=0;i<4;i++){
darkumatar 0:a3056628cb39 59 //target[i]=(sin(M_PI/180.0*(angler+90*i))+cos(M_PI/180.0*(angler+90*i)))*speedr;
darkumatar 0:a3056628cb39 60
darkumatar 0:a3056628cb39 61 }
darkumatar 0:a3056628cb39 62 return true;
darkumatar 0:a3056628cb39 63 }
darkumatar 0:a3056628cb39 64
darkumatar 0:a3056628cb39 65 bool speedz(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
darkumatar 0:a3056628cb39 66 speedr=ppp;
darkumatar 0:a3056628cb39 67 for(int i=0;i<4;i++){
darkumatar 0:a3056628cb39 68 target[i]=(sin(M_PI/180.0*(angler+90*i))+cos(M_PI/180.0*(angler+90*i)))*speedr;
darkumatar 0:a3056628cb39 69
darkumatar 0:a3056628cb39 70 }
darkumatar 0:a3056628cb39 71 return true;
darkumatar 0:a3056628cb39 72 }
darkumatar 0:a3056628cb39 73
darkumatar 0:a3056628cb39 74 bool auto_on(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
darkumatar 0:a3056628cb39 75 if(ppp==1){
darkumatar 0:a3056628cb39 76 auto_swich=true;
darkumatar 0:a3056628cb39 77 step=0;
darkumatar 0:a3056628cb39 78 }
darkumatar 0:a3056628cb39 79 return true;
darkumatar 0:a3056628cb39 80 }
darkumatar 0:a3056628cb39 81
darkumatar 0:a3056628cb39 82 bool auto_off(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
darkumatar 0:a3056628cb39 83 if(ppp==1){
darkumatar 0:a3056628cb39 84 auto_swich=false;
darkumatar 0:a3056628cb39 85 step=0;
darkumatar 0:a3056628cb39 86 }
darkumatar 0:a3056628cb39 87 return true;
darkumatar 0:a3056628cb39 88 }
darkumatar 0:a3056628cb39 89
darkumatar 0:a3056628cb39 90 bool turn(int id,int ppp){//回転
darkumatar 0:a3056628cb39 91 for(int i=0;i<4;i++){
darkumatar 0:a3056628cb39 92 target[i]=ppp;
darkumatar 0:a3056628cb39 93 }
darkumatar 0:a3056628cb39 94 return true;
darkumatar 0:a3056628cb39 95 }
darkumatar 0:a3056628cb39 96
darkumatar 0:a3056628cb39 97 bool pro(int id,int ppp){ //アングルとスピードを同時に決める　下から3桁で角度を決める(0~360)　それ以外でスピードを決める　これなに？私もわからん
darkumatar 0:a3056628cb39 98 int angle=ppp%1000;
darkumatar 0:a3056628cb39 99 int speed=ppp/1000;
darkumatar 0:a3056628cb39 100 for(int i=0;i<4;i++){
darkumatar 0:a3056628cb39 101 target[i]=(sin(M_PI/180.0*(angle+90*i))+cos(M_PI/180.0*(angle+90*i)))*speed*100.0;
darkumatar 0:a3056628cb39 102
darkumatar 0:a3056628cb39 103 }
darkumatar 0:a3056628cb39 104 return true;
darkumatar 0:a3056628cb39 105 }
darkumatar 0:a3056628cb39 106 bool pro2(){ //アングルとスピードを同時に決める　下から3桁で角度を決める(0~360)　それ以外でスピードを決める　これなに？私もわからん
darkumatar 0:a3056628cb39 107 double anglert=-135.000000000;
darkumatar 0:a3056628cb39 108 Led4.write(1);
darkumatar 0:a3056628cb39 109 for(int i=0;i<4;i++){
darkumatar 0:a3056628cb39 110 target[i]=(direct_yy*sin(M_PI/180.0*(90*i - gyro.yaw - anglert))-direct_xx*cos(M_PI/180.0*(90*i - gyro.yaw - anglert))+direct_turn)*30.0;
darkumatar 0:a3056628cb39 111
darkumatar 0:a3056628cb39 112 }
darkumatar 0:a3056628cb39 113 return true;
darkumatar 0:a3056628cb39 114 }
darkumatar 0:a3056628cb39 115 bool pro3(double goal_angle_ppp,int goal_x_speed,int goal_y_speed){
darkumatar 0:a3056628cb39 116 double anglert=-135.000000000;
darkumatar 0:a3056628cb39 117 for(int i=0;i<4;i++){
darkumatar 0:a3056628cb39 118 target[i]=(goal_y_speed*sin(M_PI/180.0*(90*i - gyro.yaw - anglert))-goal_x_speed*cos(M_PI/180.0*(90*i - gyro.yaw - anglert))+goal_angle_ppp)*30.0;
darkumatar 0:a3056628cb39 119
darkumatar 0:a3056628cb39 120 }
darkumatar 0:a3056628cb39 121 return true;
darkumatar 0:a3056628cb39 122 }
darkumatar 0:a3056628cb39 123 int test_limit(int test_limilimi){
darkumatar 0:a3056628cb39 124 test_limilimi=test_limilimi/100;
darkumatar 0:a3056628cb39 125 if(test_limilimi>64){
darkumatar 0:a3056628cb39 126 test_limilimi=64;
darkumatar 0:a3056628cb39 127 }
darkumatar 0:a3056628cb39 128 if(test_limilimi<-64){
darkumatar 0:a3056628cb39 129 test_limilimi=-64;
darkumatar 0:a3056628cb39 130 }
darkumatar 0:a3056628cb39 131 return test_limilimi;
darkumatar 0:a3056628cb39 132 }
darkumatar 0:a3056628cb39 133
darkumatar 0:a3056628cb39 134 double test_ang(double test_limilimi){
darkumatar 0:a3056628cb39 135 test_limilimi=test_limilimi*2;
darkumatar 0:a3056628cb39 136 if(test_limilimi>40){
darkumatar 0:a3056628cb39 137 test_limilimi=40;
darkumatar 0:a3056628cb39 138 }
darkumatar 0:a3056628cb39 139 if(test_limilimi<-40){
darkumatar 0:a3056628cb39 140 test_limilimi=-40;
darkumatar 0:a3056628cb39 141 }
darkumatar 0:a3056628cb39 142 return test_limilimi;
darkumatar 0:a3056628cb39 143 }
darkumatar 0:a3056628cb39 144
darkumatar 0:a3056628cb39 145 bool limit2(int id,int ppp){
darkumatar 0:a3056628cb39 146 if(ppp==1){
darkumatar 0:a3056628cb39 147 limit_switch=false;
darkumatar 0:a3056628cb39 148 }
darkumatar 0:a3056628cb39 149 return true;
darkumatar 0:a3056628cb39 150 }
darkumatar 0:a3056628cb39 151
darkumatar 0:a3056628cb39 152 bool direct_x(int id,int ppp){
darkumatar 0:a3056628cb39 153 direct_xx=ppp;
darkumatar 0:a3056628cb39 154 pro2();
darkumatar 0:a3056628cb39 155 return true;
darkumatar 0:a3056628cb39 156 }
darkumatar 0:a3056628cb39 157 bool direct_y(int id,int ppp){
darkumatar 0:a3056628cb39 158 direct_yy=ppp;
darkumatar 0:a3056628cb39 159 pro2();
darkumatar 0:a3056628cb39 160 return true;
darkumatar 0:a3056628cb39 161 }
darkumatar 0:a3056628cb39 162
darkumatar 0:a3056628cb39 163 bool direct_tu(int id,int ppp){
darkumatar 0:a3056628cb39 164 direct_turn=ppp;
darkumatar 0:a3056628cb39 165 pro2();
darkumatar 0:a3056628cb39 166 return true;
darkumatar 0:a3056628cb39 167 }
darkumatar 0:a3056628cb39 168
darkumatar 0:a3056628cb39 169 bool limit(int a,int &b){
darkumatar 0:a3056628cb39 170 return limit2(0,a);
darkumatar 0:a3056628cb39 171 }
darkumatar 0:a3056628cb39 172 bool abc1(int a,int &b){
darkumatar 0:a3056628cb39 173 check_tepu+=1;
darkumatar 0:a3056628cb39 174 return add(0,a);
darkumatar 0:a3056628cb39 175 }
darkumatar 0:a3056628cb39 176 bool abc2(int a,int &b){
darkumatar 0:a3056628cb39 177 check_tepu+=1;
darkumatar 0:a3056628cb39 178 return add(1,a);
darkumatar 0:a3056628cb39 179 }
darkumatar 0:a3056628cb39 180 bool abc3(int a,int &b){
darkumatar 0:a3056628cb39 181 check_tepu+=1;
darkumatar 0:a3056628cb39 182 return add(2,a);
darkumatar 0:a3056628cb39 183 }
darkumatar 0:a3056628cb39 184 bool abc4(int a,int &b){
darkumatar 0:a3056628cb39 185 check_tepu+=1;
darkumatar 0:a3056628cb39 186 return add(3,a);
darkumatar 0:a3056628cb39 187 }
darkumatar 0:a3056628cb39 188
darkumatar 0:a3056628cb39 189 bool abc5(int a,int &b){
darkumatar 0:a3056628cb39 190 return turn(0,a);
darkumatar 0:a3056628cb39 191 }
darkumatar 0:a3056628cb39 192 /*
darkumatar 0:a3056628cb39 193 bool abc6(int a,int &b){
darkumatar 0:a3056628cb39 194 return pro(0,a);
darkumatar 0:a3056628cb39 195 }
darkumatar 0:a3056628cb39 196 bool abc7(int a,int &b){
darkumatar 0:a3056628cb39 197 return anglez(0,a);
darkumatar 0:a3056628cb39 198 }
darkumatar 0:a3056628cb39 199 bool abc8(int a,int &b){
darkumatar 0:a3056628cb39 200 return speedz(0,a);
darkumatar 0:a3056628cb39 201 }
darkumatar 0:a3056628cb39 202 */
darkumatar 0:a3056628cb39 203 bool abc9(int a,int &b){
darkumatar 0:a3056628cb39 204 return auto_on(0,a);
darkumatar 0:a3056628cb39 205 }
darkumatar 0:a3056628cb39 206 bool abc10(int a,int &b){
darkumatar 0:a3056628cb39 207 return auto_off(0,a);
darkumatar 0:a3056628cb39 208 }
darkumatar 0:a3056628cb39 209 bool abc11(int a,int &b){
darkumatar 0:a3056628cb39 210 check_tepu+=1;
darkumatar 0:a3056628cb39 211 return direct_x(0,a);
darkumatar 0:a3056628cb39 212 }
darkumatar 0:a3056628cb39 213 bool abc12(int a,int &b){
darkumatar 0:a3056628cb39 214 check_tepu+=1;
darkumatar 0:a3056628cb39 215 return direct_y(0,a);
darkumatar 0:a3056628cb39 216 }
darkumatar 0:a3056628cb39 217 bool abc13(int a,int &b){
darkumatar 0:a3056628cb39 218 check_tepu+=1;
darkumatar 0:a3056628cb39 219 return direct_tu(0,a);
darkumatar 0:a3056628cb39 220 }
darkumatar 0:a3056628cb39 221 bool abc14(int a,int &b){
darkumatar 0:a3056628cb39 222 if(a==1){
darkumatar 0:a3056628cb39 223 liset_abc=true;
darkumatar 0:a3056628cb39 224 }else{
darkumatar 0:a3056628cb39 225 liset_abc=false;
darkumatar 0:a3056628cb39 226 }
darkumatar 0:a3056628cb39 227 return true;
darkumatar 0:a3056628cb39 228 }
darkumatar 0:a3056628cb39 229
darkumatar 0:a3056628cb39 230
darkumatar 0:a3056628cb39 231 int main() {
darkumatar 0:a3056628cb39 232 Led1.write(1);
darkumatar 0:a3056628cb39 233 Led2.write(0);
darkumatar 0:a3056628cb39 234 slave.addCMD(1,limit);
darkumatar 0:a3056628cb39 235 slave.addCMD(2,abc1);
darkumatar 0:a3056628cb39 236 slave.addCMD(3,abc2);
darkumatar 0:a3056628cb39 237 slave.addCMD(4,abc3);
darkumatar 0:a3056628cb39 238 slave.addCMD(5,abc4);
darkumatar 0:a3056628cb39 239 slave.addCMD(12,abc9);
darkumatar 0:a3056628cb39 240 slave.addCMD(13,abc10);
darkumatar 0:a3056628cb39 241 slave.addCMD(31,abc11);
darkumatar 0:a3056628cb39 242 slave.addCMD(32,abc12);
darkumatar 0:a3056628cb39 243 slave.addCMD(33,abc13);
darkumatar 0:a3056628cb39 244 slave.addCMD(34,abc14);
darkumatar 0:a3056628cb39 245 int i;
darkumatar 0:a3056628cb39 246 double x_period=0;
darkumatar 0:a3056628cb39 247 double y_period=0;
darkumatar 0:a3056628cb39 248 int after[4],before[4],before_parus[4],speed_pwm[4],after_pwm[4],before_pwm[4];
darkumatar 0:a3056628cb39 249 double ca[4],P[4],I[4],D[4],integral[4];
darkumatar 0:a3056628cb39 250 PwmOut motor_p[4]{PwmOut(PB_1),PwmOut(PB_13),PwmOut(PC_9),PwmOut(PB_7)};//モーター宣言 PB_6 PC_8
darkumatar 0:a3056628cb39 251 PwmOut motor_m[4]{PwmOut(PA_11),PwmOut(PB_14),PwmOut(PC_8),PwmOut(PB_6)};//モーター宣言 PB_7 PC_9
darkumatar 0:a3056628cb39 252 for(i=0;i<4;i++){
darkumatar 0:a3056628cb39 253 motor_p[i].period_us(2048);
darkumatar 0:a3056628cb39 254 motor_m[i].period_us(2048);
darkumatar 0:a3056628cb39 255 target[i]=0;
darkumatar 0:a3056628cb39 256 after[i]=0;
darkumatar 0:a3056628cb39 257 before[i]=0;
darkumatar 0:a3056628cb39 258 ca[i]=0;
darkumatar 0:a3056628cb39 259 P[i]=0;
darkumatar 0:a3056628cb39 260 I[i]=0;
darkumatar 0:a3056628cb39 261 D[i]=0;
darkumatar 0:a3056628cb39 262 before_parus[i]=0;
darkumatar 0:a3056628cb39 263 integral[i]=0;
darkumatar 0:a3056628cb39 264 speed_pwm[i]=0;
darkumatar 0:a3056628cb39 265 after_pwm[i]=0;
darkumatar 0:a3056628cb39 266 before_pwm[i]=0;
darkumatar 0:a3056628cb39 267 }
darkumatar 0:a3056628cb39 268 Timer name;
darkumatar 0:a3056628cb39 269 Timer limilimi;
darkumatar 0:a3056628cb39 270 limilimi.start();
darkumatar 0:a3056628cb39 271 name.start();
darkumatar 0:a3056628cb39 272 gyro.start();
darkumatar 0:a3056628cb39 273 double angle=0;
darkumatar 0:a3056628cb39 274 double goal_angle1=0;
darkumatar 0:a3056628cb39 275 double test_speed_auto=0;
darkumatar 0:a3056628cb39 276 Led1.write(1);
darkumatar 0:a3056628cb39 277 bool limilimi_swich=false;
darkumatar 0:a3056628cb39 278 while(limit_switch==true) {
darkumatar 0:a3056628cb39 279 angle=gyro.yaw-90.00000;
darkumatar 0:a3056628cb39 280 if(auto_swich==true){
darkumatar 0:a3056628cb39 281
darkumatar 0:a3056628cb39 282
darkumatar 0:a3056628cb39 283
darkumatar 0:a3056628cb39 284 /*
darkumatar 0:a3056628cb39 285
darkumatar 0:a3056628cb39 286 goal_angle1=-1.0*atan((goal_x-x_period)/(goal_y-y_period))/M_PI*180.0;
darkumatar 0:a3056628cb39 287 if(goal_angle1>0){
darkumatar 0:a3056628cb39 288 goal_angle1-=180.0;
darkumatar 0:a3056628cb39 289 }else{
darkumatar 0:a3056628cb39 290 goal_angle1+=180.0;
darkumatar 0:a3056628cb39 291 }
darkumatar 0:a3056628cb39 292 if(step==0&&((goal_angle1-gyro.yaw)>1||(goal_angle1-gyro.yaw)<-1)){
darkumatar 0:a3056628cb39 293 test_speed_auto=(goal_angle1-gyro.yaw)*-20.0;
darkumatar 0:a3056628cb39 294 if(test_speed_auto>2000){
darkumatar 0:a3056628cb39 295 test_speed_auto=2000;
darkumatar 0:a3056628cb39 296 }else if(test_speed_auto<-2000){
darkumatar 0:a3056628cb39 297 test_speed_auto=-2000;
darkumatar 0:a3056628cb39 298 }
darkumatar 0:a3056628cb39 299 turn(0,(test_speed_auto)*1);
darkumatar 0:a3056628cb39 300 }else if(step>=0&&step<3){
darkumatar 0:a3056628cb39 301 step+=1;
darkumatar 0:a3056628cb39 302 }else if(step==3&&((sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)>1000)|| (sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)<-1000))){
darkumatar 0:a3056628cb39 303 test_speed_auto=-1.0*(sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r))/10.0;
darkumatar 0:a3056628cb39 304 if(test_speed_auto>1000){
darkumatar 0:a3056628cb39 305 test_speed_auto=1000;
darkumatar 0:a3056628cb39 306 }else if(test_speed_auto<-1000){
darkumatar 0:a3056628cb39 307 test_speed_auto=-1000;
darkumatar 0:a3056628cb39 308 }
darkumatar 0:a3056628cb39 309 speedz(0,(test_speed_auto)*1);
darkumatar 0:a3056628cb39 310
darkumatar 0:a3056628cb39 311 }else {
darkumatar 0:a3056628cb39 312 auto_swich=false;
darkumatar 0:a3056628cb39 313 }
darkumatar 0:a3056628cb39 314
darkumatar 0:a3056628cb39 315 */
darkumatar 0:a3056628cb39 316 goal_angle1=-1.0*atan((goal_x2-x_period)/(goal_y2-y_period))/M_PI*180.0;
darkumatar 0:a3056628cb39 317
darkumatar 0:a3056628cb39 318 if(goal_angle1>0){
darkumatar 0:a3056628cb39 319 goal_angle1-=180.0;
darkumatar 0:a3056628cb39 320 }else{
darkumatar 0:a3056628cb39 321 goal_angle1+=180.0;
darkumatar 0:a3056628cb39 322 }
darkumatar 0:a3056628cb39 323
darkumatar 0:a3056628cb39 324
darkumatar 0:a3056628cb39 325
darkumatar 0:a3056628cb39 326 if(step==0&&((sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))>1000)|| (sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))<-1000))){
darkumatar 0:a3056628cb39 327 pro3(-test_ang(goal_angle1-gyro.yaw),test_limit(goal_x-x_period),test_limit(goal_y-y_period));
darkumatar 0:a3056628cb39 328 }else if(step>=0&&step<60){
darkumatar 0:a3056628cb39 329 for(i=0;i<4;i++){
darkumatar 0:a3056628cb39 330 target[i]=0;
darkumatar 0:a3056628cb39 331 motor_p[i]=0;
darkumatar 0:a3056628cb39 332 motor_m[i]=0;
darkumatar 0:a3056628cb39 333 }
darkumatar 0:a3056628cb39 334 pro3(-test_ang(goal_angle1-gyro.yaw),test_limit(goal_x-x_period),test_limit(goal_y-y_period));
darkumatar 0:a3056628cb39 335 }else if(step==60&&((sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)>1000)|| (sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)<-1000))){
darkumatar 0:a3056628cb39 336
darkumatar 0:a3056628cb39 337
darkumatar 0:a3056628cb39 338 }else {
darkumatar 0:a3056628cb39 339 auto_swich=false;
darkumatar 0:a3056628cb39 340 }
darkumatar 0:a3056628cb39 341
darkumatar 0:a3056628cb39 342
darkumatar 0:a3056628cb39 343 }
darkumatar 0:a3056628cb39 344
darkumatar 0:a3056628cb39 345 /*
darkumatar 0:a3056628cb39 346 if(limitB){
darkumatar 0:a3056628cb39 347 limi=true;
darkumatar 0:a3056628cb39 348 limilimi_swich=false;
darkumatar 0:a3056628cb39 349 }else{
darkumatar 0:a3056628cb39 350 int arc=0;
darkumatar 0:a3056628cb39 351 for(i=0;i<4;i++){
darkumatar 0:a3056628cb39 352 arc+=target[i]*sin((45.0+90.0*i)/90.0*M_PI);
darkumatar 0:a3056628cb39 353 }
darkumatar 0:a3056628cb39 354 if(arc>0){
darkumatar 0:a3056628cb39 355 }
darkumatar 0:a3056628cb39 356 if(limilimi_swich==false){
darkumatar 0:a3056628cb39 357 limilimi_swich=true;
darkumatar 0:a3056628cb39 358 limilimi.reset();
darkumatar 0:a3056628cb39 359 }
darkumatar 0:a3056628cb39 360 }
darkumatar 0:a3056628cb39 361 if(limilimi.read_ms()<500&&!limitB){
darkumatar 0:a3056628cb39 362 limi=false;
darkumatar 0:a3056628cb39 363 for(i=0;i<4;i++){
darkumatar 0:a3056628cb39 364 target[i] =0;
darkumatar 0:a3056628cb39 365 }
darkumatar 0:a3056628cb39 366 }else {
darkumatar 0:a3056628cb39 367 limi=true;
darkumatar 0:a3056628cb39 368 }
darkumatar 0:a3056628cb39 369
darkumatar 0:a3056628cb39 370 */
darkumatar 0:a3056628cb39 371 for(i=0;i<4;i++){
darkumatar 0:a3056628cb39 372 after_pwm[i]=rote[i].get();
darkumatar 0:a3056628cb39 373 after[i]=v[i].get();//ロリコンから値読み取り
darkumatar 0:a3056628cb39 374 integral[i]+= (((((target[i]/10.0-(after[i]-before[i]))+ before_parus[i])*0.1)/2.0)/102.40);//積分の所
darkumatar 0:a3056628cb39 375 P[i]=0.05*(target[i]/10.0-(after[i]-before[i]))/102.40;//比例
darkumatar 0:a3056628cb39 376 D[i]=0.0001*((((target[i]/10.0-(after[i]-before[i]))- before_parus[i])/0.1)/102.40);//微分
darkumatar 0:a3056628cb39 377 I[i]=0.0001*integral[i];//積分
darkumatar 0:a3056628cb39 378 ca[i]=ca[i]+P[i]+I[i]+D[i];//足し合わせる
darkumatar 0:a3056628cb39 379 if(ca[i]>0.4){ca[i]=0.4;}
darkumatar 0:a3056628cb39 380 if(ca[i]<-0.4){ca[i]=-0.4;}
darkumatar 0:a3056628cb39 381 if(0.03>ca[i]&&ca[i]>-0.03){ca[i]=0;}
darkumatar 0:a3056628cb39 382 motor_p[i]=ca[i];
darkumatar 0:a3056628cb39 383 motor_m[i]=ca[i]*-1.0;
darkumatar 0:a3056628cb39 384 speed_pwm[i]=after_pwm[i]-before_pwm[i];
darkumatar 0:a3056628cb39 385 before_parus[i]=target[i]/10.0-(after[i]-before[i]);
darkumatar 0:a3056628cb39 386 before[i]=after[i];
darkumatar 0:a3056628cb39 387 before_pwm[i]=after_pwm[i];
darkumatar 0:a3056628cb39 388 }
darkumatar 0:a3056628cb39 389 /*
darkumatar 0:a3056628cb39 390 for(i=0;i<4;i++){
darkumatar 0:a3056628cb39 391 x_period+=speed_pwm[i]*cos(M_PI/180.0*(angle + 90.0*i));
darkumatar 0:a3056628cb39 392 y_period+=speed_pwm[i]*sin(M_PI/180.0*(angle + 90.0*i));
darkumatar 0:a3056628cb39 393 }
darkumatar 0:a3056628cb39 394 */
darkumatar 0:a3056628cb39 395 x_period+=(((speed_pwm[0]-speed_pwm[2])*cos(M_PI/180.0*angle)+(speed_pwm[1]-speed_pwm[3])*cos(M_PI/180.0*(angle+90.0))*(-1.0))/2);
darkumatar 0:a3056628cb39 396 y_period+=(((speed_pwm[0]-speed_pwm[2])*sin(M_PI/180.0*angle)+(speed_pwm[1]-speed_pwm[3])*sin(M_PI/180.0*(angle+90.0))*(-1.0))/2);
darkumatar 0:a3056628cb39 397
darkumatar 0:a3056628cb39 398 if(liset_abc==true){
darkumatar 0:a3056628cb39 399 x_period=0;
darkumatar 0:a3056628cb39 400 y_period=0;
darkumatar 0:a3056628cb39 401 gyro.reset(0);
darkumatar 0:a3056628cb39 402
darkumatar 0:a3056628cb39 403 }
darkumatar 0:a3056628cb39 404
darkumatar 0:a3056628cb39 405 slave.port2.printf("%d\n",1);
darkumatar 0:a3056628cb39 406 while(name.read_ms()<30){}
darkumatar 0:a3056628cb39 407 slave.send1(255,16,x_period*1);
darkumatar 0:a3056628cb39 408 while(name.read_ms()<60){}
darkumatar 0:a3056628cb39 409 slave.send1(255,17,y_period*1);
darkumatar 0:a3056628cb39 410 while(name.read_ms()<100){}
darkumatar 0:a3056628cb39 411 slave.send1(255,18,gyro.yaw*1);
darkumatar 0:a3056628cb39 412 for(i = 0; i < 4; i++){
darkumatar 0:a3056628cb39 413 npx.global_scale = 0.05f;
darkumatar 0:a3056628cb39 414 npx.normalize = false;
darkumatar 0:a3056628cb39 415 if(check_tepu>0){
darkumatar 0:a3056628cb39 416 npx.setPixelColor(i,0xFF0000);
darkumatar 0:a3056628cb39 417 }else {
darkumatar 0:a3056628cb39 418 npx.setPixelColor(i,0x0000FF);
darkumatar 0:a3056628cb39 419 }
darkumatar 0:a3056628cb39 420 }
darkumatar 0:a3056628cb39 421 for(i = 4; i < 7; i++){
darkumatar 0:a3056628cb39 422 npx.global_scale = 0.05f;
darkumatar 0:a3056628cb39 423 npx.normalize = false;
darkumatar 0:a3056628cb39 424 if(auto_swich){
darkumatar 0:a3056628cb39 425 npx.setPixelColor(i,0xFFFFFF);
darkumatar 0:a3056628cb39 426 }else {
darkumatar 0:a3056628cb39 427 npx.setPixelColor(i,0x00FF00);
darkumatar 0:a3056628cb39 428 }
darkumatar 0:a3056628cb39 429 }
darkumatar 0:a3056628cb39 430 npx.show();
darkumatar 0:a3056628cb39 431 if(check_tepu==0){
darkumatar 0:a3056628cb39 432 auto_mode+=1;
darkumatar 0:a3056628cb39 433 Led2.write(1);
darkumatar 0:a3056628cb39 434 }else {
darkumatar 0:a3056628cb39 435 auto_mode=0;
darkumatar 0:a3056628cb39 436 Led2.write(0);
darkumatar 0:a3056628cb39 437 }
darkumatar 0:a3056628cb39 438 if(auto_mode>1){
darkumatar 0:a3056628cb39 439 limit_switch=false;
darkumatar 0:a3056628cb39 440 }
darkumatar 0:a3056628cb39 441 check_tepu=0;
darkumatar 0:a3056628cb39 442 gyro.update();
darkumatar 0:a3056628cb39 443 name.reset();
darkumatar 0:a3056628cb39 444 }
darkumatar 0:a3056628cb39 445 while(1){
darkumatar 0:a3056628cb39 446 Led3.write(1);
darkumatar 0:a3056628cb39 447 for(i=0;i<4;i++){
darkumatar 0:a3056628cb39 448 target[i]=0;
darkumatar 0:a3056628cb39 449 motor_p[i]=0;
darkumatar 0:a3056628cb39 450 motor_m[i]=0;
darkumatar 0:a3056628cb39 451 npx.global_scale = 0.05f;
darkumatar 0:a3056628cb39 452 npx.normalize = false;
darkumatar 0:a3056628cb39 453 npx.setPixelColor(i,0x00FFFF);
darkumatar 0:a3056628cb39 454 }
darkumatar 0:a3056628cb39 455 npx.show();
darkumatar 0:a3056628cb39 456 }
darkumatar 0:a3056628cb39 457 }