ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

main.cpp@83:714fa414b2bf, 2016-05-06 (annotated)

Committer:
IceTeam
Date:
Fri May 06 09:20:49 2016 +0200
Revision:
83:714fa414b2bf
Parent:
82:07e13071dd7b
Child:
84:b54822c913b0
Rajout gestion des couleurs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 81:e7b03e81b025 1 #include "entete.h"
sype 41:b5a2fbc20beb 2
IceTeam 83:714fa414b2bf 3 DigitalIn CAMP(PA_15);
IceTeam 83:714fa414b2bf 4 DigitalIn START(PB_7);
IceTeam 83:714fa414b2bf 5 DigitalOut LEDR(PC_2);
IceTeam 83:714fa414b2bf 6 DigitalOut LEDV(PC_3);
IceTeam 83:714fa414b2bf 7 int SCouleur = VERT;
IceTeam 83:714fa414b2bf 8
IceTeam 80:cd4960dfa47e 9 BusOut drapeau (PC_8, PC_6, PC_5);
sype 46:5658af4e5149 10 RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
IceTeam 80:cd4960dfa47e 11 AnalogIn Rcapt1(PA_4);
IceTeam 80:cd4960dfa47e 12 int RvalRcapt1 = 0;
IceTeam 80:cd4960dfa47e 13 AnalogIn Rcapt2(PB_0);
IceTeam 80:cd4960dfa47e 14 int RvalRcapt2 = 0;
IceTeam 80:cd4960dfa47e 15 AnalogIn Rcapt3(PC_1);
IceTeam 80:cd4960dfa47e 16 int RvalRcapt3 = 0;
IceTeam 80:cd4960dfa47e 17 int Ravance = 1;
IceTeam 81:e7b03e81b025 18 DigitalIn start(PB_7);
IceTeam 80:cd4960dfa47e 19
sype 43:d5aaff7d2bec 20 /* Debut du programme */
sype 0:ad9600df4a70 21 int main(void)
sype 10:ae3178aa94e9 22 {
IceTeam 80:cd4960dfa47e 23 Ticker ticker;
IceTeam 80:cd4960dfa47e 24 ticker.attach(&Sharps, 0.01);
IceTeam 83:714fa414b2bf 25 depart();
IceTeam 83:714fa414b2bf 26 if (SCouleur == VERT) {
IceTeam 83:714fa414b2bf 27 GotoDist(5.5);
IceTeam 83:714fa414b2bf 28 }
IceTeam 83:714fa414b2bf 29 else if (SCouleur == VIOLET) {
IceTeam 83:714fa414b2bf 30 ;
IceTeam 83:714fa414b2bf 31 }
IceTeam 83:714fa414b2bf 32 else {
IceTeam 83:714fa414b2bf 33 ;
IceTeam 83:714fa414b2bf 34 }
IceTeam 83:714fa414b2bf 35
sype 77:f19cc7f81f2a 36 while(1);
sype 2:abdf8c6823a1 37 }
IceTeam 80:cd4960dfa47e 38
IceTeam 80:cd4960dfa47e 39 void Sharps() {
IceTeam 80:cd4960dfa47e 40 if (Rcapt1.read() > R_SEUIL_SHARP) RvalRcapt1++;
IceTeam 80:cd4960dfa47e 41 else RvalRcapt1--;
IceTeam 80:cd4960dfa47e 42 RvalRcapt1 = RvalRcapt1 > 10 ? 10 : RvalRcapt1;
IceTeam 80:cd4960dfa47e 43 RvalRcapt1 = RvalRcapt1 < 0 ? 0 : RvalRcapt1;
IceTeam 80:cd4960dfa47e 44
IceTeam 80:cd4960dfa47e 45 if (Rcapt2.read() > R_SEUIL_SHARP) RvalRcapt2++;
IceTeam 80:cd4960dfa47e 46 else RvalRcapt2--;
IceTeam 80:cd4960dfa47e 47 RvalRcapt2 = RvalRcapt2 > 10 ? 10 : RvalRcapt2;
IceTeam 80:cd4960dfa47e 48 RvalRcapt2 = RvalRcapt2 < 0 ? 0 : RvalRcapt2;
IceTeam 80:cd4960dfa47e 49
IceTeam 80:cd4960dfa47e 50 if (Rcapt3.read() > R_SEUIL_SHARP) RvalRcapt3++;
IceTeam 80:cd4960dfa47e 51 else RvalRcapt3--;
IceTeam 80:cd4960dfa47e 52 RvalRcapt3 = RvalRcapt3 > 10 ? 10 : RvalRcapt3;
IceTeam 80:cd4960dfa47e 53 RvalRcapt3 = RvalRcapt3 < 0 ? 0 : RvalRcapt3;
IceTeam 80:cd4960dfa47e 54
IceTeam 80:cd4960dfa47e 55 if (RvalRcapt1 >= 5 || RvalRcapt2 >= 5 || RvalRcapt3 >= 5)
IceTeam 80:cd4960dfa47e 56 Ravance = 0;
IceTeam 80:cd4960dfa47e 57 else
IceTeam 80:cd4960dfa47e 58 Ravance = 1;
IceTeam 80:cd4960dfa47e 59
IceTeam 80:cd4960dfa47e 60 if (Ravance == 0)
IceTeam 80:cd4960dfa47e 61 drapeau = 1;
IceTeam 80:cd4960dfa47e 62 else
IceTeam 80:cd4960dfa47e 63 drapeau = 2;
IceTeam 81:e7b03e81b025 64 }
IceTeam 81:e7b03e81b025 65
IceTeam 81:e7b03e81b025 66 void GotoDist(float timer) {
IceTeam 81:e7b03e81b025 67 Timer t;
IceTeam 81:e7b03e81b025 68
IceTeam 81:e7b03e81b025 69 roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
IceTeam 81:e7b03e81b025 70 roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
IceTeam 81:e7b03e81b025 71
IceTeam 81:e7b03e81b025 72 t.reset();
IceTeam 81:e7b03e81b025 73 t.start();
IceTeam 81:e7b03e81b025 74
IceTeam 81:e7b03e81b025 75 while (t.read() < timer) {
IceTeam 81:e7b03e81b025 76 if (Ravance != 1) {
IceTeam 81:e7b03e81b025 77 roboclaw.ForwardM1(0);
IceTeam 81:e7b03e81b025 78 roboclaw.ForwardM2(0);
IceTeam 81:e7b03e81b025 79 t.stop();
IceTeam 81:e7b03e81b025 80 while (Ravance != 1);
IceTeam 81:e7b03e81b025 81 roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
IceTeam 81:e7b03e81b025 82 roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
IceTeam 81:e7b03e81b025 83 t.start();
IceTeam 81:e7b03e81b025 84 }
IceTeam 81:e7b03e81b025 85 }
IceTeam 81:e7b03e81b025 86
IceTeam 81:e7b03e81b025 87 roboclaw.ForwardM1(0);
IceTeam 81:e7b03e81b025 88 roboclaw.ForwardM2(0);
IceTeam 81:e7b03e81b025 89
IceTeam 81:e7b03e81b025 90 t.stop();
IceTeam 81:e7b03e81b025 91 t.reset();
IceTeam 81:e7b03e81b025 92
IceTeam 81:e7b03e81b025 93 wait(waiting_time);
IceTeam 81:e7b03e81b025 94 }
IceTeam 81:e7b03e81b025 95
IceTeam 81:e7b03e81b025 96 void GotoThet (float timer, int signe) {
IceTeam 81:e7b03e81b025 97 Timer t;
IceTeam 81:e7b03e81b025 98
IceTeam 81:e7b03e81b025 99 if (signe < 0) {
IceTeam 81:e7b03e81b025 100 roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
IceTeam 81:e7b03e81b025 101 roboclaw.SpeedAccelM2(-accel_dista, -vitesse_dista);
IceTeam 81:e7b03e81b025 102 }
IceTeam 81:e7b03e81b025 103 else {
IceTeam 81:e7b03e81b025 104 roboclaw.SpeedAccelM1(-accel_dista, -vitesse_dista);
IceTeam 81:e7b03e81b025 105 roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
IceTeam 81:e7b03e81b025 106 }
IceTeam 81:e7b03e81b025 107
IceTeam 81:e7b03e81b025 108 t.reset();
IceTeam 81:e7b03e81b025 109 t.start();
IceTeam 81:e7b03e81b025 110
IceTeam 81:e7b03e81b025 111 while (t.read() < timer) {
IceTeam 81:e7b03e81b025 112 if (Ravance != 1) {
IceTeam 81:e7b03e81b025 113 roboclaw.ForwardM1(0);
IceTeam 81:e7b03e81b025 114 roboclaw.ForwardM2(0);
IceTeam 81:e7b03e81b025 115 t.stop();
IceTeam 81:e7b03e81b025 116 while (Ravance != 1);
IceTeam 81:e7b03e81b025 117 t.start();
IceTeam 81:e7b03e81b025 118 }
IceTeam 81:e7b03e81b025 119 if (signe < 0) {
IceTeam 81:e7b03e81b025 120 roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
IceTeam 81:e7b03e81b025 121 roboclaw.SpeedAccelM2(-accel_dista, -vitesse_dista);
IceTeam 81:e7b03e81b025 122 }
IceTeam 81:e7b03e81b025 123 else {
IceTeam 81:e7b03e81b025 124 roboclaw.SpeedAccelM1(-accel_dista, -vitesse_dista);
IceTeam 81:e7b03e81b025 125 roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
IceTeam 81:e7b03e81b025 126 }
IceTeam 81:e7b03e81b025 127 }
IceTeam 81:e7b03e81b025 128
IceTeam 81:e7b03e81b025 129 roboclaw.ForwardM1(0);
IceTeam 81:e7b03e81b025 130 roboclaw.ForwardM2(0);
IceTeam 81:e7b03e81b025 131
IceTeam 81:e7b03e81b025 132 t.stop();
IceTeam 81:e7b03e81b025 133 t.reset();
IceTeam 81:e7b03e81b025 134
IceTeam 81:e7b03e81b025 135 wait(waiting_time);
IceTeam 80:cd4960dfa47e 136 }