ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

Functions/func.cpp@51:1056dd73a748, 2016-05-04 (annotated)

Committer:
sype
Date:
Wed May 04 16:15:13 2016 +0000
Revision:
51:1056dd73a748
Parent:
48:03da1aead032
Child:
67:244bc1cc6678
Child:
77:f19cc7f81f2a
Impl?mentation de l'homologation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sype 41:b5a2fbc20beb 1 #include "func.h"
sype 41:b5a2fbc20beb 2
sype 51:1056dd73a748 3 void init_interrupt(void)
sype 51:1056dd73a748 4 {
sype 51:1056dd73a748 5 EndL.fall(&ELpressed);
sype 51:1056dd73a748 6 EndL.rise(&ELunpressed);
sype 51:1056dd73a748 7 EndZ.fall(&EZpressed);
sype 51:1056dd73a748 8 EndZ.rise(&EZunpressed);
sype 51:1056dd73a748 9 EndR.fall(&ERpressed);
sype 51:1056dd73a748 10 EndR.rise(&ERunpressed);
sype 51:1056dd73a748 11 ticker.attach_us(&update_main, dt); // 100 Hz
sype 51:1056dd73a748 12 }
sype 51:1056dd73a748 13
sype 41:b5a2fbc20beb 14 void pressed(void)
sype 41:b5a2fbc20beb 15 {
sype 46:5658af4e5149 16 if(SIMON_i==0) {
sype 46:5658af4e5149 17 roboclaw.ForwardM1(0);
sype 46:5658af4e5149 18 roboclaw.ForwardM2(0);
sype 46:5658af4e5149 19 SIMON_i++;
sype 41:b5a2fbc20beb 20 }
sype 41:b5a2fbc20beb 21 }
sype 41:b5a2fbc20beb 22
sype 41:b5a2fbc20beb 23 void unpressed(void)
sype 41:b5a2fbc20beb 24 {
sype 46:5658af4e5149 25 if(SIMON_i==1) {
sype 46:5658af4e5149 26 SIMON_i--;
sype 41:b5a2fbc20beb 27 }
sype 41:b5a2fbc20beb 28 }
sype 41:b5a2fbc20beb 29
sype 41:b5a2fbc20beb 30 void ELpressed(void)
sype 41:b5a2fbc20beb 31 {
sype 46:5658af4e5149 32 bleu = 1;
sype 46:5658af4e5149 33 SIMON_EL = true;
sype 41:b5a2fbc20beb 34 }
sype 41:b5a2fbc20beb 35 void ELunpressed(void)
sype 41:b5a2fbc20beb 36 {
sype 46:5658af4e5149 37 bleu = 0;
sype 46:5658af4e5149 38 SIMON_EL = false;
sype 41:b5a2fbc20beb 39 }
sype 41:b5a2fbc20beb 40
sype 41:b5a2fbc20beb 41 void EZpressed(void)
sype 41:b5a2fbc20beb 42 {
sype 46:5658af4e5149 43 blanc = 1;
sype 46:5658af4e5149 44 SIMON_EZ = true;
sype 41:b5a2fbc20beb 45 }
sype 41:b5a2fbc20beb 46 void EZunpressed(void)
sype 41:b5a2fbc20beb 47 {
sype 46:5658af4e5149 48 blanc = 0;
sype 46:5658af4e5149 49 SIMON_EZ = false;
sype 41:b5a2fbc20beb 50 }
sype 41:b5a2fbc20beb 51
sype 41:b5a2fbc20beb 52 void ERpressed(void)
sype 41:b5a2fbc20beb 53 {
sype 46:5658af4e5149 54 rouge = 1;
sype 46:5658af4e5149 55 SIMON_ER = true;
sype 41:b5a2fbc20beb 56 }
sype 41:b5a2fbc20beb 57 void ERunpressed(void)
sype 41:b5a2fbc20beb 58 {
sype 46:5658af4e5149 59 rouge = 0;
sype 46:5658af4e5149 60 SIMON_ER = 0;
sype 41:b5a2fbc20beb 61 }
sype 41:b5a2fbc20beb 62
sype 41:b5a2fbc20beb 63 void JPO(void)
sype 41:b5a2fbc20beb 64 {
sype 41:b5a2fbc20beb 65 char c = logger.getc();
sype 41:b5a2fbc20beb 66 if(c=='z') {
sype 46:5658af4e5149 67 if (SIMON_state != 1) {
sype 46:5658af4e5149 68 SIMON_state = 1;
sype 41:b5a2fbc20beb 69 logger.printf("Avant (Z) \r\n");
sype 46:5658af4e5149 70 roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
sype 46:5658af4e5149 71 roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
sype 41:b5a2fbc20beb 72 }
sype 41:b5a2fbc20beb 73 } else if(c == 's') {
sype 46:5658af4e5149 74 if (SIMON_state != 2) {
sype 46:5658af4e5149 75 SIMON_state = 2;
sype 41:b5a2fbc20beb 76 logger.printf("Stop (S) \r\n");
sype 46:5658af4e5149 77 roboclaw.SpeedAccelM1(accel_angle, 0);
sype 46:5658af4e5149 78 roboclaw.SpeedAccelM2(accel_angle, 0);
sype 41:b5a2fbc20beb 79 }
sype 41:b5a2fbc20beb 80 } else if(c == 'd') {
sype 46:5658af4e5149 81 if (SIMON_state != 3) {
sype 46:5658af4e5149 82 SIMON_state = 3;
sype 41:b5a2fbc20beb 83 logger.printf("Droite (D) \r\n");
sype 46:5658af4e5149 84 roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
sype 46:5658af4e5149 85 roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
sype 41:b5a2fbc20beb 86 }
sype 41:b5a2fbc20beb 87 } else if(c == 'q') {
sype 46:5658af4e5149 88 if (SIMON_state != 4) {
sype 46:5658af4e5149 89 SIMON_state = 4;
sype 41:b5a2fbc20beb 90 logger.printf("Gauche (Q)\r\n");
sype 46:5658af4e5149 91 roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
sype 46:5658af4e5149 92 roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
sype 41:b5a2fbc20beb 93 }
sype 41:b5a2fbc20beb 94 } else if(c == 'x') {
sype 46:5658af4e5149 95 if (SIMON_state != 5) {
sype 46:5658af4e5149 96 SIMON_state = 5;
sype 46:5658af4e5149 97 roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
sype 46:5658af4e5149 98 roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
sype 41:b5a2fbc20beb 99 }
sype 41:b5a2fbc20beb 100 } else {
sype 46:5658af4e5149 101 if (SIMON_state != 0) {
sype 51:1056dd73a748 102 roboclaw.SpeedAccelM1M2(accel_angle, 0, 0);
sype 46:5658af4e5149 103 SIMON_state = 0;
sype 41:b5a2fbc20beb 104 }
sype 41:b5a2fbc20beb 105 }
sype 41:b5a2fbc20beb 106 }
sype 41:b5a2fbc20beb 107
sype 41:b5a2fbc20beb 108 void goHome(void)
sype 41:b5a2fbc20beb 109 {
sype 51:1056dd73a748 110 goHomeZ();
sype 51:1056dd73a748 111 goHomeL();
sype 51:1056dd73a748 112 goHomeR();
sype 51:1056dd73a748 113 }
sype 51:1056dd73a748 114
sype 51:1056dd73a748 115 void goHomeL(void)
sype 51:1056dd73a748 116 {
sype 51:1056dd73a748 117 while(EnL==true) LMot.step(1, BAS, DELAY);
sype 51:1056dd73a748 118 LMot.step(10, HAUT, DELAY+0.01);
sype 51:1056dd73a748 119 LMot.step(5, BAS, DELAY);
sype 51:1056dd73a748 120 }
sype 51:1056dd73a748 121
sype 51:1056dd73a748 122 void goHomeZ(void)
sype 51:1056dd73a748 123 {
sype 51:1056dd73a748 124 while(EnZ==true) ZMot.step(1, BAS, DELAY);
sype 51:1056dd73a748 125 ZMot.step(10, HAUT, DELAY+0.01);
sype 51:1056dd73a748 126 ZMot.step(5, BAS, DELAY);
sype 51:1056dd73a748 127 }
sype 51:1056dd73a748 128
sype 51:1056dd73a748 129 void goHomeR(void)
sype 51:1056dd73a748 130 {
sype 51:1056dd73a748 131 while(EnR==true) RMot.step(1, BAS, DELAY);
sype 51:1056dd73a748 132 RMot.step(10, HAUT, DELAY+0.01);
sype 51:1056dd73a748 133 RMot.step(5, BAS, DELAY);
sype 41:b5a2fbc20beb 134 }
sype 41:b5a2fbc20beb 135
sype 41:b5a2fbc20beb 136 void checkAround(void)
sype 41:b5a2fbc20beb 137 {
sype 51:1056dd73a748 138 if(capt1.read() > SEUIL+0.1) SGauche = true;
sype 51:1056dd73a748 139 else SGauche = false;
sype 51:1056dd73a748 140 if(capt2.read() > SEUIL) SDevant = true;
sype 51:1056dd73a748 141 else SDevant = false;
sype 51:1056dd73a748 142 if(capt3.read() > SEUIL+0.1) SDroite = true;
sype 51:1056dd73a748 143 else SDroite = false;
sype 51:1056dd73a748 144 logger.printf("%1.3f\t%1.3f\t%1.3f\n\r", capt1.read(), capt2.read(), capt3.read());
sype 51:1056dd73a748 145 }
sype 51:1056dd73a748 146
sype 51:1056dd73a748 147 void homologation(void)
sype 51:1056dd73a748 148 {
sype 51:1056dd73a748 149 goHomeZ();
sype 51:1056dd73a748 150 ZMot.mm(50, HAUT);
sype 51:1056dd73a748 151 goHomeL();
sype 51:1056dd73a748 152 goHomeR();
sype 51:1056dd73a748 153 LMot.mm(50, HAUT);
sype 51:1056dd73a748 154 RMot.mm(50, HAUT);
sype 51:1056dd73a748 155 while(START==0) logger.printf("%1.3f\t%1.3f\t%1.3f\n\r", capt1.read(), capt2.read(), capt3.read());
sype 51:1056dd73a748 156 roboclaw.SpeedAccelM1M2(accel_dista, vitesse_dista, vitesse_dista);
sype 51:1056dd73a748 157 while(1) {
sype 51:1056dd73a748 158 while(capt2.read() < SEUIL) checkAround();
sype 51:1056dd73a748 159 while(1) roboclaw.SpeedAccelM1M2(accel_dista, 0, 0);
sype 51:1056dd73a748 160 }
sype 41:b5a2fbc20beb 161 }
sype 41:b5a2fbc20beb 162
sype 48:03da1aead032 163 /*void init_ax12(void)
sype 41:b5a2fbc20beb 164 {
sype 41:b5a2fbc20beb 165 left_hand.setMode(0);
sype 41:b5a2fbc20beb 166 wait_ms(10);
sype 41:b5a2fbc20beb 167 right_hand.setMode(0);
sype 41:b5a2fbc20beb 168 wait_ms(50);
sype 41:b5a2fbc20beb 169 left_hand.setMode(0);
sype 41:b5a2fbc20beb 170 wait_ms(10);
sype 41:b5a2fbc20beb 171 right_hand.setMode(0);
sype 41:b5a2fbc20beb 172 wait_ms(50);
sype 41:b5a2fbc20beb 173 right_hand.setGoal(0);
sype 41:b5a2fbc20beb 174 left_hand.setGoal(0);
sype 41:b5a2fbc20beb 175 wait(2);
sype 41:b5a2fbc20beb 176 right_hand.setGoal(180);
sype 41:b5a2fbc20beb 177 left_hand.setGoal(180);
sype 41:b5a2fbc20beb 178 wait(2);
sype 48:03da1aead032 179 }*/
sype 44:b1fd7489369f 180
sype 46:5658af4e5149 181 void changeCamp(void)
sype 46:5658af4e5149 182 {
sype 48:03da1aead032 183 if(SCouleur==VERT) {
sype 48:03da1aead032 184 SCouleur = VIOLET;
sype 48:03da1aead032 185 LEDR = 1;
sype 48:03da1aead032 186 LEDV = 0;
sype 51:1056dd73a748 187 } else {
sype 48:03da1aead032 188 SCouleur = VERT;
sype 48:03da1aead032 189 LEDV = 1;
sype 51:1056dd73a748 190 LEDR = 0;
sype 48:03da1aead032 191 }
sype 46:5658af4e5149 192 }
sype 46:5658af4e5149 193
sype 51:1056dd73a748 194 void depart(void)
sype 45:b53ae54062c6 195 {
sype 51:1056dd73a748 196 while(button==1) {
sype 51:1056dd73a748 197 if(CAMP==0) {
sype 51:1056dd73a748 198 changeCamp();
sype 51:1056dd73a748 199 }
sype 51:1056dd73a748 200 wait_ms(100);
sype 51:1056dd73a748 201 }
sype 51:1056dd73a748 202
sype 51:1056dd73a748 203 if(SCouleur == VERT)logger.printf("Couleur VERT !\n\r");
sype 51:1056dd73a748 204 else logger.printf("Couleur ROUGE !\n\r");
sype 51:1056dd73a748 205
sype 51:1056dd73a748 206 while(START==0) {
sype 51:1056dd73a748 207 drapeau = SSchema;
sype 51:1056dd73a748 208 if(CAMP==0) {
sype 51:1056dd73a748 209 SSchema++;
sype 51:1056dd73a748 210 if(SSchema == 6) SSchema = 1;
sype 51:1056dd73a748 211 drapeau = SSchema;
sype 51:1056dd73a748 212 }
sype 51:1056dd73a748 213 wait_ms(100);
sype 51:1056dd73a748 214 }
sype 51:1056dd73a748 215 logger.printf("Schema numero : %d !\n\r", SSchema);
sype 51:1056dd73a748 216 }