ouais ok
Dependents: roboticLab_withclass_3_July
Fork of ISR_Mini-explorer by
Diff: robot.h
- Revision:
- 2:92ca9a6a1d4e
- Parent:
- 1:b81277897a5b
- Child:
- 3:b996cbe5dabd
diff -r b81277897a5b -r 92ca9a6a1d4e robot.h --- a/robot.h Fri Jul 07 11:49:22 2017 +0000 +++ b/robot.h Tue Jul 11 16:54:58 2017 +0000 @@ -51,6 +51,9 @@ int IRobot=0; int JRobot=0; +float D_cm=0; +float L_cm=0; + //SAIDAS DIGITAIS (normal) DigitalOut q_pha_mot_rig (PTE4,0); //Phase Motor Right DigitalOut q_sleep_mot_rig (PTE3,0); //Nano Sleep Motor Right @@ -1004,24 +1007,25 @@ { long int ticks1d=R_encoder(); long int ticks1e=L_encoder(); - + //pc.printf("\r\nticks1d:%f",ticks1d); + //pc.printf("\r\nticks1e:%f",ticks1d); long int D_ticks=ticks1d - ticks2d; long int E_ticks=ticks1e - ticks2e; - + //pc.printf("\r\nD_ticks:%f",D_ticks); + //pc.printf("\r\nE_ticks:%f",E_ticks); ticks2d=ticks1d; ticks2e=ticks1e; - float D_cm= (float)D_ticks*((3.25*3.1415)/4096); - float L_cm= (float)E_ticks*((3.25*3.1415)/4096); - + D_cm= (float)D_ticks*((3.25*3.1415)/4096); + L_cm= (float)E_ticks*((3.25*3.1415)/4096); + //pc.printf("\r\nD_cm:%f",D_cm); + //pc.printf("\r\nL_cm:%f",L_cm); float CM=(D_cm + L_cm)/2; - + //pc.printf("\r\nCM:%f",CM); theta +=(D_cm - L_cm)/7.18; - theta = atan2(sin(theta), cos(theta)); // meter entre 0 - X += CM*cos(theta); Y += CM*sin(theta); @@ -1038,7 +1042,20 @@ ticks2d=ticks1d; ticks2e=ticks1e; - *R_cm= (float)D_ticks*((3.25*3.1415)/4096); - *L_cm= (float)E_ticks*((3.25*3.1415)/4096); + float D_cm= (float)D_ticks*((3.25*3.1415)/4096); + float E_cm= (float)E_ticks*((3.25*3.1415)/4096); + + float CM=(D_cm + E_cm)/2; + + theta +=(D_cm - E_cm)/7.18; + + theta = atan2(sin(theta), cos(theta)); + // meter entre 0 + + X += CM*cos(theta); + Y += CM*sin(theta); + + *R_cm= D_cm; + *L_cm= E_cm; } \ No newline at end of file