Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Revision 20:3f68738c4019, committed 2019-10-11
- Comitter:
- Tom0108
- Date:
- Fri Oct 11 07:29:15 2019 +0000
- Parent:
- 19:340fc4370258
- Commit message:
- Gyro reset measures
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 09 09:00:01 2019 +0000 +++ b/main.cpp Fri Oct 11 07:29:15 2019 +0000 @@ -95,12 +95,14 @@ rawyaw[1]=rawyaw[0]; rawyaw[0]=bno.euler.yaw; - //180~-180をまたいだ時 - if(rawyaw[1]<90 && rawyaw[0]>270) rawyaw[1]+=360; - else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360; + if(rawyaw[0]!=0) { + //180~-180をまたいだ時 + if(rawyaw[1]<90 && rawyaw[0]>270) rawyaw[1]+=360; + else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360; - yaw-=rawyaw[0]-rawyaw[1]; - yaw=fmod(yaw, 360.0); + yaw-=rawyaw[0]-rawyaw[1]; + yaw=fmod(yaw, 360.0); + } //ゾーン対応 if(zone==0) PALETTE(RED); @@ -547,18 +549,18 @@ /* --------------電磁弁系-------------------- */ /*タオルを引っ張るやつ*/ - if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1); - else i2c.Out_Set(Out_Data,4,0); + if(sbdbt.R2)i2c.Out_Set(Out_Data,3,1); + else i2c.Out_Set(Out_Data,3,0); //タオル展開 - if(unfold_f) i2c.Out_Set(Out_Data,3,1); - else i2c.Out_Set(Out_Data,3,0); + if(unfold_f) i2c.Out_Set(Out_Data,5,1); + else i2c.Out_Set(Out_Data,5,0); //エアー解放 if(release_f) { PALETTE(YELLOW); - i2c.Out_Set(Out_Data,5,1); - } else i2c.Out_Set(Out_Data,5,0); + i2c.Out_Set(Out_Data,4,1); + } else i2c.Out_Set(Out_Data,4,0); if(!rod_hit) PALETTE(CYAN); @@ -652,9 +654,9 @@ // pc.printf("yaw:%.1f %.1f",yaw, bno.euler.yaw); // pc.printf("%d %d",keep_tim[0].read_ms(),keep_f[0]); // pc.printf("LX:%d, LY:%d, RX:%d, RY:%d",sbdbt.LX, sbdbt.LY, sbdbt.RX, sbdbt.RY); - for(int i=0; i<9; i++) - pc.printf("%d ",(In_Data+i)->in_data); - pc.printf("\n\r"); +// for(int i=0; i<9; i++) +// pc.printf("%d ",(In_Data+i)->in_data); +// pc.printf("\n\r"); } // while(1) } // int main()
--- a/main.h Wed Oct 09 09:00:01 2019 +0000 +++ b/main.h Fri Oct 11 07:29:15 2019 +0000 @@ -82,9 +82,9 @@ {0,0,0}, // 腕の上下 {0,1,0}, // 爪 {0,2,0}, // タオルの先 - {0,3,0}, // タオル展開 {1,0,0}, // タオルの中央 {1,1,0}, // 全体 + {1,2,0}, // タオル展開 {41,0,0}, // g {41,1,0}, // r {41,2,0}, // b;