go-roboB / Mbed 2 deprecated ashimawari_aa2_left

left

Dependencies:   mbed arrc_mbed

main.cpp@1:1f9a3449bbb9, 2022-04-08 (annotated)

Committer:
darkumatar
Date:
Fri Apr 08 09:31:13 2022 +0000
Revision:
1:1f9a3449bbb9
Parent:
0:c60fccf1ea71 
o

Who changed what in which revision?

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