Alle functies van de motoren werken
Dependencies: MODSERIAL QEI mbed
Fork of worknotjet by
Diff: main.cpp
- Revision:
- 7:c7bdeee1bbac
- Parent:
- 6:4e4df2f6157e
- Child:
- 8:8a3ab396853f
--- a/main.cpp Fri Oct 21 08:56:33 2016 +0000 +++ b/main.cpp Fri Oct 21 10:28:27 2016 +0000 @@ -30,8 +30,17 @@ bool draailinks; bool turn = 0; float waiter = 0.1; -float afstand = 150; +float afstand = 50; float translation = 0; +float degrees3 = 0; + +float Puls_degree = (8400/360); +float wheel1 = 16; +float wheel2 = 31; +float wheel3 = 41; +float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1)); +float pi = 3.14159265359; + void GetDirections() { pc.baud(115200); @@ -74,39 +83,69 @@ wait(2*waiter); } -void GetPosition() +float GetPositionM2() +{ + float pulses2 = motor2.getPulses(); + float degrees2 = (pulses2/Puls_degree); + float radians2 = (degrees2/360)*2*pi; + float translation = ((radians2/overbrenging)*32.25); + + return translation; +} +float GetRotationM3() { - float pulses = motor2.getPulses(); - float Puls_degree = (8400/360); - float degrees = (pulses/Puls_degree); - float wheel1 = 16; - float wheel2 = 31; - float wheel3 = 41; - float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1)); - float pi = 3.14159265359; - float radians = (degrees/360)*2*pi; - float translation = ((radians/overbrenging)*32.25); - pc.printf("translation %f , degrees %f, radians %f, pulses %f \n \r ",translation, degrees, radians, pulses); + float pulses3 = motor3.getPulses(); + float degrees3 = (pulses3/Puls_degree); + float radians3 = (degrees3/360)*2*pi; + + return degrees3; +} +void GoBack() +{ + while (GetPositionM2() > 0) { + + M2_Speed = 0.8; + M2_Rotate = 0; + pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); + } + M2_Speed = 0; + while (GetRotationM3 < 0) { + M3_Rotate = 1; + M3_Speed = 0.2; + + } + M3_Speed = 0; + + turn = 0; + } void Burgerflip() { - M2_Speed = 0.5; - M2_Rotate = 1; - M3_Speed = 0.5; - M3_Rotate = 1; + if (GetPositionM2() > afstand) { + M3_Speed = 0.2; + M3_Rotate = 0; + M2_Speed = 0; + } else if (GetPositionM2() < afstand) { + M2_Speed = 0.8; + M2_Rotate = 1; + } + if (GetRotationM3() > 75) { + GoBack(); + } +} -} int main() { while (true) { + GetDirections(); if (draairechts == true) { - M1_Speed = 0.5; + M1_Speed = 0.2; M1_Rotate = 0; } else if (draailinks == true) { - M1_Speed = 0.5; + M1_Speed = 0.2; M1_Rotate = 1; } else if (turn == 1) { /*M2_Speed = 0.5; @@ -121,7 +160,7 @@ if ((draailinks == false) && (draairechts == false)) { M1_Speed = 0; } - GetPosition(); + pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); /* pulses = 8400 */ } }