go-roboB / aa2_master

master

Dependencies:   arrc_mbed

main.cpp@3:a35f7cab9de2, 2022-04-15 (annotated)

Committer:
darkumatar
Date:
Fri Apr 15 08:11:58 2022 +0000
Revision:
3:a35f7cab9de2
Parent:
2:2feb9be14c08 
Is this OK?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
darkumatar 3:a35f7cab9de2 1 #include "mbed.h" //mbed
darkumatar 3:a35f7cab9de2 2 #include"scrp_slave.hpp" //通信規格scrのファイル
darkumatar 3:a35f7cab9de2 3 #include "rotary_inc.hpp" //モーターのhファイル
darkumatar 3:a35f7cab9de2 4 #include"cmath" //sin cos計算などをするためのhファイル
darkumatar 3:a35f7cab9de2 5 #include"gy521.hpp" //ジャイロセンサのためのhファイル
darkumatar 3:a35f7cab9de2 6 #include "neopixel.h" //テープLEDのためのやつ
darkumatar 0:55f25431e6d1 7
darkumatar 3:a35f7cab9de2 8 bool the_left=true; //フィールドの左側でやるか　右側でやるかで変える
darkumatar 2:2feb9be14c08 9
darkumatar 2:2feb9be14c08 10
darkumatar 3:a35f7cab9de2 11 double goal_x = -17700.0; //ゴールの位置の座標　今は仮に右側の値が代入されている
darkumatar 0:55f25431e6d1 12 double goal_y = 28800.0;
darkumatar 0:55f25431e6d1 13
darkumatar 3:a35f7cab9de2 14 int goal_x2 = -8000000; //発射時のロボットの向きを決める変数　アークタンジェントで計算してる
darkumatar 0:55f25431e6d1 15 int goal_y2 = 10000000;
darkumatar 0:55f25431e6d1 16
darkumatar 3:a35f7cab9de2 17 #define goal_x_right -17700.0 //右側の時の設定集（ゴール位置　向き）
darkumatar 0:55f25431e6d1 18 #define goal_y_right 28800.0//23800.0
darkumatar 0:55f25431e6d1 19 #define goal_x2_right -8000000
darkumatar 0:55f25431e6d1 20 #define goal_y2_right 10000000
darkumatar 0:55f25431e6d1 21
darkumatar 3:a35f7cab9de2 22 #define goal_x_left -17700.0 //左側の時の設定集（ゴール位置　向き）
darkumatar 3:a35f7cab9de2 23 #define goal_y_left -25800.0//-23800.0
darkumatar 0:55f25431e6d1 24 #define goal_x2_left -8000000
darkumatar 0:55f25431e6d1 25 #define goal_y2_left -8000000
darkumatar 0:55f25431e6d1 26
darkumatar 3:a35f7cab9de2 27 //#define period_r 8000.0 //今は　もう　使ってない
darkumatar 3:a35f7cab9de2 28 NeoPixelOut npx(PB_5,36); //テープLEDの使用宣言
darkumatar 0:55f25431e6d1 29 I2C i2d(PB_3,PB_10);
darkumatar 0:55f25431e6d1 30 GY521 gyro(i2d);
darkumatar 3:a35f7cab9de2 31 ScrpSlave slave(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800); //通信規格宣言
darkumatar 0:55f25431e6d1 32 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:55f25431e6d1 33 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 3:a35f7cab9de2 34 int target[4]; //target変数　この変数の値を目指すように動く
darkumatar 3:a35f7cab9de2 35 bool auto_swich = false; //auto_swich true=自動モード false=手動モード
darkumatar 3:a35f7cab9de2 36 bool limit_switch = true; //limit_swich これがonの間　このプログラムが動く
darkumatar 3:a35f7cab9de2 37 int step=0; //自動操縦の時の移行
darkumatar 3:a35f7cab9de2 38 int auto_mode=0; //通信状態のチェック　不安定時　limit_swichをoff
darkumatar 3:a35f7cab9de2 39 int side_change=0; //左か右かのときでの初期設定　たしか、1の時が左　0の時が右
darkumatar 1:9f57d402ea10 40
darkumatar 3:a35f7cab9de2 41 int ty=0; //テープLEDの色変えに使う　絶対モードか相対モードか
darkumatar 1:9f57d402ea10 42
darkumatar 1:9f57d402ea10 43
darkumatar 1:9f57d402ea10 44
darkumatar 1:9f57d402ea10 45
darkumatar 0:55f25431e6d1 46
darkumatar 3:a35f7cab9de2 47 int direct_xx=0; //絶対操作時の　上になんぼいけばいいかの変数(だいたい-64~64)
darkumatar 3:a35f7cab9de2 48 int direct_yy=0; //絶対操作時の　横になんぼいけばいいかの変数(だいたい-64~64)
darkumatar 3:a35f7cab9de2 49 int direct_turn=0; //絶対操作時の　回転
darkumatar 0:55f25431e6d1 50
darkumatar 3:a35f7cab9de2 51 DigitalOut Led1(PB_2); //LED　　　非常停止中かどうか
darkumatar 3:a35f7cab9de2 52 DigitalOut Led2(PC_6); //LED このマイコンが起動してるかどうか
darkumatar 3:a35f7cab9de2 53 DigitalOut Led3(PB_15); //LED 通信が途絶えて非常がかかったかどうか
darkumatar 3:a35f7cab9de2 54 DigitalOut Led4(PA_10); //LED　　　絶対モードか相対モードか
darkumatar 3:a35f7cab9de2 55 int check_tepu=0; // 通信状態のチェック　途絶えてたら切る
darkumatar 0:55f25431e6d1 56
darkumatar 3:a35f7cab9de2 57 bool limi=true; //用途不明 名残り説
darkumatar 3:a35f7cab9de2 58 bool liset_abc=false; //位置リセット
darkumatar 0:55f25431e6d1 59
darkumatar 0:55f25431e6d1 60
darkumatar 3:a35f7cab9de2 61 DigitalIn limitB(PB_12,PullUp); //今のところ　用途無し
darkumatar 0:55f25431e6d1 62
darkumatar 0:55f25431e6d1 63
darkumatar 0:55f25431e6d1 64
darkumatar 3:a35f7cab9de2 65 bool add(int id,int ppp){ //速度を変えるやつ
darkumatar 0:55f25431e6d1 66 Led4.write(0);
darkumatar 3:a35f7cab9de2 67 if(auto_swich==false&&limi==true){
darkumatar 0:55f25431e6d1 68 target[id]=30*ppp;
darkumatar 0:55f25431e6d1 69 }
darkumatar 0:55f25431e6d1 70 return true;
darkumatar 0:55f25431e6d1 71 }
darkumatar 0:55f25431e6d1 72
darkumatar 3:a35f7cab9de2 73 bool auto_on(int id,int ppp){//
darkumatar 0:55f25431e6d1 74 if(ppp==1){
darkumatar 0:55f25431e6d1 75 auto_swich=true;
darkumatar 0:55f25431e6d1 76 }
darkumatar 0:55f25431e6d1 77 return true;
darkumatar 0:55f25431e6d1 78 }
darkumatar 0:55f25431e6d1 79
darkumatar 3:a35f7cab9de2 80 bool auto_off(int id,int ppp){//
darkumatar 0:55f25431e6d1 81 if(ppp==1){
darkumatar 0:55f25431e6d1 82 auto_swich=false;
darkumatar 0:55f25431e6d1 83 }
darkumatar 0:55f25431e6d1 84 return true;
darkumatar 0:55f25431e6d1 85 }
darkumatar 0:55f25431e6d1 86
darkumatar 3:a35f7cab9de2 87 bool pro2(){ //アングルとスピードを同時に決める　下から3桁で角度を決める(0~360)　それ以外でスピードを決める　
darkumatar 1:9f57d402ea10 88 double anglert=-135.000000000+180.0*side_change;
darkumatar 0:55f25431e6d1 89 Led4.write(1);
darkumatar 0:55f25431e6d1 90 for(int i=0;i<4;i++){
darkumatar 0:55f25431e6d1 91 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:55f25431e6d1 92
darkumatar 0:55f25431e6d1 93 }
darkumatar 0:55f25431e6d1 94 return true;
darkumatar 0:55f25431e6d1 95 }
darkumatar 0:55f25431e6d1 96 bool pro3(double goal_angle_ppp,int goal_x_speed,int goal_y_speed){
darkumatar 0:55f25431e6d1 97 double anglert=-135.000000000;
darkumatar 0:55f25431e6d1 98 for(int i=0;i<4;i++){
darkumatar 0:55f25431e6d1 99 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:55f25431e6d1 100
darkumatar 0:55f25431e6d1 101 }
darkumatar 0:55f25431e6d1 102 return true;
darkumatar 0:55f25431e6d1 103 }
darkumatar 0:55f25431e6d1 104 int test_limit(int test_limilimi){
darkumatar 0:55f25431e6d1 105 test_limilimi=test_limilimi/100;
darkumatar 0:55f25431e6d1 106 if(test_limilimi>64){
darkumatar 0:55f25431e6d1 107 test_limilimi=64;
darkumatar 0:55f25431e6d1 108 }
darkumatar 0:55f25431e6d1 109 if(test_limilimi<-64){
darkumatar 0:55f25431e6d1 110 test_limilimi=-64;
darkumatar 0:55f25431e6d1 111 }
darkumatar 0:55f25431e6d1 112 return test_limilimi;
darkumatar 0:55f25431e6d1 113 }
darkumatar 0:55f25431e6d1 114
darkumatar 0:55f25431e6d1 115 double test_ang(double test_limilimi){
darkumatar 0:55f25431e6d1 116 test_limilimi=test_limilimi*2;
darkumatar 0:55f25431e6d1 117 if(test_limilimi>40){
darkumatar 0:55f25431e6d1 118 test_limilimi=40;
darkumatar 0:55f25431e6d1 119 }
darkumatar 0:55f25431e6d1 120 if(test_limilimi<-40){
darkumatar 0:55f25431e6d1 121 test_limilimi=-40;
darkumatar 0:55f25431e6d1 122 }
darkumatar 0:55f25431e6d1 123 return test_limilimi;
darkumatar 0:55f25431e6d1 124 }
darkumatar 0:55f25431e6d1 125
darkumatar 0:55f25431e6d1 126 bool limit2(int id,int ppp){
darkumatar 0:55f25431e6d1 127 if(ppp==1){
darkumatar 0:55f25431e6d1 128 limit_switch=false;
darkumatar 0:55f25431e6d1 129 }
darkumatar 0:55f25431e6d1 130 return true;
darkumatar 0:55f25431e6d1 131 }
darkumatar 0:55f25431e6d1 132
darkumatar 0:55f25431e6d1 133 bool direct_x(int id,int ppp){
darkumatar 0:55f25431e6d1 134 direct_xx=ppp;
darkumatar 0:55f25431e6d1 135 pro2();
darkumatar 0:55f25431e6d1 136 return true;
darkumatar 0:55f25431e6d1 137 }
darkumatar 0:55f25431e6d1 138 bool direct_y(int id,int ppp){
darkumatar 0:55f25431e6d1 139 direct_yy=ppp;
darkumatar 0:55f25431e6d1 140 pro2();
darkumatar 0:55f25431e6d1 141 return true;
darkumatar 0:55f25431e6d1 142 }
darkumatar 0:55f25431e6d1 143
darkumatar 0:55f25431e6d1 144 bool direct_tu(int id,int ppp){
darkumatar 0:55f25431e6d1 145 direct_turn=ppp;
darkumatar 0:55f25431e6d1 146 pro2();
darkumatar 0:55f25431e6d1 147 return true;
darkumatar 0:55f25431e6d1 148 }
darkumatar 0:55f25431e6d1 149
darkumatar 0:55f25431e6d1 150 bool limit(int a,int &b){
darkumatar 0:55f25431e6d1 151 return limit2(0,a);
darkumatar 0:55f25431e6d1 152 }
darkumatar 0:55f25431e6d1 153 bool abc1(int a,int &b){
darkumatar 0:55f25431e6d1 154 check_tepu+=1;
darkumatar 2:2feb9be14c08 155 ty=0;
darkumatar 0:55f25431e6d1 156 return add(0,a);
darkumatar 0:55f25431e6d1 157 }
darkumatar 0:55f25431e6d1 158 bool abc2(int a,int &b){
darkumatar 0:55f25431e6d1 159 check_tepu+=1;
darkumatar 0:55f25431e6d1 160 return add(1,a);
darkumatar 0:55f25431e6d1 161 }
darkumatar 0:55f25431e6d1 162 bool abc3(int a,int &b){
darkumatar 0:55f25431e6d1 163 check_tepu+=1;
darkumatar 0:55f25431e6d1 164 return add(2,a);
darkumatar 0:55f25431e6d1 165 }
darkumatar 0:55f25431e6d1 166 bool abc4(int a,int &b){
darkumatar 0:55f25431e6d1 167 check_tepu+=1;
darkumatar 0:55f25431e6d1 168 return add(3,a);
darkumatar 0:55f25431e6d1 169 }
darkumatar 0:55f25431e6d1 170
darkumatar 0:55f25431e6d1 171 bool abc9(int a,int &b){
darkumatar 0:55f25431e6d1 172 return auto_on(0,a);
darkumatar 0:55f25431e6d1 173 }
darkumatar 0:55f25431e6d1 174 bool abc10(int a,int &b){
darkumatar 0:55f25431e6d1 175 return auto_off(0,a);
darkumatar 0:55f25431e6d1 176 }
darkumatar 0:55f25431e6d1 177 bool abc11(int a,int &b){
darkumatar 0:55f25431e6d1 178 check_tepu+=1;
darkumatar 0:55f25431e6d1 179 return direct_x(0,a);
darkumatar 0:55f25431e6d1 180 }
darkumatar 0:55f25431e6d1 181 bool abc12(int a,int &b){
darkumatar 0:55f25431e6d1 182 check_tepu+=1;
darkumatar 0:55f25431e6d1 183 return direct_y(0,a);
darkumatar 0:55f25431e6d1 184 }
darkumatar 0:55f25431e6d1 185 bool abc13(int a,int &b){
darkumatar 0:55f25431e6d1 186 check_tepu+=1;
darkumatar 2:2feb9be14c08 187 ty=1;
darkumatar 0:55f25431e6d1 188 return direct_tu(0,a);
darkumatar 0:55f25431e6d1 189 }
darkumatar 0:55f25431e6d1 190 bool abc14(int a,int &b){
darkumatar 0:55f25431e6d1 191 if(a==1){
darkumatar 0:55f25431e6d1 192 liset_abc=true;
darkumatar 0:55f25431e6d1 193 }else{
darkumatar 0:55f25431e6d1 194 liset_abc=false;
darkumatar 0:55f25431e6d1 195 }
darkumatar 0:55f25431e6d1 196 return true;
darkumatar 0:55f25431e6d1 197 }
darkumatar 0:55f25431e6d1 198
darkumatar 0:55f25431e6d1 199
darkumatar 0:55f25431e6d1 200 int main() {
darkumatar 0:55f25431e6d1 201 Led1.write(1);
darkumatar 0:55f25431e6d1 202 Led2.write(0);
darkumatar 0:55f25431e6d1 203 slave.addCMD(1,limit);
darkumatar 0:55f25431e6d1 204 slave.addCMD(2,abc1);
darkumatar 0:55f25431e6d1 205 slave.addCMD(3,abc2);
darkumatar 0:55f25431e6d1 206 slave.addCMD(4,abc3);
darkumatar 0:55f25431e6d1 207 slave.addCMD(5,abc4);
darkumatar 0:55f25431e6d1 208 slave.addCMD(12,abc9);
darkumatar 0:55f25431e6d1 209 slave.addCMD(13,abc10);
darkumatar 0:55f25431e6d1 210 slave.addCMD(31,abc11);
darkumatar 0:55f25431e6d1 211 slave.addCMD(32,abc12);
darkumatar 0:55f25431e6d1 212 slave.addCMD(33,abc13);
darkumatar 0:55f25431e6d1 213 slave.addCMD(34,abc14);
darkumatar 1:9f57d402ea10 214 side_change=0;
darkumatar 0:55f25431e6d1 215
darkumatar 0:55f25431e6d1 216 goal_x=goal_x_right;
darkumatar 0:55f25431e6d1 217 goal_y=goal_y_right;
darkumatar 0:55f25431e6d1 218 goal_x2=goal_x2_right;
darkumatar 0:55f25431e6d1 219 goal_y2=goal_y2_right;
darkumatar 0:55f25431e6d1 220
darkumatar 2:2feb9be14c08 221 if(the_left==true){
darkumatar 0:55f25431e6d1 222 side_change=1;
darkumatar 0:55f25431e6d1 223 goal_x=goal_x_left;
darkumatar 0:55f25431e6d1 224 goal_y=goal_y_left;
darkumatar 0:55f25431e6d1 225 goal_x2=goal_x2_left;
darkumatar 0:55f25431e6d1 226 goal_y2=goal_y2_left;
darkumatar 0:55f25431e6d1 227 }
darkumatar 0:55f25431e6d1 228
darkumatar 0:55f25431e6d1 229
darkumatar 0:55f25431e6d1 230
darkumatar 0:55f25431e6d1 231
darkumatar 0:55f25431e6d1 232 int i;
darkumatar 0:55f25431e6d1 233 double x_period=0;
darkumatar 0:55f25431e6d1 234 double y_period=0;
darkumatar 0:55f25431e6d1 235 int after[4],before[4],before_parus[4],speed_pwm[4],after_pwm[4],before_pwm[4];
darkumatar 0:55f25431e6d1 236 double ca[4],P[4],I[4],D[4],integral[4];
darkumatar 0:55f25431e6d1 237 PwmOut motor_p[4]{PwmOut(PB_1),PwmOut(PB_13),PwmOut(PC_9),PwmOut(PB_7)};//モーター宣言 PB_6 PC_8
darkumatar 0:55f25431e6d1 238 PwmOut motor_m[4]{PwmOut(PA_11),PwmOut(PB_14),PwmOut(PC_8),PwmOut(PB_6)};//モーター宣言 PB_7 PC_9
darkumatar 0:55f25431e6d1 239 for(i=0;i<4;i++){
darkumatar 0:55f25431e6d1 240 motor_p[i].period_us(2048);
darkumatar 0:55f25431e6d1 241 motor_m[i].period_us(2048);
darkumatar 0:55f25431e6d1 242 target[i]=0;
darkumatar 0:55f25431e6d1 243 after[i]=0;
darkumatar 0:55f25431e6d1 244 before[i]=0;
darkumatar 0:55f25431e6d1 245 ca[i]=0;
darkumatar 0:55f25431e6d1 246 P[i]=0;
darkumatar 0:55f25431e6d1 247 I[i]=0;
darkumatar 0:55f25431e6d1 248 D[i]=0;
darkumatar 0:55f25431e6d1 249 before_parus[i]=0;
darkumatar 0:55f25431e6d1 250 integral[i]=0;
darkumatar 0:55f25431e6d1 251 speed_pwm[i]=0;
darkumatar 0:55f25431e6d1 252 after_pwm[i]=0;
darkumatar 0:55f25431e6d1 253 before_pwm[i]=0;
darkumatar 0:55f25431e6d1 254 }
darkumatar 0:55f25431e6d1 255 Timer name;
darkumatar 0:55f25431e6d1 256 name.start();
darkumatar 0:55f25431e6d1 257 gyro.start();
darkumatar 0:55f25431e6d1 258 double angle=0;
darkumatar 0:55f25431e6d1 259 double goal_angle1=0;
darkumatar 0:55f25431e6d1 260 Led1.write(1);
darkumatar 3:a35f7cab9de2 261
darkumatar 3:a35f7cab9de2 262
darkumatar 3:a35f7cab9de2 263
darkumatar 0:55f25431e6d1 264 while(limit_switch==true) {
darkumatar 0:55f25431e6d1 265 angle=gyro.yaw-90.00000;
darkumatar 0:55f25431e6d1 266 if(auto_swich==true){
darkumatar 0:55f25431e6d1 267 goal_angle1=-1.0*atan((goal_x2-x_period)/(goal_y2-y_period))/M_PI*180.0;
darkumatar 0:55f25431e6d1 268
darkumatar 0:55f25431e6d1 269 if(side_change==0){
darkumatar 0:55f25431e6d1 270 if(goal_angle1>0){
darkumatar 0:55f25431e6d1 271 goal_angle1-=180.0;
darkumatar 0:55f25431e6d1 272 }else{
darkumatar 0:55f25431e6d1 273 goal_angle1+=180.0;
darkumatar 0:55f25431e6d1 274 }
darkumatar 0:55f25431e6d1 275 }
darkumatar 0:55f25431e6d1 276
darkumatar 0:55f25431e6d1 277
darkumatar 3:a35f7cab9de2 278 if(((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:55f25431e6d1 279 pro3(-test_ang(goal_angle1-gyro.yaw),test_limit(goal_x-x_period),test_limit(goal_y-y_period));
darkumatar 0:55f25431e6d1 280 }
darkumatar 0:55f25431e6d1 281
darkumatar 0:55f25431e6d1 282
darkumatar 0:55f25431e6d1 283 }
darkumatar 0:55f25431e6d1 284
darkumatar 0:55f25431e6d1 285 for(i=0;i<4;i++){
darkumatar 0:55f25431e6d1 286 after_pwm[i]=rote[i].get();
darkumatar 0:55f25431e6d1 287 after[i]=v[i].get();//ロリコンから値読み取り
darkumatar 0:55f25431e6d1 288 integral[i]+= (((((target[i]/10.0-(after[i]-before[i]))+ before_parus[i])*0.1)/2.0)/102.40);//積分の所
darkumatar 0:55f25431e6d1 289 P[i]=0.05*(target[i]/10.0-(after[i]-before[i]))/102.40;//比例
darkumatar 0:55f25431e6d1 290 D[i]=0.0001*((((target[i]/10.0-(after[i]-before[i]))- before_parus[i])/0.1)/102.40);//微分
darkumatar 0:55f25431e6d1 291 I[i]=0.0001*integral[i];//積分
darkumatar 0:55f25431e6d1 292 ca[i]=ca[i]+P[i]+I[i]+D[i];//足し合わせる
darkumatar 0:55f25431e6d1 293 if(ca[i]>0.4){ca[i]=0.4;}
darkumatar 0:55f25431e6d1 294 if(ca[i]<-0.4){ca[i]=-0.4;}
darkumatar 0:55f25431e6d1 295 if(0.03>ca[i]&&ca[i]>-0.03){ca[i]=0;}
darkumatar 0:55f25431e6d1 296 motor_p[i]=ca[i];
darkumatar 0:55f25431e6d1 297 motor_m[i]=ca[i]*-1.0;
darkumatar 0:55f25431e6d1 298 speed_pwm[i]=after_pwm[i]-before_pwm[i];
darkumatar 0:55f25431e6d1 299 before_parus[i]=target[i]/10.0-(after[i]-before[i]);
darkumatar 0:55f25431e6d1 300 before[i]=after[i];
darkumatar 0:55f25431e6d1 301 before_pwm[i]=after_pwm[i];
darkumatar 0:55f25431e6d1 302 }
darkumatar 0:55f25431e6d1 303 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:55f25431e6d1 304 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:55f25431e6d1 305
darkumatar 0:55f25431e6d1 306 if(liset_abc==true){
darkumatar 0:55f25431e6d1 307 x_period=0;
darkumatar 0:55f25431e6d1 308 y_period=0;
darkumatar 0:55f25431e6d1 309 gyro.reset(0);
darkumatar 0:55f25431e6d1 310
darkumatar 0:55f25431e6d1 311 }
darkumatar 0:55f25431e6d1 312
darkumatar 0:55f25431e6d1 313 //slave.port2.printf("%d\n",1);
darkumatar 3:a35f7cab9de2 314 //while(name.read_ms()<30){}
darkumatar 0:55f25431e6d1 315 //slave.send1(255,16,x_period*1);
darkumatar 3:a35f7cab9de2 316 //while(name.read_ms()<60){}
darkumatar 0:55f25431e6d1 317 //slave.send1(255,17,y_period*1);
darkumatar 0:55f25431e6d1 318 while(name.read_ms()<100){}
darkumatar 0:55f25431e6d1 319 //slave.send1(255,18,gyro.yaw*1);
darkumatar 3:a35f7cab9de2 320 for(i = 0; i < 12; i++){
darkumatar 0:55f25431e6d1 321 npx.global_scale = 0.05f;
darkumatar 0:55f25431e6d1 322 npx.normalize = false;
darkumatar 2:2feb9be14c08 323 if(ty==0){
darkumatar 3:a35f7cab9de2 324 npx.setPixelColor(i,0x0000FF);
darkumatar 0:55f25431e6d1 325 }else {
darkumatar 3:a35f7cab9de2 326 npx.setPixelColor(i,0xFF0000);
darkumatar 0:55f25431e6d1 327 }
darkumatar 0:55f25431e6d1 328 }
darkumatar 3:a35f7cab9de2 329 for(i = 12; i < 24; i++){
darkumatar 0:55f25431e6d1 330 npx.global_scale = 0.05f;
darkumatar 0:55f25431e6d1 331 npx.normalize = false;
darkumatar 0:55f25431e6d1 332 if(auto_swich){
darkumatar 0:55f25431e6d1 333 npx.setPixelColor(i,0xFFFFFF);
darkumatar 0:55f25431e6d1 334 }else {
darkumatar 0:55f25431e6d1 335 npx.setPixelColor(i,0x00FF00);
darkumatar 0:55f25431e6d1 336 }
darkumatar 0:55f25431e6d1 337 }
darkumatar 3:a35f7cab9de2 338 for(i = 24; i < 36; i++){
darkumatar 0:55f25431e6d1 339 npx.global_scale = 0.05f;
darkumatar 0:55f25431e6d1 340 npx.normalize = false;
darkumatar 0:55f25431e6d1 341 if(side_change==0){
darkumatar 0:55f25431e6d1 342 npx.setPixelColor(i,0xFF00FF);
darkumatar 0:55f25431e6d1 343 }else {
darkumatar 0:55f25431e6d1 344 npx.setPixelColor(i,0xFFFF00);
darkumatar 0:55f25431e6d1 345 }
darkumatar 0:55f25431e6d1 346 }
darkumatar 0:55f25431e6d1 347 npx.show();
darkumatar 0:55f25431e6d1 348
darkumatar 0:55f25431e6d1 349
darkumatar 0:55f25431e6d1 350 if(check_tepu==0){
darkumatar 0:55f25431e6d1 351 auto_mode+=1;
darkumatar 0:55f25431e6d1 352 Led2.write(1);
darkumatar 0:55f25431e6d1 353 }else {
darkumatar 0:55f25431e6d1 354 auto_mode=0;
darkumatar 0:55f25431e6d1 355 Led2.write(0);
darkumatar 0:55f25431e6d1 356 }
darkumatar 0:55f25431e6d1 357 if(auto_mode>1){
darkumatar 0:55f25431e6d1 358 limit_switch=false;
darkumatar 0:55f25431e6d1 359 }
darkumatar 0:55f25431e6d1 360 check_tepu=0;
darkumatar 0:55f25431e6d1 361 gyro.update();
darkumatar 0:55f25431e6d1 362 name.reset();
darkumatar 0:55f25431e6d1 363 }
darkumatar 3:a35f7cab9de2 364
darkumatar 3:a35f7cab9de2 365
darkumatar 3:a35f7cab9de2 366
darkumatar 0:55f25431e6d1 367 while(1){
darkumatar 0:55f25431e6d1 368 Led3.write(1);
darkumatar 0:55f25431e6d1 369 for(i=0;i<4;i++){
darkumatar 0:55f25431e6d1 370 target[i]=0;
darkumatar 0:55f25431e6d1 371 motor_p[i]=0;
darkumatar 0:55f25431e6d1 372 motor_m[i]=0;
darkumatar 0:55f25431e6d1 373 npx.global_scale = 0.05f;
darkumatar 0:55f25431e6d1 374 npx.normalize = false;
darkumatar 0:55f25431e6d1 375 npx.setPixelColor(i,0x00FFFF);
darkumatar 0:55f25431e6d1 376 }
darkumatar 3:a35f7cab9de2 377 for(i=4;i<36;i++){
darkumatar 2:2feb9be14c08 378 npx.global_scale = 0.05f;
darkumatar 2:2feb9be14c08 379 npx.normalize = false;
darkumatar 2:2feb9be14c08 380 npx.setPixelColor(i,0x00FFFF);
darkumatar 2:2feb9be14c08 381 }
darkumatar 0:55f25431e6d1 382 npx.show();
darkumatar 0:55f25431e6d1 383 }
darkumatar 0:55f25431e6d1 384 }