func.cpp

00001 #include "func.h"
00002 
00003 void pressed(void)
00004 {
00005     if(i==0) {
00006         roboclaw.ForwardM1(ADR, 0);
00007         roboclaw.ForwardM2(ADR, 0);
00008         i++;
00009     }
00010 }
00011 
00012 void unpressed(void)
00013 {
00014     if(i==1) {
00015         i--;
00016     }
00017 }
00018 
00019 void ELpressed(void)
00020 {
00021     //bleu = 1;
00022     EL = true;
00023 }
00024 void ELunpressed(void)
00025 {
00026     //bleu = 0;
00027     EL = false;
00028 }
00029 
00030 void EZpressed(void)
00031 {
00032     //blanc = 1;
00033     EZ = true;
00034 }
00035 void EZunpressed(void)
00036 {
00037     //blanc = 0;
00038     EZ = false;
00039 }
00040 
00041 void ERpressed(void)
00042 {
00043     //rouge = 1;
00044     ER = true;
00045 }
00046 void ERunpressed(void)
00047 {
00048     //rouge = 0;
00049     ER = 0;
00050 }
00051 
00052 void JPO(void)
00053 {
00054     char c = logger.getc();
00055     if(c=='z') {
00056         if (state != 1) {
00057             state = 1;
00058             logger.printf("Avant (Z) \r\n");
00059             roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
00060             roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
00061         }
00062     } else if(c == 's') {
00063         if (state != 2) {
00064             state = 2;
00065             logger.printf("Stop (S) \r\n");
00066             roboclaw.SpeedAccelM1(ADR, accel_angle, 0);
00067             roboclaw.SpeedAccelM2(ADR, accel_angle, 0);
00068         }
00069     } else if(c == 'd') {
00070         if (state != 3) {
00071             state = 3;
00072             logger.printf("Droite (D) \r\n");
00073             roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
00074             roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
00075         }
00076     } else if(c == 'q') {
00077         if (state != 4) {
00078             state = 4;
00079             logger.printf("Gauche (Q)\r\n");
00080             roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
00081             roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
00082         }
00083     } else if(c == 'x') {
00084         if (state != 5) {
00085             state = 5;
00086             roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
00087             roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
00088         }
00089     } else {
00090         if (state != 0) {
00091             roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
00092             state = 0;
00093         }
00094     }
00095 }
00096 
00097 void goHome(void)
00098 {
00099     while(EZ==false) ZMot.step(1, 0, DELAY);
00100     while(ER==false) RMot.step(1, 0, DELAY);
00101     RMot.step(10, 1, DELAY);
00102     RMot.step(5, 0, DELAY);
00103     while(EL==false) LMot.step(1, 0, DELAY);
00104     LMot.step(10, 1, DELAY);
00105     LMot.step(5, 0, DELAY);
00106 }
00107 
00108 void checkAround(void)
00109 {
00110     if(capt1 > SEUIL+0.1) S::bleu = 1;
00111     else S::bleu = 0;
00112     if(capt2 > SEUIL) blanc = 1;
00113     else blanc = 0;
00114     if(capt3 > SEUIL+0.1) rouge = 1;
00115     else rouge = 0;
00116 }
00117 
00118 void init_ax12(void)
00119 {
00120     left_hand.setMode(0);
00121     wait_ms(10);
00122     right_hand.setMode(0);
00123     wait_ms(50);
00124     left_hand.setMode(0);
00125     wait_ms(10);
00126     right_hand.setMode(0);
00127     wait_ms(50);
00128     right_hand.setGoal(0);
00129     left_hand.setGoal(0);
00130     wait(2);
00131     right_hand.setGoal(180);
00132     left_hand.setGoal(180);
00133     wait(2);
00134 }
00135 
00136 void init_interrupt(void)
00137 {
00138     S::mybutton.fall(&pressed);
00139     S::mybutton.rise(&unpressed);
00140 
00141     EndL.fall(&ELpressed);
00142     EndL.rise(&ELunpressed);
00143     EndZ.fall(&EZpressed);
00144     EndZ.rise(&EZunpressed);
00145     EndR.fall(&ERpressed);
00146     EndR.rise(&ERunpressed);
00147     ticker.attach_us(&update_main,dt); // 100 Hz
00148 }
00149 
00150 void update_main(void)
00151 {
00152     odo.update_odo();
00153     checkAround();
00154 }