ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

main.cpp@94:86b9bd6d5c28, 2016-05-06 (annotated)

Committer:
IceTeam
Date:
Fri May 06 18:25:24 2016 +0000
Revision:
94:86b9bd6d5c28
Parent:
93:c0b040954eac
Child:
97:42167cfeb8d7
Save new GotoThet

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 }
IceTeam 88:e4de39dd3e2e 89 else if (SCouleur == NOIR) {
IceTeam 94:86b9bd6d5c28 90 GotoThet(-3.14159/2);
IceTeam 94:86b9bd6d5c28 91 GotoDist(1);
IceTeam 83:714fa414b2bf 92 }
IceTeam 83:714fa414b2bf 93
sype 77:f19cc7f81f2a 94 while(1);
sype 2:abdf8c6823a1 95 }
IceTeam 80:cd4960dfa47e 96
IceTeam 80:cd4960dfa47e 97 void Sharps() {
IceTeam 80:cd4960dfa47e 98 if (Rcapt1.read() > R_SEUIL_SHARP) RvalRcapt1++;
IceTeam 80:cd4960dfa47e 99 else RvalRcapt1--;
IceTeam 80:cd4960dfa47e 100 RvalRcapt1 = RvalRcapt1 > 10 ? 10 : RvalRcapt1;
IceTeam 80:cd4960dfa47e 101 RvalRcapt1 = RvalRcapt1 < 0 ? 0 : RvalRcapt1;
IceTeam 80:cd4960dfa47e 102
IceTeam 80:cd4960dfa47e 103 if (Rcapt2.read() > R_SEUIL_SHARP) RvalRcapt2++;
IceTeam 80:cd4960dfa47e 104 else RvalRcapt2--;
IceTeam 80:cd4960dfa47e 105 RvalRcapt2 = RvalRcapt2 > 10 ? 10 : RvalRcapt2;
IceTeam 80:cd4960dfa47e 106 RvalRcapt2 = RvalRcapt2 < 0 ? 0 : RvalRcapt2;
IceTeam 80:cd4960dfa47e 107
IceTeam 80:cd4960dfa47e 108 if (Rcapt3.read() > R_SEUIL_SHARP) RvalRcapt3++;
IceTeam 80:cd4960dfa47e 109 else RvalRcapt3--;
IceTeam 80:cd4960dfa47e 110 RvalRcapt3 = RvalRcapt3 > 10 ? 10 : RvalRcapt3;
IceTeam 80:cd4960dfa47e 111 RvalRcapt3 = RvalRcapt3 < 0 ? 0 : RvalRcapt3;
IceTeam 80:cd4960dfa47e 112
IceTeam 90:78d2c1527c95 113 if ((RvalRcapt1 >= 5 || RvalRcapt2 >= 5 || RvalRcapt3 >= 5))
IceTeam 80:cd4960dfa47e 114 Ravance = 0;
IceTeam 80:cd4960dfa47e 115 else
IceTeam 80:cd4960dfa47e 116 Ravance = 1;
IceTeam 80:cd4960dfa47e 117
IceTeam 80:cd4960dfa47e 118 if (Ravance == 0)
IceTeam 80:cd4960dfa47e 119 drapeau = 1;
IceTeam 80:cd4960dfa47e 120 else
IceTeam 80:cd4960dfa47e 121 drapeau = 2;
IceTeam 81:e7b03e81b025 122 }
IceTeam 81:e7b03e81b025 123
IceTeam 86:0c5e9ac3d8d8 124 void endFonc () {
IceTeam 88:e4de39dd3e2e 125 roboclaw.ForwardM1(0);
IceTeam 88:e4de39dd3e2e 126 roboclaw.ForwardM2(0);
IceTeam 88:e4de39dd3e2e 127 wait(1);
IceTeam 93:c0b040954eac 128 Parasol.setMaxTorque(1000);
IceTeam 90:78d2c1527c95 129 wait(1);
IceTeam 93:c0b040954eac 130 Parasol.setGoal(300);
IceTeam 88:e4de39dd3e2e 131 while(1);
IceTeam 81:e7b03e81b025 132 }
IceTeam 92:f09f55aa992b 133
IceTeam 93:c0b040954eac 134 void init_globals() {
IceTeam 92:f09f55aa992b 135 roboclaw.ForwardM1(0);
IceTeam 92:f09f55aa992b 136 roboclaw.ForwardM2(0);
IceTeam 92:f09f55aa992b 137
IceTeam 93:c0b040954eac 138 Parasol.setMode(0);
IceTeam 93:c0b040954eac 139 Parasol.setMaxTorque(200);
IceTeam 93:c0b040954eac 140 Parasol.setGoal(150);
IceTeam 92:f09f55aa992b 141 wait(1);
IceTeam 93:c0b040954eac 142 Parasol.setGoal(160);
IceTeam 92:f09f55aa992b 143 wait(1);
IceTeam 93:c0b040954eac 144 Parasol.setGoal(150);
IceTeam 92:f09f55aa992b 145 wait(1);
IceTeam 93:c0b040954eac 146 Parasol.setMaxTorque(0);
IceTeam 92:f09f55aa992b 147
IceTeam 92:f09f55aa992b 148 while(START == 1)
IceTeam 92:f09f55aa992b 149 {
IceTeam 92:f09f55aa992b 150 LEDR = 1;
IceTeam 92:f09f55aa992b 151 LEDV = 1;
IceTeam 92:f09f55aa992b 152 wait(0.5);
IceTeam 92:f09f55aa992b 153 LEDR = 0;
IceTeam 92:f09f55aa992b 154 LEDV = 0;
IceTeam 92:f09f55aa992b 155 wait(0.5);
IceTeam 92:f09f55aa992b 156 }
IceTeam 92:f09f55aa992b 157
IceTeam 92:f09f55aa992b 158 wait(1);
IceTeam 92:f09f55aa992b 159 depart();
IceTeam 92:f09f55aa992b 160 }