ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

deplacement.cpp@97:42167cfeb8d7, 2016-05-06 (annotated)

Committer:
sype
Date:
Fri May 06 22:15:43 2016 +0000
Revision:
97:42167cfeb8d7
Parent:
96:1e91cac784fe
Child:
98:2426d699362b
update gotoDistPos

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 86:0c5e9ac3d8d8 1 #include "entete.h"
IceTeam 86:0c5e9ac3d8d8 2
IceTeam 86:0c5e9ac3d8d8 3 void GotoDist(float timer) {
IceTeam 86:0c5e9ac3d8d8 4 Timer t;
IceTeam 86:0c5e9ac3d8d8 5
IceTeam 86:0c5e9ac3d8d8 6 roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
IceTeam 86:0c5e9ac3d8d8 7 roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
IceTeam 86:0c5e9ac3d8d8 8
IceTeam 86:0c5e9ac3d8d8 9 t.reset();
IceTeam 86:0c5e9ac3d8d8 10 t.start();
IceTeam 86:0c5e9ac3d8d8 11
IceTeam 86:0c5e9ac3d8d8 12 while (t.read() < timer) {
IceTeam 86:0c5e9ac3d8d8 13 if (Ravance != 1) {
IceTeam 86:0c5e9ac3d8d8 14 roboclaw.ForwardM1(0);
IceTeam 86:0c5e9ac3d8d8 15 roboclaw.ForwardM2(0);
IceTeam 93:c0b040954eac 16 wait_ms(1);
IceTeam 86:0c5e9ac3d8d8 17 t.stop();
IceTeam 86:0c5e9ac3d8d8 18 while (Ravance != 1);
IceTeam 93:c0b040954eac 19 wait_ms(1);
IceTeam 86:0c5e9ac3d8d8 20 roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
IceTeam 86:0c5e9ac3d8d8 21 roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
IceTeam 86:0c5e9ac3d8d8 22 t.start();
IceTeam 86:0c5e9ac3d8d8 23 }
IceTeam 86:0c5e9ac3d8d8 24 }
IceTeam 86:0c5e9ac3d8d8 25
IceTeam 86:0c5e9ac3d8d8 26 roboclaw.ForwardM1(0);
IceTeam 86:0c5e9ac3d8d8 27 roboclaw.ForwardM2(0);
IceTeam 86:0c5e9ac3d8d8 28
IceTeam 86:0c5e9ac3d8d8 29 t.stop();
IceTeam 86:0c5e9ac3d8d8 30 t.reset();
IceTeam 86:0c5e9ac3d8d8 31
IceTeam 86:0c5e9ac3d8d8 32 wait(waiting_time);
IceTeam 86:0c5e9ac3d8d8 33 }
IceTeam 86:0c5e9ac3d8d8 34
IceTeam 88:e4de39dd3e2e 35 void GotoArr(float timer) {
IceTeam 88:e4de39dd3e2e 36 Timer t;
IceTeam 88:e4de39dd3e2e 37
IceTeam 88:e4de39dd3e2e 38 roboclaw.SpeedAccelM1(accel_dista, -vitesse_dista);
IceTeam 88:e4de39dd3e2e 39 roboclaw.SpeedAccelM2(accel_dista, -vitesse_dista);
IceTeam 93:c0b040954eac 40 wait_ms(1);
IceTeam 88:e4de39dd3e2e 41
IceTeam 88:e4de39dd3e2e 42 t.reset();
IceTeam 88:e4de39dd3e2e 43 t.start();
IceTeam 88:e4de39dd3e2e 44
IceTeam 88:e4de39dd3e2e 45 while (t.read() < timer) {
IceTeam 88:e4de39dd3e2e 46 if (Ravance != 1) {
IceTeam 88:e4de39dd3e2e 47 roboclaw.ForwardM1(0);
IceTeam 93:c0b040954eac 48 wait_ms(1);
IceTeam 88:e4de39dd3e2e 49 roboclaw.ForwardM2(0);
IceTeam 88:e4de39dd3e2e 50 t.stop();
IceTeam 88:e4de39dd3e2e 51 while (Ravance != 1);
IceTeam 93:c0b040954eac 52 wait_ms(1);
IceTeam 93:c0b040954eac 53 roboclaw.SpeedAccelM1(accel_dista, -vitesse_dista);
IceTeam 93:c0b040954eac 54 roboclaw.SpeedAccelM2(accel_dista, -vitesse_dista);
IceTeam 88:e4de39dd3e2e 55 t.start();
IceTeam 88:e4de39dd3e2e 56 }
IceTeam 88:e4de39dd3e2e 57 }
IceTeam 88:e4de39dd3e2e 58
IceTeam 88:e4de39dd3e2e 59 roboclaw.ForwardM1(0);
IceTeam 93:c0b040954eac 60 wait_ms(1);
IceTeam 88:e4de39dd3e2e 61 roboclaw.ForwardM2(0);
IceTeam 88:e4de39dd3e2e 62
IceTeam 88:e4de39dd3e2e 63 t.stop();
IceTeam 88:e4de39dd3e2e 64 t.reset();
IceTeam 88:e4de39dd3e2e 65
IceTeam 88:e4de39dd3e2e 66 wait(waiting_time);
IceTeam 89:46730de0d013 67 }
IceTeam 89:46730de0d013 68
IceTeam 89:46730de0d013 69 void GotoThet(double theta_) {
sype 97:42167cfeb8d7 70 wait_ms(10);
IceTeam 89:46730de0d013 71 roboclaw.ResetEnc();
IceTeam 90:78d2c1527c95 72 wait_ms(10);
IceTeam 96:1e91cac784fe 73 int distance_ticks_left = -theta_*ENTRAXE/(2*(DIAMETRE_ROUE_GAUCHE*3.14159/4096));
IceTeam 96:1e91cac784fe 74 int distance_ticks_right = theta_*ENTRAXE/(2*(DIAMETRE_ROUE_DROITE*3.14159/4096));
IceTeam 89:46730de0d013 75
IceTeam 89:46730de0d013 76 if (theta_ < 0)
IceTeam 94:86b9bd6d5c28 77 roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
IceTeam 89:46730de0d013 78 else
IceTeam 94:86b9bd6d5c28 79 roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
IceTeam 89:46730de0d013 80
IceTeam 94:86b9bd6d5c28 81 wait(3);
IceTeam 89:46730de0d013 82 roboclaw.ForwardM1(0);
sype 97:42167cfeb8d7 83 wait_ms(10);
IceTeam 89:46730de0d013 84 roboclaw.ForwardM2(0);
sype 97:42167cfeb8d7 85 wait_ms(10);
IceTeam 94:86b9bd6d5c28 86 }
IceTeam 94:86b9bd6d5c28 87
IceTeam 96:1e91cac784fe 88 void GotoDistPos(double distance)
IceTeam 94:86b9bd6d5c28 89 {
sype 97:42167cfeb8d7 90 wait_ms(10);
IceTeam 96:1e91cac784fe 91 roboclaw.ResetEnc();
sype 97:42167cfeb8d7 92 wait_ms(10);
IceTeam 96:1e91cac784fe 93 int32_t distance_ticks_left = distance/((DIAMETRE_ROUE_GAUCHE*3.14156)/4096);
IceTeam 96:1e91cac784fe 94 int32_t distance_ticks_right = distance/((DIAMETRE_ROUE_GAUCHE*3.14156)/4096);
IceTeam 94:86b9bd6d5c28 95
IceTeam 96:1e91cac784fe 96 roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
sype 97:42167cfeb8d7 97
sype 97:42167cfeb8d7 98 wait_ms(10);
sype 97:42167cfeb8d7 99 int EncM1 = roboclaw.ReadEncM1();
sype 97:42167cfeb8d7 100 wait_ms(10);
sype 97:42167cfeb8d7 101 int EncM2 = roboclaw.ReadEncM2();
sype 97:42167cfeb8d7 102
sype 97:42167cfeb8d7 103
sype 97:42167cfeb8d7 104 while(abs(EncM1 - distance_ticks_right) > 20 || abs(EncM2 - distance_ticks_left) > 20)
sype 97:42167cfeb8d7 105 {
sype 97:42167cfeb8d7 106 wait_ms(10);
sype 97:42167cfeb8d7 107 EncM1 = roboclaw.ReadEncM1();
sype 97:42167cfeb8d7 108 wait_ms(10);
sype 97:42167cfeb8d7 109 EncM2 = roboclaw.ReadEncM2();
sype 97:42167cfeb8d7 110 }
IceTeam 94:86b9bd6d5c28 111
IceTeam 96:1e91cac784fe 112 wait(0.1);
sype 97:42167cfeb8d7 113 roboclaw.ForwardM1(0);
sype 97:42167cfeb8d7 114 wait_ms(10);
sype 97:42167cfeb8d7 115 roboclaw.ForwardM2(0);
sype 97:42167cfeb8d7 116 wait_ms(10);
IceTeam 86:0c5e9ac3d8d8 117 }