go-roboB / Mbed 2 deprecated aa

a

Dependencies:   mbed SBDBT arrc_mbed

main.cpp@1:0867d6d1421a, 2022-03-12 (annotated)

Committer:
darkumatar
Date:
Sat Mar 12 01:21:56 2022 +0000
Revision:
1:0867d6d1421a
Parent:
0:e22d70b99d51 
a

Who changed what in which revision?

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