ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

deplacement.cpp@94:86b9bd6d5c28, 2016-05-06 (annotated)

Committer:
IceTeam
Date:
Fri May 06 18:25:24 2016 +0000
Revision:
94:86b9bd6d5c28
Parent:
93:c0b040954eac
Child:
96:1e91cac784fe
Save new GotoThet

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_) {
IceTeam 89:46730de0d013 70 roboclaw.ResetEnc();
IceTeam 90:78d2c1527c95 71 wait_ms(10);
IceTeam 89:46730de0d013 72 float diameter_left = 61.7;
IceTeam 89:46730de0d013 73 float diameter_right = 61.8;
IceTeam 89:46730de0d013 74 int distance_ticks_left = -theta_*ENTRAXE/(2*(diameter_left*3.14159/4096));
IceTeam 89:46730de0d013 75 int distance_ticks_right = theta_*ENTRAXE/(2*(diameter_right*3.14159/4096));
IceTeam 89:46730de0d013 76
IceTeam 89:46730de0d013 77 if (theta_ < 0)
IceTeam 94:86b9bd6d5c28 78 roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
IceTeam 89:46730de0d013 79 else
IceTeam 94:86b9bd6d5c28 80 roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
IceTeam 89:46730de0d013 81
IceTeam 94:86b9bd6d5c28 82 wait(3);
IceTeam 89:46730de0d013 83 roboclaw.ForwardM1(0);
IceTeam 89:46730de0d013 84 roboclaw.ForwardM2(0);
IceTeam 94:86b9bd6d5c28 85 }
IceTeam 94:86b9bd6d5c28 86
IceTeam 94:86b9bd6d5c28 87 void Odometry::GotoDistPos(double distance)
IceTeam 94:86b9bd6d5c28 88 {
IceTeam 94:86b9bd6d5c28 89 int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
IceTeam 94:86b9bd6d5c28 90
IceTeam 94:86b9bd6d5c28 91 int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
IceTeam 94:86b9bd6d5c28 92 int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;
IceTeam 94:86b9bd6d5c28 93
IceTeam 94:86b9bd6d5c28 94 roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, avant ? vitesse_dista : 0 - vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, avant ? vitesse_dista : 0 - vitesse_dista, deccel_dista, distance_ticks_left, 1);
IceTeam 94:86b9bd6d5c28 95
IceTeam 94:86b9bd6d5c28 96 //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
IceTeam 94:86b9bd6d5c28 97
IceTeam 94:86b9bd6d5c28 98 while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
IceTeam 94:86b9bd6d5c28 99
IceTeam 94:86b9bd6d5c28 100 wait(0.4);
IceTeam 94:86b9bd6d5c28 101 //logger.printf("arrivey %d\n\r",pos_prog);
IceTeam 94:86b9bd6d5c28 102 //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
IceTeam 94:86b9bd6d5c28 103 logger.printf("End\n\r");
IceTeam 86:0c5e9ac3d8d8 104 }