Romain Ame / Mbed 2 deprecated Stressed

coucou

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

Functions/func.cpp@67:244bc1cc6678, 2016-05-05 (annotated)

Committer:
IceTeam
Date:
Thu May 05 02:11:25 2016 +0200
Revision:
67:244bc1cc6678
Parent:
52:1056dd73a748 
Tentative correction bug

Who changed what in which revision?

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