ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 81:e7b03e81b025 1 #include "entete.h"
IceTeam 92:f09f55aa992b 2
IceTeam 85:38cbf5fd22e1 3 #include "AX12/AX12.h"
IceTeam 93:c0b040954eac 4 #include "ControlleurPince/ControlleurPince.h"
sype 41:b5a2fbc20beb 5
IceTeam 83:714fa414b2bf 6 DigitalIn CAMP(PA_15);
IceTeam 83:714fa414b2bf 7 DigitalIn START(PB_7);
IceTeam 83:714fa414b2bf 8 DigitalOut LEDR(PC_2);
IceTeam 83:714fa414b2bf 9 DigitalOut LEDV(PC_3);
IceTeam 84:b54822c913b0 10 DigitalIn button(USER_BUTTON);
IceTeam 83:714fa414b2bf 11 int SCouleur = VERT;
IceTeam 83:714fa414b2bf 12
IceTeam 80:cd4960dfa47e 13 BusOut drapeau (PC_8, PC_6, PC_5);
sype 46:5658af4e5149 14 RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
IceTeam 93:c0b040954eac 15 DigitalIn start(PB_7);
IceTeam 93:c0b040954eac 16
IceTeam 93:c0b040954eac 17 AX12 Parasol(PA_9, PA_10, 2, 250000);
IceTeam 93:c0b040954eac 18
IceTeam 93:c0b040954eac 19 /* Sharps */
IceTeam 80:cd4960dfa47e 20 AnalogIn Rcapt1(PA_4);
IceTeam 80:cd4960dfa47e 21 int RvalRcapt1 = 0;
IceTeam 80:cd4960dfa47e 22 AnalogIn Rcapt2(PB_0);
IceTeam 80:cd4960dfa47e 23 int RvalRcapt2 = 0;
IceTeam 80:cd4960dfa47e 24 AnalogIn Rcapt3(PC_1);
IceTeam 80:cd4960dfa47e 25 int RvalRcapt3 = 0;
IceTeam 80:cd4960dfa47e 26 int Ravance = 1;
IceTeam 90:78d2c1527c95 27 float R_SEUIL_SHARP = 1;
IceTeam 80:cd4960dfa47e 28
IceTeam 93:c0b040954eac 29 /* Pour la pince */
IceTeam 93:c0b040954eac 30 AX12 left_hand(PA_9, PA_10, 3, 250000);
IceTeam 93:c0b040954eac 31 AX12 right_hand(PA_9, PA_10, 1, 250000);
IceTeam 93:c0b040954eac 32 Stepper RMot(NC, PA_8, PC_7, PB_15, 4);
IceTeam 93:c0b040954eac 33 Stepper ZMot(NC, PB_4, PB_10, PB_14, 5);
IceTeam 93:c0b040954eac 34 Stepper LMot(NC, PB_5, PB_3, PB_13, 4);
IceTeam 93:c0b040954eac 35 DigitalIn EnR(PB_15);
IceTeam 93:c0b040954eac 36 DigitalIn EnZ(PB_14);
IceTeam 93:c0b040954eac 37 DigitalIn EnL(PB_13);
IceTeam 93:c0b040954eac 38
IceTeam 93:c0b040954eac 39 ControlleurPince controlleurPince(RMot, ZMot, LMot, EnR, EnZ, EnL,left_hand,right_hand);
IceTeam 93:c0b040954eac 40
sype 43:d5aaff7d2bec 41 /* Debut du programme */
sype 0:ad9600df4a70 42 int main(void)
sype 10:ae3178aa94e9 43 {
IceTeam 93:c0b040954eac 44 Ticker ticker;
IceTeam 93:c0b040954eac 45 Timeout end;
IceTeam 93:c0b040954eac 46
IceTeam 93:c0b040954eac 47 ticker.attach(&Sharps, 0.01);
IceTeam 93:c0b040954eac 48
IceTeam 93:c0b040954eac 49 init_globals();
IceTeam 91:65fb6b9f3932 50
IceTeam 83:714fa414b2bf 51 if (SCouleur == VERT) {
Jagang 99:1fcb088f8f36 52 // Chateau
IceTeam 91:65fb6b9f3932 53 end.attach(&endFonc, 90);
Jagang 99:1fcb088f8f36 54 GotoDist(9.0,2000,2000,2000);
Jagang 99:1fcb088f8f36 55
Jagang 99:1fcb088f8f36 56 // Callage
Jagang 99:1fcb088f8f36 57 GotoArr(5);
IceTeam 90:78d2c1527c95 58 R_SEUIL_SHARP = 0.25;
Jagang 99:1fcb088f8f36 59
Jagang 99:1fcb088f8f36 60 // Cabane 1
Jagang 99:1fcb088f8f36 61 GotoDist(1.25);
Jagang 98:2426d699362b 62 GotoThet(-PI/2.f);
IceTeam 91:65fb6b9f3932 63 R_SEUIL_SHARP = 0.35;
Jagang 99:1fcb088f8f36 64 GotoDist(2.75);
IceTeam 90:78d2c1527c95 65 R_SEUIL_SHARP = 1;
Jagang 99:1fcb088f8f36 66 controlleurPince.setPos(-1,150);
Jagang 99:1fcb088f8f36 67 GotoDist(2.25);
Jagang 99:1fcb088f8f36 68
Jagang 99:1fcb088f8f36 69 // Cabane 2
Jagang 99:1fcb088f8f36 70 GotoArr(2);
IceTeam 91:65fb6b9f3932 71 R_SEUIL_SHARP = 0.35;
Jagang 98:2426d699362b 72 GotoThet(PI/2.f);
Jagang 99:1fcb088f8f36 73 GotoDist(1.75);
Jagang 99:1fcb088f8f36 74 GotoThet(PI/2.f);
Jagang 99:1fcb088f8f36 75 GotoArr(3);
Jagang 99:1fcb088f8f36 76
Jagang 99:1fcb088f8f36 77 // Coquillage 1
Jagang 99:1fcb088f8f36 78 R_SEUIL_SHARP = 0.35;
IceTeam 90:78d2c1527c95 79 GotoDist(3.5);
Jagang 99:1fcb088f8f36 80 controlleurPince.setPos(0);
Jagang 99:1fcb088f8f36 81 controlleurPince.angle(50);
Jagang 99:1fcb088f8f36 82
Jagang 99:1fcb088f8f36 83 GotoThet(-0.3f);
Jagang 99:1fcb088f8f36 84
Jagang 99:1fcb088f8f36 85 GotoDist(2);
Jagang 99:1fcb088f8f36 86
Jagang 99:1fcb088f8f36 87 controlleurPince.close();
Jagang 99:1fcb088f8f36 88 controlleurPince.setPos(-1,70);
Jagang 99:1fcb088f8f36 89
Jagang 99:1fcb088f8f36 90 // Coquillage 2
Jagang 99:1fcb088f8f36 91 GotoThet(PI/2.f-0.3f);
Jagang 99:1fcb088f8f36 92 R_SEUIL_SHARP = 1;
Jagang 99:1fcb088f8f36 93 controlleurPince.angle(50);
Jagang 99:1fcb088f8f36 94 GotoDist(2);
Jagang 99:1fcb088f8f36 95 controlleurPince.close();
Jagang 99:1fcb088f8f36 96
Jagang 99:1fcb088f8f36 97 // Fin !!!
Jagang 99:1fcb088f8f36 98 GotoThet(PI/2.f);
Jagang 99:1fcb088f8f36 99 GotoDist(1.75);
Jagang 99:1fcb088f8f36 100 controlleurPince.angle(50);
Jagang 99:1fcb088f8f36 101 GotoArr(1);
Jagang 99:1fcb088f8f36 102
IceTeam 83:714fa414b2bf 103 }
Jagang 98:2426d699362b 104 else// if (SCouleur == VIOLET)
Jagang 98:2426d699362b 105 {
Jagang 99:1fcb088f8f36 106
Jagang 99:1fcb088f8f36 107 // Chateau
IceTeam 91:65fb6b9f3932 108 end.attach(&endFonc, 90);
Jagang 98:2426d699362b 109 GotoDist(9.0,2000,2000,2000);
Jagang 99:1fcb088f8f36 110
Jagang 99:1fcb088f8f36 111 // Callage
Jagang 98:2426d699362b 112 GotoArr(5);
IceTeam 90:78d2c1527c95 113 R_SEUIL_SHARP = 0.25;
Jagang 99:1fcb088f8f36 114
Jagang 99:1fcb088f8f36 115 // Cabane 1
Jagang 98:2426d699362b 116 GotoDist(1.25);
Jagang 98:2426d699362b 117 GotoThet(PI/2.f);
IceTeam 91:65fb6b9f3932 118 R_SEUIL_SHARP = 0.35;
Jagang 98:2426d699362b 119 GotoDist(2.75);
IceTeam 90:78d2c1527c95 120 R_SEUIL_SHARP = 1;
Jagang 99:1fcb088f8f36 121 controlleurPince.setPos(-1,150);
Jagang 98:2426d699362b 122 GotoDist(2.25);
Jagang 99:1fcb088f8f36 123
Jagang 99:1fcb088f8f36 124 // Cabane 2
Jagang 98:2426d699362b 125 GotoArr(2);
IceTeam 91:65fb6b9f3932 126 R_SEUIL_SHARP = 0.35;
Jagang 98:2426d699362b 127 GotoThet(-PI/2.f);
Jagang 98:2426d699362b 128 GotoDist(1.75);
Jagang 99:1fcb088f8f36 129 GotoThet(-PI/2.f);
Jagang 99:1fcb088f8f36 130 GotoArr(3);
Jagang 99:1fcb088f8f36 131
Jagang 99:1fcb088f8f36 132 // Coquillage 1
Jagang 99:1fcb088f8f36 133 R_SEUIL_SHARP = 0.35;
Jagang 99:1fcb088f8f36 134 GotoDist(3.5);
Jagang 99:1fcb088f8f36 135 controlleurPince.setPos(0);
Jagang 99:1fcb088f8f36 136 controlleurPince.angle(50);
Jagang 99:1fcb088f8f36 137
Jagang 99:1fcb088f8f36 138 GotoThet(0.3f);
Jagang 99:1fcb088f8f36 139
Jagang 99:1fcb088f8f36 140 GotoDist(2);
Jagang 99:1fcb088f8f36 141
Jagang 99:1fcb088f8f36 142 controlleurPince.close();
Jagang 99:1fcb088f8f36 143 controlleurPince.setPos(-1,70);
Jagang 99:1fcb088f8f36 144
Jagang 99:1fcb088f8f36 145 // Coquillage 2
Jagang 99:1fcb088f8f36 146 GotoThet(-PI/2.f+0.3f);
Jagang 99:1fcb088f8f36 147 R_SEUIL_SHARP = 1;
Jagang 99:1fcb088f8f36 148 controlleurPince.angle(50);
Jagang 99:1fcb088f8f36 149 GotoDist(2);
Jagang 99:1fcb088f8f36 150 controlleurPince.close();
Jagang 99:1fcb088f8f36 151
Jagang 99:1fcb088f8f36 152 // Fin !!!
Jagang 99:1fcb088f8f36 153 GotoThet(-PI/2.f);
Jagang 99:1fcb088f8f36 154 GotoDist(1.75);
Jagang 99:1fcb088f8f36 155 controlleurPince.angle(50);
Jagang 99:1fcb088f8f36 156 GotoArr(1);
IceTeam 83:714fa414b2bf 157 }
Jagang 98:2426d699362b 158 /* else {
Jagang 98:2426d699362b 159 GotoDistPos(200);
sype 97:42167cfeb8d7 160 GotoThet(PI/2.f);
Jagang 98:2426d699362b 161 GotoDistPos(400);
sype 97:42167cfeb8d7 162
sype 97:42167cfeb8d7 163 GotoThet(PI/2.f);
sype 97:42167cfeb8d7 164 GotoThet(PI/2.f);
sype 97:42167cfeb8d7 165
Jagang 98:2426d699362b 166 GotoDistPos(400);
sype 97:42167cfeb8d7 167
sype 97:42167cfeb8d7 168 GotoThet(-PI/2.f);
sype 97:42167cfeb8d7 169
Jagang 98:2426d699362b 170 GotoDistPos(200);
sype 97:42167cfeb8d7 171 GotoThet(PI/2.f);
sype 97:42167cfeb8d7 172 GotoThet(PI/2.f);
Jagang 98:2426d699362b 173 }*/
IceTeam 83:714fa414b2bf 174
sype 77:f19cc7f81f2a 175 while(1);
sype 2:abdf8c6823a1 176 }
IceTeam 80:cd4960dfa47e 177
IceTeam 80:cd4960dfa47e 178 void Sharps() {
IceTeam 80:cd4960dfa47e 179 if (Rcapt1.read() > R_SEUIL_SHARP) RvalRcapt1++;
IceTeam 80:cd4960dfa47e 180 else RvalRcapt1--;
IceTeam 80:cd4960dfa47e 181 RvalRcapt1 = RvalRcapt1 > 10 ? 10 : RvalRcapt1;
IceTeam 80:cd4960dfa47e 182 RvalRcapt1 = RvalRcapt1 < 0 ? 0 : RvalRcapt1;
IceTeam 80:cd4960dfa47e 183
IceTeam 80:cd4960dfa47e 184 if (Rcapt2.read() > R_SEUIL_SHARP) RvalRcapt2++;
IceTeam 80:cd4960dfa47e 185 else RvalRcapt2--;
IceTeam 80:cd4960dfa47e 186 RvalRcapt2 = RvalRcapt2 > 10 ? 10 : RvalRcapt2;
IceTeam 80:cd4960dfa47e 187 RvalRcapt2 = RvalRcapt2 < 0 ? 0 : RvalRcapt2;
IceTeam 80:cd4960dfa47e 188
IceTeam 80:cd4960dfa47e 189 if (Rcapt3.read() > R_SEUIL_SHARP) RvalRcapt3++;
IceTeam 80:cd4960dfa47e 190 else RvalRcapt3--;
IceTeam 80:cd4960dfa47e 191 RvalRcapt3 = RvalRcapt3 > 10 ? 10 : RvalRcapt3;
IceTeam 80:cd4960dfa47e 192 RvalRcapt3 = RvalRcapt3 < 0 ? 0 : RvalRcapt3;
IceTeam 80:cd4960dfa47e 193
IceTeam 90:78d2c1527c95 194 if ((RvalRcapt1 >= 5 || RvalRcapt2 >= 5 || RvalRcapt3 >= 5))
IceTeam 80:cd4960dfa47e 195 Ravance = 0;
IceTeam 80:cd4960dfa47e 196 else
IceTeam 80:cd4960dfa47e 197 Ravance = 1;
IceTeam 80:cd4960dfa47e 198
IceTeam 80:cd4960dfa47e 199 if (Ravance == 0)
IceTeam 80:cd4960dfa47e 200 drapeau = 1;
IceTeam 80:cd4960dfa47e 201 else
IceTeam 80:cd4960dfa47e 202 drapeau = 2;
IceTeam 81:e7b03e81b025 203 }
IceTeam 81:e7b03e81b025 204
IceTeam 86:0c5e9ac3d8d8 205 void endFonc () {
IceTeam 88:e4de39dd3e2e 206 roboclaw.ForwardM1(0);
IceTeam 88:e4de39dd3e2e 207 roboclaw.ForwardM2(0);
IceTeam 88:e4de39dd3e2e 208 wait(1);
Jagang 99:1fcb088f8f36 209 Parasol.setGoal(300);
IceTeam 90:78d2c1527c95 210 wait(1);
Jagang 99:1fcb088f8f36 211 Parasol.setMaxTorque(1000);
IceTeam 88:e4de39dd3e2e 212 while(1);
IceTeam 81:e7b03e81b025 213 }
IceTeam 92:f09f55aa992b 214
IceTeam 93:c0b040954eac 215 void init_globals() {
IceTeam 92:f09f55aa992b 216 roboclaw.ForwardM1(0);
IceTeam 92:f09f55aa992b 217 roboclaw.ForwardM2(0);
IceTeam 92:f09f55aa992b 218
Jagang 99:1fcb088f8f36 219
Jagang 99:1fcb088f8f36 220 controlleurPince.init();
IceTeam 93:c0b040954eac 221 Parasol.setMode(0);
IceTeam 93:c0b040954eac 222 Parasol.setMaxTorque(200);
IceTeam 93:c0b040954eac 223 Parasol.setGoal(150);
Jagang 99:1fcb088f8f36 224 wait(0.5);
IceTeam 93:c0b040954eac 225 Parasol.setGoal(160);
Jagang 99:1fcb088f8f36 226 wait(0.5);
IceTeam 93:c0b040954eac 227 Parasol.setGoal(150);
IceTeam 92:f09f55aa992b 228 wait(1);
IceTeam 93:c0b040954eac 229 Parasol.setMaxTorque(0);
IceTeam 92:f09f55aa992b 230
Jagang 99:1fcb088f8f36 231 controlleurPince.home();
Jagang 99:1fcb088f8f36 232 controlleurPince.setPos(130,0,0);
Jagang 99:1fcb088f8f36 233 controlleurPince.open();
Jagang 99:1fcb088f8f36 234
IceTeam 92:f09f55aa992b 235 while(START == 1)
IceTeam 92:f09f55aa992b 236 {
IceTeam 92:f09f55aa992b 237 LEDR = 1;
IceTeam 92:f09f55aa992b 238 LEDV = 1;
IceTeam 92:f09f55aa992b 239 wait(0.5);
IceTeam 92:f09f55aa992b 240 LEDR = 0;
IceTeam 92:f09f55aa992b 241 LEDV = 0;
IceTeam 92:f09f55aa992b 242 wait(0.5);
IceTeam 92:f09f55aa992b 243 }
IceTeam 92:f09f55aa992b 244
IceTeam 92:f09f55aa992b 245 wait(1);
sype 97:42167cfeb8d7 246 LEDV = 1;
IceTeam 92:f09f55aa992b 247 depart();
IceTeam 92:f09f55aa992b 248 }