ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

main.cpp@100:7c7174048302, 2016-05-31 (annotated)

Committer:
IceTeam
Date:
Tue May 31 14:24:48 2016 +0000
Revision:
100:7c7174048302
Parent:
99:1fcb088f8f36 
Bug

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 100:7c7174048302 40 int SIMON_state = 's';
IceTeam 100:7c7174048302 41 Serial logger(PA_2, PA_3);
IceTeam 100:7c7174048302 42
IceTeam 100:7c7174048302 43 void JPO(void);
IceTeam 93:c0b040954eac 44
sype 43:d5aaff7d2bec 45 /* Debut du programme */
sype 0:ad9600df4a70 46 int main(void)
sype 10:ae3178aa94e9 47 {
IceTeam 100:7c7174048302 48
IceTeam 93:c0b040954eac 49 init_globals();
IceTeam 91:65fb6b9f3932 50
IceTeam 100:7c7174048302 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);
IceTeam 100:7c7174048302 101 GotoArr(1);*/
IceTeam 100:7c7174048302 102
IceTeam 100:7c7174048302 103 R_SEUIL_SHARP = 0.35;
IceTeam 100:7c7174048302 104 drapeau = 3;
IceTeam 100:7c7174048302 105 while (1)
IceTeam 100:7c7174048302 106 JPO();
IceTeam 83:714fa414b2bf 107
sype 77:f19cc7f81f2a 108 while(1);
sype 2:abdf8c6823a1 109 }
IceTeam 80:cd4960dfa47e 110
IceTeam 80:cd4960dfa47e 111 void Sharps() {
IceTeam 80:cd4960dfa47e 112 if (Rcapt1.read() > R_SEUIL_SHARP) RvalRcapt1++;
IceTeam 80:cd4960dfa47e 113 else RvalRcapt1--;
IceTeam 80:cd4960dfa47e 114 RvalRcapt1 = RvalRcapt1 > 10 ? 10 : RvalRcapt1;
IceTeam 80:cd4960dfa47e 115 RvalRcapt1 = RvalRcapt1 < 0 ? 0 : RvalRcapt1;
IceTeam 80:cd4960dfa47e 116
IceTeam 80:cd4960dfa47e 117 if (Rcapt2.read() > R_SEUIL_SHARP) RvalRcapt2++;
IceTeam 80:cd4960dfa47e 118 else RvalRcapt2--;
IceTeam 80:cd4960dfa47e 119 RvalRcapt2 = RvalRcapt2 > 10 ? 10 : RvalRcapt2;
IceTeam 80:cd4960dfa47e 120 RvalRcapt2 = RvalRcapt2 < 0 ? 0 : RvalRcapt2;
IceTeam 80:cd4960dfa47e 121
IceTeam 80:cd4960dfa47e 122 if (Rcapt3.read() > R_SEUIL_SHARP) RvalRcapt3++;
IceTeam 80:cd4960dfa47e 123 else RvalRcapt3--;
IceTeam 80:cd4960dfa47e 124 RvalRcapt3 = RvalRcapt3 > 10 ? 10 : RvalRcapt3;
IceTeam 80:cd4960dfa47e 125 RvalRcapt3 = RvalRcapt3 < 0 ? 0 : RvalRcapt3;
IceTeam 80:cd4960dfa47e 126
IceTeam 90:78d2c1527c95 127 if ((RvalRcapt1 >= 5 || RvalRcapt2 >= 5 || RvalRcapt3 >= 5))
IceTeam 80:cd4960dfa47e 128 Ravance = 0;
IceTeam 80:cd4960dfa47e 129 else
IceTeam 80:cd4960dfa47e 130 Ravance = 1;
IceTeam 80:cd4960dfa47e 131
IceTeam 80:cd4960dfa47e 132 if (Ravance == 0)
IceTeam 80:cd4960dfa47e 133 drapeau = 1;
IceTeam 80:cd4960dfa47e 134 else
IceTeam 80:cd4960dfa47e 135 drapeau = 2;
IceTeam 81:e7b03e81b025 136 }
IceTeam 81:e7b03e81b025 137
IceTeam 86:0c5e9ac3d8d8 138 void endFonc () {
IceTeam 88:e4de39dd3e2e 139 roboclaw.ForwardM1(0);
IceTeam 88:e4de39dd3e2e 140 roboclaw.ForwardM2(0);
IceTeam 88:e4de39dd3e2e 141 wait(1);
Jagang 99:1fcb088f8f36 142 Parasol.setGoal(300);
IceTeam 90:78d2c1527c95 143 wait(1);
Jagang 99:1fcb088f8f36 144 Parasol.setMaxTorque(1000);
IceTeam 88:e4de39dd3e2e 145 while(1);
IceTeam 81:e7b03e81b025 146 }
IceTeam 92:f09f55aa992b 147
IceTeam 93:c0b040954eac 148 void init_globals() {
IceTeam 92:f09f55aa992b 149 roboclaw.ForwardM1(0);
IceTeam 92:f09f55aa992b 150 roboclaw.ForwardM2(0);
IceTeam 92:f09f55aa992b 151
Jagang 99:1fcb088f8f36 152
Jagang 99:1fcb088f8f36 153 controlleurPince.init();
IceTeam 93:c0b040954eac 154 Parasol.setMode(0);
IceTeam 93:c0b040954eac 155 Parasol.setMaxTorque(200);
IceTeam 93:c0b040954eac 156 Parasol.setGoal(150);
Jagang 99:1fcb088f8f36 157 wait(0.5);
IceTeam 93:c0b040954eac 158 Parasol.setGoal(160);
Jagang 99:1fcb088f8f36 159 wait(0.5);
IceTeam 93:c0b040954eac 160 Parasol.setGoal(150);
IceTeam 92:f09f55aa992b 161 wait(1);
IceTeam 93:c0b040954eac 162 Parasol.setMaxTorque(0);
IceTeam 92:f09f55aa992b 163
Jagang 99:1fcb088f8f36 164 controlleurPince.home();
Jagang 99:1fcb088f8f36 165 controlleurPince.setPos(130,0,0);
Jagang 99:1fcb088f8f36 166 controlleurPince.open();
Jagang 99:1fcb088f8f36 167
IceTeam 92:f09f55aa992b 168 while(START == 1)
IceTeam 92:f09f55aa992b 169 {
IceTeam 92:f09f55aa992b 170 LEDR = 1;
IceTeam 92:f09f55aa992b 171 LEDV = 1;
IceTeam 92:f09f55aa992b 172 wait(0.5);
IceTeam 92:f09f55aa992b 173 LEDR = 0;
IceTeam 92:f09f55aa992b 174 LEDV = 0;
IceTeam 92:f09f55aa992b 175 wait(0.5);
IceTeam 92:f09f55aa992b 176 }
IceTeam 92:f09f55aa992b 177
IceTeam 92:f09f55aa992b 178 wait(1);
sype 97:42167cfeb8d7 179 LEDV = 1;
IceTeam 92:f09f55aa992b 180 depart();
IceTeam 92:f09f55aa992b 181 }
IceTeam 100:7c7174048302 182
IceTeam 100:7c7174048302 183
IceTeam 100:7c7174048302 184 void JPO(void)
IceTeam 100:7c7174048302 185 {
IceTeam 100:7c7174048302 186 char c = logger.getc();
IceTeam 100:7c7174048302 187 printf("Coucou :)\n\r");
IceTeam 100:7c7174048302 188 if(c=='z') {
IceTeam 100:7c7174048302 189 if (SIMON_state != 1) {
IceTeam 100:7c7174048302 190 SIMON_state = 1;
IceTeam 100:7c7174048302 191 logger.printf("Avant (Z) \r\n");
IceTeam 100:7c7174048302 192 roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
IceTeam 100:7c7174048302 193 roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
IceTeam 100:7c7174048302 194 }
IceTeam 100:7c7174048302 195 } else if(c == 's') {
IceTeam 100:7c7174048302 196 if (SIMON_state != 2) {
IceTeam 100:7c7174048302 197 SIMON_state = 2;
IceTeam 100:7c7174048302 198 logger.printf("Stop (S) \r\n");
IceTeam 100:7c7174048302 199 roboclaw.SpeedAccelM1(accel_angle, 0);
IceTeam 100:7c7174048302 200 roboclaw.SpeedAccelM2(accel_angle, 0);
IceTeam 100:7c7174048302 201 }
IceTeam 100:7c7174048302 202 } else if(c == 'd') {
IceTeam 100:7c7174048302 203 if (SIMON_state != 3) {
IceTeam 100:7c7174048302 204 SIMON_state = 3;
IceTeam 100:7c7174048302 205 logger.printf("Droite (D) \r\n");
IceTeam 100:7c7174048302 206 roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
IceTeam 100:7c7174048302 207 roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
IceTeam 100:7c7174048302 208 }
IceTeam 100:7c7174048302 209 } else if(c == 'q') {
IceTeam 100:7c7174048302 210 if (SIMON_state != 4) {
IceTeam 100:7c7174048302 211 SIMON_state = 4;
IceTeam 100:7c7174048302 212 logger.printf("Gauche (Q)\r\n");
IceTeam 100:7c7174048302 213 roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
IceTeam 100:7c7174048302 214 roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
IceTeam 100:7c7174048302 215 }
IceTeam 100:7c7174048302 216 } else if(c == 'x') {
IceTeam 100:7c7174048302 217 if (SIMON_state != 5) {
IceTeam 100:7c7174048302 218 SIMON_state = 5;
IceTeam 100:7c7174048302 219 roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
IceTeam 100:7c7174048302 220 roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
IceTeam 100:7c7174048302 221 }
IceTeam 100:7c7174048302 222 } else if (c == 'c') {
IceTeam 100:7c7174048302 223 if (SIMON_state != 6) {
IceTeam 100:7c7174048302 224 SIMON_state = 6;
IceTeam 100:7c7174048302 225
IceTeam 100:7c7174048302 226 }
IceTeam 100:7c7174048302 227 } else if (c == 'o') {
IceTeam 100:7c7174048302 228 if (SIMON_state != 7) {
IceTeam 100:7c7174048302 229 SIMON_state = 7;
IceTeam 100:7c7174048302 230 }
IceTeam 100:7c7174048302 231 } else if (c == 'h') {
IceTeam 100:7c7174048302 232 if (SIMON_state != 8) {
IceTeam 100:7c7174048302 233 SIMON_state = 8;
IceTeam 100:7c7174048302 234 }
IceTeam 100:7c7174048302 235 } else if (c == 'b') {
IceTeam 100:7c7174048302 236 if (SIMON_state != 9) {
IceTeam 100:7c7174048302 237 SIMON_state = 9;
IceTeam 100:7c7174048302 238 }
IceTeam 100:7c7174048302 239 }
IceTeam 100:7c7174048302 240 else {
IceTeam 100:7c7174048302 241 if (SIMON_state != 0) {
IceTeam 100:7c7174048302 242 roboclaw.SpeedAccelM1M2(accel_angle, 0, 0);
IceTeam 100:7c7174048302 243 SIMON_state = 0;
IceTeam 100:7c7174048302 244 }
IceTeam 100:7c7174048302 245 }
IceTeam 100:7c7174048302 246 }