ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

main.cpp@98:2426d699362b, 2016-05-06 (annotated)

Committer:
Jagang
Date:
Fri May 06 23:55:01 2016 +0000
Revision:
98:2426d699362b
Parent:
97:42167cfeb8d7
Child:
99:1fcb088f8f36
petit push ? romain;

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