go-roboB
/
ashimawari_aa2_left
left
Revision 1:1f9a3449bbb9, committed 2022-04-08
- Comitter:
- darkumatar
- Date:
- Fri Apr 08 09:31:13 2022 +0000
- Parent:
- 0:c60fccf1ea71
- Commit message:
- o
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r c60fccf1ea71 -r 1f9a3449bbb9 main.cpp --- a/main.cpp Fri Apr 01 00:20:40 2022 +0000 +++ b/main.cpp Fri Apr 08 09:31:13 2022 +0000 @@ -7,9 +7,9 @@ #define goal_x -17700.0 -#define goal_y -23800.0 +#define goal_y -23800.0//-23800.0 #define goal_x2 -8000000 -#define goal_y2 -8000000 +#define goal_y2 -80000000 #define period_r 8000.0 NeoPixelOut npx(PB_4,7); I2C i2d(PB_3,PB_10); @@ -36,7 +36,7 @@ int check_tepu=0; bool limi=true; - +bool liset_abc=false; DigitalIn limitB(PB_12,PullUp); @@ -102,7 +102,7 @@ return true; } bool pro2(){ //アングルとスピードを同時に決める 下から3桁で角度を決める(0~360) それ以外でスピードを決める これなに?私もわからん - double anglert=-135.000000000; + double anglert=-135.000000000+180.0; Led4.write(1); for(int i=0;i<4;i++){ 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; @@ -216,8 +216,17 @@ check_tepu+=1; return direct_tu(0,a); } +bool abc14(int a,int &b){ + if(a==1){ + liset_abc=true; + }else{ + liset_abc=false; + } + return true; +} + int main() { Led1.write(1); Led2.write(0); @@ -231,7 +240,7 @@ slave.addCMD(31,abc11); slave.addCMD(32,abc12); slave.addCMD(33,abc13); - + slave.addCMD(34,abc14); int i; double x_period=0; double y_period=0; @@ -384,6 +393,12 @@ */ 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); 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); + if(liset_abc==true){ + x_period=0; + y_period=0; + gyro.reset(0); + + } slave.port2.printf("%d\n",1); while(name.read_ms()<30){} slave.send1(255,16,x_period*1);