ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

main.cpp@97:42167cfeb8d7, 2016-05-06 (annotated)

Committer:
sype
Date:
Fri May 06 22:15:43 2016 +0000
Revision:
97:42167cfeb8d7
Parent:
94:86b9bd6d5c28
Child:
98:2426d699362b
update gotoDistPos

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) {
IceTeam 91:65fb6b9f3932 52 end.attach(&endFonc, 90);
IceTeam 90:78d2c1527c95 53 GotoDist(9.0);
IceTeam 90:78d2c1527c95 54 GotoArr(9.2);
IceTeam 90:78d2c1527c95 55 R_SEUIL_SHARP = 0.25;
IceTeam 90:78d2c1527c95 56 GotoDist(2.5);
IceTeam 91:65fb6b9f3932 57 GotoThet(-3.04);
IceTeam 91:65fb6b9f3932 58 R_SEUIL_SHARP = 0.35;
IceTeam 91:65fb6b9f3932 59 GotoDist(5.5);
IceTeam 90:78d2c1527c95 60 R_SEUIL_SHARP = 1;
IceTeam 91:65fb6b9f3932 61 GotoDist(4.5);
IceTeam 91:65fb6b9f3932 62 GotoArr(3);
IceTeam 91:65fb6b9f3932 63 R_SEUIL_SHARP = 0.35;
IceTeam 91:65fb6b9f3932 64 GotoThet(3.04);
IceTeam 90:78d2c1527c95 65 GotoDist(3.5);
IceTeam 91:65fb6b9f3932 66 GotoThet(-3.04);
IceTeam 91:65fb6b9f3932 67 R_SEUIL_SHARP = 1;
IceTeam 93:c0b040954eac 68 GotoDist(4.6);
IceTeam 83:714fa414b2bf 69 }
IceTeam 86:0c5e9ac3d8d8 70 else if (SCouleur == VIOLET) {
IceTeam 91:65fb6b9f3932 71 end.attach(&endFonc, 90);
IceTeam 90:78d2c1527c95 72 GotoDist(9.0);
IceTeam 90:78d2c1527c95 73 GotoArr(9.2);
IceTeam 90:78d2c1527c95 74 R_SEUIL_SHARP = 0.25;
IceTeam 90:78d2c1527c95 75 GotoDist(2.5);
IceTeam 91:65fb6b9f3932 76 GotoThet(3.04);
IceTeam 91:65fb6b9f3932 77 R_SEUIL_SHARP = 0.35;
IceTeam 91:65fb6b9f3932 78 GotoDist(5.5);
IceTeam 90:78d2c1527c95 79 R_SEUIL_SHARP = 1;
IceTeam 91:65fb6b9f3932 80 GotoDist(4.5);
IceTeam 91:65fb6b9f3932 81 GotoArr(4);
IceTeam 91:65fb6b9f3932 82 R_SEUIL_SHARP = 0.35;
IceTeam 91:65fb6b9f3932 83 GotoThet(-3.04);
IceTeam 90:78d2c1527c95 84 GotoDist(3.5);
IceTeam 91:65fb6b9f3932 85 GotoThet(3.04);
IceTeam 91:65fb6b9f3932 86 R_SEUIL_SHARP = 1;
IceTeam 93:c0b040954eac 87 GotoDist(4.6);
IceTeam 83:714fa414b2bf 88 }
sype 97:42167cfeb8d7 89 else {
sype 97:42167cfeb8d7 90 GotoDistPos(100);
sype 97:42167cfeb8d7 91 GotoThet(PI/2.f);
sype 97:42167cfeb8d7 92 GotoDistPos(200);
sype 97:42167cfeb8d7 93
sype 97:42167cfeb8d7 94 GotoThet(PI/2.f);
sype 97:42167cfeb8d7 95 GotoThet(PI/2.f);
sype 97:42167cfeb8d7 96
sype 97:42167cfeb8d7 97 GotoDistPos(200);
sype 97:42167cfeb8d7 98
sype 97:42167cfeb8d7 99 GotoThet(-PI/2.f);
sype 97:42167cfeb8d7 100
sype 97:42167cfeb8d7 101 GotoDistPos(100);
sype 97:42167cfeb8d7 102 GotoThet(PI/2.f);
sype 97:42167cfeb8d7 103 GotoThet(PI/2.f);
IceTeam 83:714fa414b2bf 104 }
IceTeam 83:714fa414b2bf 105
sype 77:f19cc7f81f2a 106 while(1);
sype 2:abdf8c6823a1 107 }
IceTeam 80:cd4960dfa47e 108
IceTeam 80:cd4960dfa47e 109 void Sharps() {
IceTeam 80:cd4960dfa47e 110 if (Rcapt1.read() > R_SEUIL_SHARP) RvalRcapt1++;
IceTeam 80:cd4960dfa47e 111 else RvalRcapt1--;
IceTeam 80:cd4960dfa47e 112 RvalRcapt1 = RvalRcapt1 > 10 ? 10 : RvalRcapt1;
IceTeam 80:cd4960dfa47e 113 RvalRcapt1 = RvalRcapt1 < 0 ? 0 : RvalRcapt1;
IceTeam 80:cd4960dfa47e 114
IceTeam 80:cd4960dfa47e 115 if (Rcapt2.read() > R_SEUIL_SHARP) RvalRcapt2++;
IceTeam 80:cd4960dfa47e 116 else RvalRcapt2--;
IceTeam 80:cd4960dfa47e 117 RvalRcapt2 = RvalRcapt2 > 10 ? 10 : RvalRcapt2;
IceTeam 80:cd4960dfa47e 118 RvalRcapt2 = RvalRcapt2 < 0 ? 0 : RvalRcapt2;
IceTeam 80:cd4960dfa47e 119
IceTeam 80:cd4960dfa47e 120 if (Rcapt3.read() > R_SEUIL_SHARP) RvalRcapt3++;
IceTeam 80:cd4960dfa47e 121 else RvalRcapt3--;
IceTeam 80:cd4960dfa47e 122 RvalRcapt3 = RvalRcapt3 > 10 ? 10 : RvalRcapt3;
IceTeam 80:cd4960dfa47e 123 RvalRcapt3 = RvalRcapt3 < 0 ? 0 : RvalRcapt3;
IceTeam 80:cd4960dfa47e 124
IceTeam 90:78d2c1527c95 125 if ((RvalRcapt1 >= 5 || RvalRcapt2 >= 5 || RvalRcapt3 >= 5))
IceTeam 80:cd4960dfa47e 126 Ravance = 0;
IceTeam 80:cd4960dfa47e 127 else
IceTeam 80:cd4960dfa47e 128 Ravance = 1;
IceTeam 80:cd4960dfa47e 129
IceTeam 80:cd4960dfa47e 130 if (Ravance == 0)
IceTeam 80:cd4960dfa47e 131 drapeau = 1;
IceTeam 80:cd4960dfa47e 132 else
IceTeam 80:cd4960dfa47e 133 drapeau = 2;
IceTeam 81:e7b03e81b025 134 }
IceTeam 81:e7b03e81b025 135
IceTeam 86:0c5e9ac3d8d8 136 void endFonc () {
IceTeam 88:e4de39dd3e2e 137 roboclaw.ForwardM1(0);
IceTeam 88:e4de39dd3e2e 138 roboclaw.ForwardM2(0);
IceTeam 88:e4de39dd3e2e 139 wait(1);
IceTeam 93:c0b040954eac 140 Parasol.setMaxTorque(1000);
IceTeam 90:78d2c1527c95 141 wait(1);
IceTeam 93:c0b040954eac 142 Parasol.setGoal(300);
IceTeam 88:e4de39dd3e2e 143 while(1);
IceTeam 81:e7b03e81b025 144 }
IceTeam 92:f09f55aa992b 145
IceTeam 93:c0b040954eac 146 void init_globals() {
IceTeam 92:f09f55aa992b 147 roboclaw.ForwardM1(0);
IceTeam 92:f09f55aa992b 148 roboclaw.ForwardM2(0);
IceTeam 92:f09f55aa992b 149
IceTeam 93:c0b040954eac 150 Parasol.setMode(0);
IceTeam 93:c0b040954eac 151 Parasol.setMaxTorque(200);
IceTeam 93:c0b040954eac 152 Parasol.setGoal(150);
IceTeam 92:f09f55aa992b 153 wait(1);
IceTeam 93:c0b040954eac 154 Parasol.setGoal(160);
IceTeam 92:f09f55aa992b 155 wait(1);
IceTeam 93:c0b040954eac 156 Parasol.setGoal(150);
IceTeam 92:f09f55aa992b 157 wait(1);
IceTeam 93:c0b040954eac 158 Parasol.setMaxTorque(0);
IceTeam 92:f09f55aa992b 159
IceTeam 92:f09f55aa992b 160 while(START == 1)
IceTeam 92:f09f55aa992b 161 {
IceTeam 92:f09f55aa992b 162 LEDR = 1;
IceTeam 92:f09f55aa992b 163 LEDV = 1;
IceTeam 92:f09f55aa992b 164 wait(0.5);
IceTeam 92:f09f55aa992b 165 LEDR = 0;
IceTeam 92:f09f55aa992b 166 LEDV = 0;
IceTeam 92:f09f55aa992b 167 wait(0.5);
IceTeam 92:f09f55aa992b 168 }
IceTeam 92:f09f55aa992b 169
IceTeam 92:f09f55aa992b 170 wait(1);
sype 97:42167cfeb8d7 171 LEDV = 1;
IceTeam 92:f09f55aa992b 172 depart();
IceTeam 92:f09f55aa992b 173 }