ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

main.cpp@85:38cbf5fd22e1, 2016-05-06 (annotated)

Committer:
IceTeam
Date:
Fri May 06 09:49:01 2016 +0200
Revision:
85:38cbf5fd22e1
Parent:
84:b54822c913b0
Child:
86:0c5e9ac3d8d8
Rajout des AX12

Who changed what in which revision?

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