ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

deplacement.cpp@99:1fcb088f8f36, 2016-05-07 (annotated)

Committer:
Jagang
Date:
Sat May 07 02:15:23 2016 +0000
Revision:
99:1fcb088f8f36
Parent:
98:2426d699362b 
75pts (? tester avec le bras r?par?)

Who changed what in which revision?

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