re
Dependencies: arrc_mbed
main.cpp@0:da972c3ac4de, 2022-04-12 (annotated)
- Committer:
- darkumatar
- Date:
- Tue Apr 12 10:33:33 2022 +0000
- Revision:
- 0:da972c3ac4de
re
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
darkumatar | 0:da972c3ac4de | 1 | #include "mbed.h" |
darkumatar | 0:da972c3ac4de | 2 | #include"scrp_slave.hpp" |
darkumatar | 0:da972c3ac4de | 3 | #include "rotary_inc.hpp" |
darkumatar | 0:da972c3ac4de | 4 | #include"cmath" |
darkumatar | 0:da972c3ac4de | 5 | #include"gy521.hpp" |
darkumatar | 0:da972c3ac4de | 6 | #include "neopixel.h" |
darkumatar | 0:da972c3ac4de | 7 | |
darkumatar | 0:da972c3ac4de | 8 | |
darkumatar | 0:da972c3ac4de | 9 | #define goal_x -17700.0 |
darkumatar | 0:da972c3ac4de | 10 | #define goal_y -23800.0 |
darkumatar | 0:da972c3ac4de | 11 | #define goal_x2 -8000000 |
darkumatar | 0:da972c3ac4de | 12 | #define goal_y2 -8000000 |
darkumatar | 0:da972c3ac4de | 13 | #define period_r 8000.0 |
darkumatar | 0:da972c3ac4de | 14 | NeoPixelOut npx(PB_4,7); |
darkumatar | 0:da972c3ac4de | 15 | I2C i2d(PB_3,PB_10); |
darkumatar | 0:da972c3ac4de | 16 | GY521 gyro(i2d); |
darkumatar | 0:da972c3ac4de | 17 | ScrpSlave slave(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);//srcslave設定 |
darkumatar | 0:da972c3ac4de | 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:da972c3ac4de | 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:da972c3ac4de | 20 | int target[4];//target変数 この変数の値を目指すように動く |
darkumatar | 0:da972c3ac4de | 21 | //多分これは 今いらない機能 |
darkumatar | 0:da972c3ac4de | 22 | int angler=90; |
darkumatar | 0:da972c3ac4de | 23 | int speedr=1024; |
darkumatar | 0:da972c3ac4de | 24 | // |
darkumatar | 0:da972c3ac4de | 25 | bool auto_swich = false; |
darkumatar | 0:da972c3ac4de | 26 | bool limit_switch = true; |
darkumatar | 0:da972c3ac4de | 27 | int step=0; |
darkumatar | 0:da972c3ac4de | 28 | int auto_mode=0; |
darkumatar | 0:da972c3ac4de | 29 | |
darkumatar | 0:da972c3ac4de | 30 | int direct_xx=0; |
darkumatar | 0:da972c3ac4de | 31 | int direct_yy=0; |
darkumatar | 0:da972c3ac4de | 32 | int direct_turn=0; |
darkumatar | 0:da972c3ac4de | 33 | |
darkumatar | 0:da972c3ac4de | 34 | DigitalOut Led1(PB_2); |
darkumatar | 0:da972c3ac4de | 35 | DigitalOut Led2(PC_6); |
darkumatar | 0:da972c3ac4de | 36 | DigitalOut Led3(PB_15); |
darkumatar | 0:da972c3ac4de | 37 | DigitalOut Led4(PA_10); |
darkumatar | 0:da972c3ac4de | 38 | int check_tepu=0; |
darkumatar | 0:da972c3ac4de | 39 | |
darkumatar | 0:da972c3ac4de | 40 | bool limi=true; |
darkumatar | 0:da972c3ac4de | 41 | bool liset_abc=false; |
darkumatar | 0:da972c3ac4de | 42 | |
darkumatar | 0:da972c3ac4de | 43 | |
darkumatar | 0:da972c3ac4de | 44 | DigitalIn limitB(PB_12,PullUp); |
darkumatar | 0:da972c3ac4de | 45 | |
darkumatar | 0:da972c3ac4de | 46 | |
darkumatar | 0:da972c3ac4de | 47 | |
darkumatar | 0:da972c3ac4de | 48 | bool add(int id,int ppp){//速度を変えるやつ |
darkumatar | 0:da972c3ac4de | 49 | Led4.write(0); |
darkumatar | 0:da972c3ac4de | 50 | if(auto_swich==false&&limi==true){ |
darkumatar | 0:da972c3ac4de | 51 | target[id]=30*ppp; |
darkumatar | 0:da972c3ac4de | 52 | } |
darkumatar | 0:da972c3ac4de | 53 | return true; |
darkumatar | 0:da972c3ac4de | 54 | } |
darkumatar | 0:da972c3ac4de | 55 | |
darkumatar | 0:da972c3ac4de | 56 | |
darkumatar | 0:da972c3ac4de | 57 | |
darkumatar | 0:da972c3ac4de | 58 | bool auto_on(int id,int ppp){//おまけで作ったやつ スピードを決めてその方向へ走る |
darkumatar | 0:da972c3ac4de | 59 | if(ppp==1){ |
darkumatar | 0:da972c3ac4de | 60 | auto_swich=true; |
darkumatar | 0:da972c3ac4de | 61 | step=0; |
darkumatar | 0:da972c3ac4de | 62 | } |
darkumatar | 0:da972c3ac4de | 63 | return true; |
darkumatar | 0:da972c3ac4de | 64 | } |
darkumatar | 0:da972c3ac4de | 65 | |
darkumatar | 0:da972c3ac4de | 66 | bool auto_off(int id,int ppp){//おまけで作ったやつ スピードを決めてその方向へ走る |
darkumatar | 0:da972c3ac4de | 67 | if(ppp==1){ |
darkumatar | 0:da972c3ac4de | 68 | auto_swich=false; |
darkumatar | 0:da972c3ac4de | 69 | step=0; |
darkumatar | 0:da972c3ac4de | 70 | } |
darkumatar | 0:da972c3ac4de | 71 | return true; |
darkumatar | 0:da972c3ac4de | 72 | } |
darkumatar | 0:da972c3ac4de | 73 | |
darkumatar | 0:da972c3ac4de | 74 | bool pro2(){ //アングルとスピードを同時に決める 下から3桁で角度を決める(0~360) それ以外でスピードを決める これなに?私もわからん |
darkumatar | 0:da972c3ac4de | 75 | double anglert=-135.000000000+180.0; |
darkumatar | 0:da972c3ac4de | 76 | Led4.write(1); |
darkumatar | 0:da972c3ac4de | 77 | for(int i=0;i<4;i++){ |
darkumatar | 0:da972c3ac4de | 78 | 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:da972c3ac4de | 79 | |
darkumatar | 0:da972c3ac4de | 80 | } |
darkumatar | 0:da972c3ac4de | 81 | return true; |
darkumatar | 0:da972c3ac4de | 82 | } |
darkumatar | 0:da972c3ac4de | 83 | bool pro3(double goal_angle_ppp,int goal_x_speed,int goal_y_speed){ |
darkumatar | 0:da972c3ac4de | 84 | double anglert=-135.000000000; |
darkumatar | 0:da972c3ac4de | 85 | for(int i=0;i<4;i++){ |
darkumatar | 0:da972c3ac4de | 86 | 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:da972c3ac4de | 87 | |
darkumatar | 0:da972c3ac4de | 88 | } |
darkumatar | 0:da972c3ac4de | 89 | return true; |
darkumatar | 0:da972c3ac4de | 90 | } |
darkumatar | 0:da972c3ac4de | 91 | int test_limit(int test_limilimi){ |
darkumatar | 0:da972c3ac4de | 92 | test_limilimi=test_limilimi/100; |
darkumatar | 0:da972c3ac4de | 93 | if(test_limilimi>64){ |
darkumatar | 0:da972c3ac4de | 94 | test_limilimi=64; |
darkumatar | 0:da972c3ac4de | 95 | } |
darkumatar | 0:da972c3ac4de | 96 | if(test_limilimi<-64){ |
darkumatar | 0:da972c3ac4de | 97 | test_limilimi=-64; |
darkumatar | 0:da972c3ac4de | 98 | } |
darkumatar | 0:da972c3ac4de | 99 | return test_limilimi; |
darkumatar | 0:da972c3ac4de | 100 | } |
darkumatar | 0:da972c3ac4de | 101 | |
darkumatar | 0:da972c3ac4de | 102 | double test_ang(double test_limilimi){ |
darkumatar | 0:da972c3ac4de | 103 | test_limilimi=test_limilimi*2; |
darkumatar | 0:da972c3ac4de | 104 | if(test_limilimi>40){ |
darkumatar | 0:da972c3ac4de | 105 | test_limilimi=40; |
darkumatar | 0:da972c3ac4de | 106 | } |
darkumatar | 0:da972c3ac4de | 107 | if(test_limilimi<-40){ |
darkumatar | 0:da972c3ac4de | 108 | test_limilimi=-40; |
darkumatar | 0:da972c3ac4de | 109 | } |
darkumatar | 0:da972c3ac4de | 110 | return test_limilimi; |
darkumatar | 0:da972c3ac4de | 111 | } |
darkumatar | 0:da972c3ac4de | 112 | |
darkumatar | 0:da972c3ac4de | 113 | bool limit2(int id,int ppp){ |
darkumatar | 0:da972c3ac4de | 114 | if(ppp==1){ |
darkumatar | 0:da972c3ac4de | 115 | limit_switch=false; |
darkumatar | 0:da972c3ac4de | 116 | } |
darkumatar | 0:da972c3ac4de | 117 | return true; |
darkumatar | 0:da972c3ac4de | 118 | } |
darkumatar | 0:da972c3ac4de | 119 | |
darkumatar | 0:da972c3ac4de | 120 | bool direct_x(int id,int ppp){ |
darkumatar | 0:da972c3ac4de | 121 | direct_xx=ppp; |
darkumatar | 0:da972c3ac4de | 122 | pro2(); |
darkumatar | 0:da972c3ac4de | 123 | return true; |
darkumatar | 0:da972c3ac4de | 124 | } |
darkumatar | 0:da972c3ac4de | 125 | bool direct_y(int id,int ppp){ |
darkumatar | 0:da972c3ac4de | 126 | direct_yy=ppp; |
darkumatar | 0:da972c3ac4de | 127 | pro2(); |
darkumatar | 0:da972c3ac4de | 128 | return true; |
darkumatar | 0:da972c3ac4de | 129 | } |
darkumatar | 0:da972c3ac4de | 130 | |
darkumatar | 0:da972c3ac4de | 131 | bool direct_tu(int id,int ppp){ |
darkumatar | 0:da972c3ac4de | 132 | direct_turn=ppp; |
darkumatar | 0:da972c3ac4de | 133 | pro2(); |
darkumatar | 0:da972c3ac4de | 134 | return true; |
darkumatar | 0:da972c3ac4de | 135 | } |
darkumatar | 0:da972c3ac4de | 136 | |
darkumatar | 0:da972c3ac4de | 137 | bool limit(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 138 | return limit2(0,a); |
darkumatar | 0:da972c3ac4de | 139 | } |
darkumatar | 0:da972c3ac4de | 140 | bool abc1(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 141 | check_tepu+=1; |
darkumatar | 0:da972c3ac4de | 142 | return add(0,a); |
darkumatar | 0:da972c3ac4de | 143 | } |
darkumatar | 0:da972c3ac4de | 144 | bool abc2(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 145 | check_tepu+=1; |
darkumatar | 0:da972c3ac4de | 146 | return add(1,a); |
darkumatar | 0:da972c3ac4de | 147 | } |
darkumatar | 0:da972c3ac4de | 148 | bool abc3(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 149 | check_tepu+=1; |
darkumatar | 0:da972c3ac4de | 150 | return add(2,a); |
darkumatar | 0:da972c3ac4de | 151 | } |
darkumatar | 0:da972c3ac4de | 152 | bool abc4(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 153 | check_tepu+=1; |
darkumatar | 0:da972c3ac4de | 154 | return add(3,a); |
darkumatar | 0:da972c3ac4de | 155 | } |
darkumatar | 0:da972c3ac4de | 156 | bool abc9(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 157 | return auto_on(0,a); |
darkumatar | 0:da972c3ac4de | 158 | } |
darkumatar | 0:da972c3ac4de | 159 | bool abc10(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 160 | return auto_off(0,a); |
darkumatar | 0:da972c3ac4de | 161 | } |
darkumatar | 0:da972c3ac4de | 162 | bool abc11(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 163 | check_tepu+=1; |
darkumatar | 0:da972c3ac4de | 164 | return direct_x(0,a); |
darkumatar | 0:da972c3ac4de | 165 | } |
darkumatar | 0:da972c3ac4de | 166 | bool abc12(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 167 | check_tepu+=1; |
darkumatar | 0:da972c3ac4de | 168 | return direct_y(0,a); |
darkumatar | 0:da972c3ac4de | 169 | } |
darkumatar | 0:da972c3ac4de | 170 | bool abc13(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 171 | check_tepu+=1; |
darkumatar | 0:da972c3ac4de | 172 | return direct_tu(0,a); |
darkumatar | 0:da972c3ac4de | 173 | } |
darkumatar | 0:da972c3ac4de | 174 | bool abc14(int a,int &b){ |
darkumatar | 0:da972c3ac4de | 175 | if(a==1){ |
darkumatar | 0:da972c3ac4de | 176 | liset_abc=true; |
darkumatar | 0:da972c3ac4de | 177 | }else{ |
darkumatar | 0:da972c3ac4de | 178 | liset_abc=false; |
darkumatar | 0:da972c3ac4de | 179 | } |
darkumatar | 0:da972c3ac4de | 180 | return true; |
darkumatar | 0:da972c3ac4de | 181 | } |
darkumatar | 0:da972c3ac4de | 182 | |
darkumatar | 0:da972c3ac4de | 183 | int main() { |
darkumatar | 0:da972c3ac4de | 184 | Led1.write(1); |
darkumatar | 0:da972c3ac4de | 185 | Led2.write(0); |
darkumatar | 0:da972c3ac4de | 186 | slave.addCMD(1,limit); |
darkumatar | 0:da972c3ac4de | 187 | slave.addCMD(2,abc1); |
darkumatar | 0:da972c3ac4de | 188 | slave.addCMD(3,abc2); |
darkumatar | 0:da972c3ac4de | 189 | slave.addCMD(4,abc3); |
darkumatar | 0:da972c3ac4de | 190 | slave.addCMD(5,abc4); |
darkumatar | 0:da972c3ac4de | 191 | slave.addCMD(12,abc9); |
darkumatar | 0:da972c3ac4de | 192 | slave.addCMD(13,abc10); |
darkumatar | 0:da972c3ac4de | 193 | slave.addCMD(31,abc11); |
darkumatar | 0:da972c3ac4de | 194 | slave.addCMD(32,abc12); |
darkumatar | 0:da972c3ac4de | 195 | slave.addCMD(33,abc13); |
darkumatar | 0:da972c3ac4de | 196 | slave.addCMD(34,abc14); |
darkumatar | 0:da972c3ac4de | 197 | int i; |
darkumatar | 0:da972c3ac4de | 198 | double x_period=0; |
darkumatar | 0:da972c3ac4de | 199 | double y_period=0; |
darkumatar | 0:da972c3ac4de | 200 | int after[4],before[4],before_parus[4],speed_pwm[4],after_pwm[4],before_pwm[4]; |
darkumatar | 0:da972c3ac4de | 201 | double ca[4],P[4],I[4],D[4],integral[4]; |
darkumatar | 0:da972c3ac4de | 202 | PwmOut motor_p[4]{PwmOut(PB_1),PwmOut(PB_13),PwmOut(PC_9),PwmOut(PB_7)};//モーター宣言 PB_6 PC_8 |
darkumatar | 0:da972c3ac4de | 203 | PwmOut motor_m[4]{PwmOut(PA_11),PwmOut(PB_14),PwmOut(PC_8),PwmOut(PB_6)};//モーター宣言 PB_7 PC_9 |
darkumatar | 0:da972c3ac4de | 204 | //一斉に初期化 |
darkumatar | 0:da972c3ac4de | 205 | for(i=0;i<4;i++){ |
darkumatar | 0:da972c3ac4de | 206 | motor_p[i].period_us(2048); |
darkumatar | 0:da972c3ac4de | 207 | motor_m[i].period_us(2048); |
darkumatar | 0:da972c3ac4de | 208 | target[i]=0; |
darkumatar | 0:da972c3ac4de | 209 | after[i]=0; |
darkumatar | 0:da972c3ac4de | 210 | before[i]=0; |
darkumatar | 0:da972c3ac4de | 211 | ca[i]=0; |
darkumatar | 0:da972c3ac4de | 212 | P[i]=0; |
darkumatar | 0:da972c3ac4de | 213 | I[i]=0; |
darkumatar | 0:da972c3ac4de | 214 | D[i]=0; |
darkumatar | 0:da972c3ac4de | 215 | before_parus[i]=0; |
darkumatar | 0:da972c3ac4de | 216 | integral[i]=0; |
darkumatar | 0:da972c3ac4de | 217 | speed_pwm[i]=0; |
darkumatar | 0:da972c3ac4de | 218 | after_pwm[i]=0; |
darkumatar | 0:da972c3ac4de | 219 | before_pwm[i]=0; |
darkumatar | 0:da972c3ac4de | 220 | } |
darkumatar | 0:da972c3ac4de | 221 | //0.1秒はかる タイマー ネームをスタート |
darkumatar | 0:da972c3ac4de | 222 | Timer name; |
darkumatar | 0:da972c3ac4de | 223 | name.start(); |
darkumatar | 0:da972c3ac4de | 224 | gyro.start(); |
darkumatar | 0:da972c3ac4de | 225 | double angle=0; |
darkumatar | 0:da972c3ac4de | 226 | double goal_angle1=0; |
darkumatar | 0:da972c3ac4de | 227 | Led1.write(1); |
darkumatar | 0:da972c3ac4de | 228 | |
darkumatar | 0:da972c3ac4de | 229 | while(limit_switch==true) { |
darkumatar | 0:da972c3ac4de | 230 | angle=gyro.yaw-90.00000; |
darkumatar | 0:da972c3ac4de | 231 | if(auto_swich==true){ |
darkumatar | 0:da972c3ac4de | 232 | |
darkumatar | 0:da972c3ac4de | 233 | goal_angle1=-1.0*atan((goal_x2-x_period)/(goal_y2-y_period))/M_PI*180.0; |
darkumatar | 0:da972c3ac4de | 234 | |
darkumatar | 0:da972c3ac4de | 235 | 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:da972c3ac4de | 236 | pro3(-test_ang(goal_angle1-gyro.yaw),test_limit(goal_x-x_period),test_limit(goal_y-y_period)); |
darkumatar | 0:da972c3ac4de | 237 | }else if(step>=0&&step<60){ |
darkumatar | 0:da972c3ac4de | 238 | for(i=0;i<4;i++){ |
darkumatar | 0:da972c3ac4de | 239 | target[i]=0; |
darkumatar | 0:da972c3ac4de | 240 | motor_p[i]=0; |
darkumatar | 0:da972c3ac4de | 241 | motor_m[i]=0; |
darkumatar | 0:da972c3ac4de | 242 | } |
darkumatar | 0:da972c3ac4de | 243 | pro3(-test_ang(goal_angle1-gyro.yaw),test_limit(goal_x-x_period),test_limit(goal_y-y_period)); |
darkumatar | 0:da972c3ac4de | 244 | } |
darkumatar | 0:da972c3ac4de | 245 | |
darkumatar | 0:da972c3ac4de | 246 | |
darkumatar | 0:da972c3ac4de | 247 | } |
darkumatar | 0:da972c3ac4de | 248 | for(i=0;i<4;i++){ |
darkumatar | 0:da972c3ac4de | 249 | after_pwm[i]=rote[i].get(); |
darkumatar | 0:da972c3ac4de | 250 | after[i]=v[i].get();//ロリコンから値読み取り |
darkumatar | 0:da972c3ac4de | 251 | integral[i]+= (((((target[i]/10.0-(after[i]-before[i]))+ before_parus[i])*0.1)/2.0)/102.40);//積分の所 |
darkumatar | 0:da972c3ac4de | 252 | P[i]=0.05*(target[i]/10.0-(after[i]-before[i]))/102.40;//比例 |
darkumatar | 0:da972c3ac4de | 253 | D[i]=0.0001*((((target[i]/10.0-(after[i]-before[i]))- before_parus[i])/0.1)/102.40);//微分 |
darkumatar | 0:da972c3ac4de | 254 | I[i]=0.0001*integral[i];//積分 |
darkumatar | 0:da972c3ac4de | 255 | ca[i]=ca[i]+P[i]+I[i]+D[i];//足し合わせる |
darkumatar | 0:da972c3ac4de | 256 | if(ca[i]>0.4){ca[i]=0.4;} |
darkumatar | 0:da972c3ac4de | 257 | if(ca[i]<-0.4){ca[i]=-0.4;} |
darkumatar | 0:da972c3ac4de | 258 | if(0.015>ca[i]&&ca[i]>-0.015){ca[i]=0;} |
darkumatar | 0:da972c3ac4de | 259 | motor_p[i]=ca[i]; |
darkumatar | 0:da972c3ac4de | 260 | motor_m[i]=ca[i]*-1.0; |
darkumatar | 0:da972c3ac4de | 261 | speed_pwm[i]=after_pwm[i]-before_pwm[i]; |
darkumatar | 0:da972c3ac4de | 262 | before_parus[i]=target[i]/10.0-(after[i]-before[i]); |
darkumatar | 0:da972c3ac4de | 263 | before[i]=after[i]; |
darkumatar | 0:da972c3ac4de | 264 | before_pwm[i]=after_pwm[i]; |
darkumatar | 0:da972c3ac4de | 265 | } |
darkumatar | 0:da972c3ac4de | 266 | |
darkumatar | 0:da972c3ac4de | 267 | 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:da972c3ac4de | 268 | 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:da972c3ac4de | 269 | if(liset_abc==true){ |
darkumatar | 0:da972c3ac4de | 270 | x_period=0; |
darkumatar | 0:da972c3ac4de | 271 | y_period=0; |
darkumatar | 0:da972c3ac4de | 272 | gyro.reset(0); |
darkumatar | 0:da972c3ac4de | 273 | |
darkumatar | 0:da972c3ac4de | 274 | } |
darkumatar | 0:da972c3ac4de | 275 | slave.port2.printf("%d\n",1); |
darkumatar | 0:da972c3ac4de | 276 | while(name.read_ms()<30){} |
darkumatar | 0:da972c3ac4de | 277 | slave.send1(255,16,x_period*1); |
darkumatar | 0:da972c3ac4de | 278 | while(name.read_ms()<60){} |
darkumatar | 0:da972c3ac4de | 279 | slave.send1(255,17,y_period*1); |
darkumatar | 0:da972c3ac4de | 280 | while(name.read_ms()<100){} |
darkumatar | 0:da972c3ac4de | 281 | slave.send1(255,18,gyro.yaw*1); |
darkumatar | 0:da972c3ac4de | 282 | for(i = 0; i < 4; i++){ |
darkumatar | 0:da972c3ac4de | 283 | npx.global_scale = 0.05f; |
darkumatar | 0:da972c3ac4de | 284 | npx.normalize = false; |
darkumatar | 0:da972c3ac4de | 285 | if(check_tepu>0){ |
darkumatar | 0:da972c3ac4de | 286 | npx.setPixelColor(i,0xFF0000); |
darkumatar | 0:da972c3ac4de | 287 | }else { |
darkumatar | 0:da972c3ac4de | 288 | npx.setPixelColor(i,0x0000FF); |
darkumatar | 0:da972c3ac4de | 289 | } |
darkumatar | 0:da972c3ac4de | 290 | } |
darkumatar | 0:da972c3ac4de | 291 | for(i = 4; i < 7; i++){ |
darkumatar | 0:da972c3ac4de | 292 | npx.global_scale = 0.05f; |
darkumatar | 0:da972c3ac4de | 293 | npx.normalize = false; |
darkumatar | 0:da972c3ac4de | 294 | if(auto_swich){ |
darkumatar | 0:da972c3ac4de | 295 | npx.setPixelColor(i,0xFFFFFF); |
darkumatar | 0:da972c3ac4de | 296 | }else { |
darkumatar | 0:da972c3ac4de | 297 | npx.setPixelColor(i,0x00FF00); |
darkumatar | 0:da972c3ac4de | 298 | } |
darkumatar | 0:da972c3ac4de | 299 | } |
darkumatar | 0:da972c3ac4de | 300 | npx.show(); |
darkumatar | 0:da972c3ac4de | 301 | if(check_tepu==0){ |
darkumatar | 0:da972c3ac4de | 302 | auto_mode+=1; |
darkumatar | 0:da972c3ac4de | 303 | Led2.write(1); |
darkumatar | 0:da972c3ac4de | 304 | }else { |
darkumatar | 0:da972c3ac4de | 305 | auto_mode=0; |
darkumatar | 0:da972c3ac4de | 306 | Led2.write(0); |
darkumatar | 0:da972c3ac4de | 307 | } |
darkumatar | 0:da972c3ac4de | 308 | if(auto_mode>1){ |
darkumatar | 0:da972c3ac4de | 309 | limit_switch=false; |
darkumatar | 0:da972c3ac4de | 310 | } |
darkumatar | 0:da972c3ac4de | 311 | check_tepu=0; |
darkumatar | 0:da972c3ac4de | 312 | gyro.update(); |
darkumatar | 0:da972c3ac4de | 313 | name.reset(); |
darkumatar | 0:da972c3ac4de | 314 | } |
darkumatar | 0:da972c3ac4de | 315 | while(1){ |
darkumatar | 0:da972c3ac4de | 316 | Led3.write(1); |
darkumatar | 0:da972c3ac4de | 317 | for(i=0;i<4;i++){ |
darkumatar | 0:da972c3ac4de | 318 | target[i]=0; |
darkumatar | 0:da972c3ac4de | 319 | motor_p[i]=0; |
darkumatar | 0:da972c3ac4de | 320 | motor_m[i]=0; |
darkumatar | 0:da972c3ac4de | 321 | npx.global_scale = 0.05f; |
darkumatar | 0:da972c3ac4de | 322 | npx.normalize = false; |
darkumatar | 0:da972c3ac4de | 323 | npx.setPixelColor(i,0x00FFFF); |
darkumatar | 0:da972c3ac4de | 324 | } |
darkumatar | 0:da972c3ac4de | 325 | npx.show(); |
darkumatar | 0:da972c3ac4de | 326 | } |
darkumatar | 0:da972c3ac4de | 327 | } |