go-roboB / aa2_left2

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?

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