ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

Functions/func.cpp@45:b53ae54062c6, 2016-04-13 (annotated)

Committer:
sype
Date:
Wed Apr 13 12:47:47 2016 +0000
Revision:
45:b53ae54062c6
Parent:
44:b1fd7489369f
Child:
46:5658af4e5149
Impl?mentation des dispositifs finie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sype 41:b5a2fbc20beb 1 #include "func.h"
sype 41:b5a2fbc20beb 2
sype 41:b5a2fbc20beb 3 void pressed(void)
sype 41:b5a2fbc20beb 4 {
sype 41:b5a2fbc20beb 5 if(i==0) {
sype 41:b5a2fbc20beb 6 roboclaw.ForwardM1(ADR, 0);
sype 41:b5a2fbc20beb 7 roboclaw.ForwardM2(ADR, 0);
sype 41:b5a2fbc20beb 8 i++;
sype 41:b5a2fbc20beb 9 }
sype 41:b5a2fbc20beb 10 }
sype 41:b5a2fbc20beb 11
sype 41:b5a2fbc20beb 12 void unpressed(void)
sype 41:b5a2fbc20beb 13 {
sype 41:b5a2fbc20beb 14 if(i==1) {
sype 41:b5a2fbc20beb 15 i--;
sype 41:b5a2fbc20beb 16 }
sype 41:b5a2fbc20beb 17 }
sype 41:b5a2fbc20beb 18
sype 41:b5a2fbc20beb 19 void ELpressed(void)
sype 41:b5a2fbc20beb 20 {
sype 41:b5a2fbc20beb 21 //bleu = 1;
sype 41:b5a2fbc20beb 22 EL = true;
sype 41:b5a2fbc20beb 23 }
sype 41:b5a2fbc20beb 24 void ELunpressed(void)
sype 41:b5a2fbc20beb 25 {
sype 41:b5a2fbc20beb 26 //bleu = 0;
sype 41:b5a2fbc20beb 27 EL = false;
sype 41:b5a2fbc20beb 28 }
sype 41:b5a2fbc20beb 29
sype 41:b5a2fbc20beb 30 void EZpressed(void)
sype 41:b5a2fbc20beb 31 {
sype 41:b5a2fbc20beb 32 //blanc = 1;
sype 41:b5a2fbc20beb 33 EZ = true;
sype 41:b5a2fbc20beb 34 }
sype 41:b5a2fbc20beb 35 void EZunpressed(void)
sype 41:b5a2fbc20beb 36 {
sype 41:b5a2fbc20beb 37 //blanc = 0;
sype 41:b5a2fbc20beb 38 EZ = false;
sype 41:b5a2fbc20beb 39 }
sype 41:b5a2fbc20beb 40
sype 41:b5a2fbc20beb 41 void ERpressed(void)
sype 41:b5a2fbc20beb 42 {
sype 41:b5a2fbc20beb 43 //rouge = 1;
sype 41:b5a2fbc20beb 44 ER = true;
sype 41:b5a2fbc20beb 45 }
sype 41:b5a2fbc20beb 46 void ERunpressed(void)
sype 41:b5a2fbc20beb 47 {
sype 41:b5a2fbc20beb 48 //rouge = 0;
sype 41:b5a2fbc20beb 49 ER = 0;
sype 41:b5a2fbc20beb 50 }
sype 41:b5a2fbc20beb 51
sype 41:b5a2fbc20beb 52 void JPO(void)
sype 41:b5a2fbc20beb 53 {
sype 41:b5a2fbc20beb 54 char c = logger.getc();
sype 41:b5a2fbc20beb 55 if(c=='z') {
sype 41:b5a2fbc20beb 56 if (state != 1) {
sype 41:b5a2fbc20beb 57 state = 1;
sype 41:b5a2fbc20beb 58 logger.printf("Avant (Z) \r\n");
sype 41:b5a2fbc20beb 59 roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
sype 41:b5a2fbc20beb 60 roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
sype 41:b5a2fbc20beb 61 }
sype 41:b5a2fbc20beb 62 } else if(c == 's') {
sype 41:b5a2fbc20beb 63 if (state != 2) {
sype 41:b5a2fbc20beb 64 state = 2;
sype 41:b5a2fbc20beb 65 logger.printf("Stop (S) \r\n");
sype 41:b5a2fbc20beb 66 roboclaw.SpeedAccelM1(ADR, accel_angle, 0);
sype 41:b5a2fbc20beb 67 roboclaw.SpeedAccelM2(ADR, accel_angle, 0);
sype 41:b5a2fbc20beb 68 }
sype 41:b5a2fbc20beb 69 } else if(c == 'd') {
sype 41:b5a2fbc20beb 70 if (state != 3) {
sype 41:b5a2fbc20beb 71 state = 3;
sype 41:b5a2fbc20beb 72 logger.printf("Droite (D) \r\n");
sype 41:b5a2fbc20beb 73 roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
sype 41:b5a2fbc20beb 74 roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
sype 41:b5a2fbc20beb 75 }
sype 41:b5a2fbc20beb 76 } else if(c == 'q') {
sype 41:b5a2fbc20beb 77 if (state != 4) {
sype 41:b5a2fbc20beb 78 state = 4;
sype 41:b5a2fbc20beb 79 logger.printf("Gauche (Q)\r\n");
sype 41:b5a2fbc20beb 80 roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
sype 41:b5a2fbc20beb 81 roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
sype 41:b5a2fbc20beb 82 }
sype 41:b5a2fbc20beb 83 } else if(c == 'x') {
sype 41:b5a2fbc20beb 84 if (state != 5) {
sype 41:b5a2fbc20beb 85 state = 5;
sype 41:b5a2fbc20beb 86 roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
sype 41:b5a2fbc20beb 87 roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
sype 41:b5a2fbc20beb 88 }
sype 41:b5a2fbc20beb 89 } else {
sype 41:b5a2fbc20beb 90 if (state != 0) {
sype 41:b5a2fbc20beb 91 roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
sype 41:b5a2fbc20beb 92 state = 0;
sype 41:b5a2fbc20beb 93 }
sype 41:b5a2fbc20beb 94 }
sype 41:b5a2fbc20beb 95 }
sype 41:b5a2fbc20beb 96
sype 41:b5a2fbc20beb 97 void goHome(void)
sype 41:b5a2fbc20beb 98 {
sype 41:b5a2fbc20beb 99 while(EZ==false) ZMot.step(1, 0, DELAY);
sype 41:b5a2fbc20beb 100 while(ER==false) RMot.step(1, 0, DELAY);
sype 41:b5a2fbc20beb 101 RMot.step(10, 1, DELAY);
sype 41:b5a2fbc20beb 102 RMot.step(5, 0, DELAY);
sype 41:b5a2fbc20beb 103 while(EL==false) LMot.step(1, 0, DELAY);
sype 41:b5a2fbc20beb 104 LMot.step(10, 1, DELAY);
sype 41:b5a2fbc20beb 105 LMot.step(5, 0, DELAY);
sype 41:b5a2fbc20beb 106 }
sype 41:b5a2fbc20beb 107
sype 41:b5a2fbc20beb 108 void checkAround(void)
sype 41:b5a2fbc20beb 109 {
sype 41:b5a2fbc20beb 110 if(capt1 > SEUIL+0.1) bleu = 1;
sype 41:b5a2fbc20beb 111 else bleu = 0;
sype 41:b5a2fbc20beb 112 if(capt2 > SEUIL) blanc = 1;
sype 41:b5a2fbc20beb 113 else blanc = 0;
sype 41:b5a2fbc20beb 114 if(capt3 > SEUIL+0.1) rouge = 1;
sype 41:b5a2fbc20beb 115 else rouge = 0;
sype 41:b5a2fbc20beb 116 }
sype 41:b5a2fbc20beb 117
sype 41:b5a2fbc20beb 118 void init_ax12(void)
sype 41:b5a2fbc20beb 119 {
sype 41:b5a2fbc20beb 120 left_hand.setMode(0);
sype 41:b5a2fbc20beb 121 wait_ms(10);
sype 41:b5a2fbc20beb 122 right_hand.setMode(0);
sype 41:b5a2fbc20beb 123 wait_ms(50);
sype 41:b5a2fbc20beb 124 left_hand.setMode(0);
sype 41:b5a2fbc20beb 125 wait_ms(10);
sype 41:b5a2fbc20beb 126 right_hand.setMode(0);
sype 41:b5a2fbc20beb 127 wait_ms(50);
sype 41:b5a2fbc20beb 128 right_hand.setGoal(0);
sype 41:b5a2fbc20beb 129 left_hand.setGoal(0);
sype 41:b5a2fbc20beb 130 wait(2);
sype 41:b5a2fbc20beb 131 right_hand.setGoal(180);
sype 41:b5a2fbc20beb 132 left_hand.setGoal(180);
sype 41:b5a2fbc20beb 133 wait(2);
sype 44:b1fd7489369f 134 }
sype 44:b1fd7489369f 135
sype 44:b1fd7489369f 136 void init_interrupt(void)
sype 44:b1fd7489369f 137 {
sype 44:b1fd7489369f 138 mybutton.fall(&pressed);
sype 44:b1fd7489369f 139 mybutton.rise(&unpressed);
sype 44:b1fd7489369f 140
sype 44:b1fd7489369f 141 EndL.fall(&ELpressed);
sype 44:b1fd7489369f 142 EndL.rise(&ELunpressed);
sype 44:b1fd7489369f 143 EndZ.fall(&EZpressed);
sype 44:b1fd7489369f 144 EndZ.rise(&EZunpressed);
sype 44:b1fd7489369f 145 EndR.fall(&ERpressed);
sype 44:b1fd7489369f 146 EndR.rise(&ERunpressed);
sype 44:b1fd7489369f 147 ticker.attach_us(&update_main,dt); // 100 Hz
sype 45:b53ae54062c6 148 }
sype 45:b53ae54062c6 149
sype 45:b53ae54062c6 150 void update_main(void)
sype 45:b53ae54062c6 151 {
sype 45:b53ae54062c6 152 odo.update_odo();
sype 45:b53ae54062c6 153 checkAround();
sype 41:b5a2fbc20beb 154 }