![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |