Automate
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of TestBoard by
main.cpp@8:ad8b64ca548d, 2017-06-09 (annotated)
- Committer:
- DOREL
- Date:
- Fri Jun 09 17:06:05 2017 +0000
- Revision:
- 8:ad8b64ca548d
- Parent:
- 6:88b4805d33e1
Automate;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
haarkon | 0:f00e68bef80c | 1 | /** Main Test Board |
haarkon | 0:f00e68bef80c | 2 | * |
haarkon | 0:f00e68bef80c | 3 | * \brief Programme de tests pour le robot NCR 2017 |
haarkon | 0:f00e68bef80c | 4 | * \author H. Angelis |
haarkon | 0:f00e68bef80c | 5 | * \version alpha_1 |
haarkon | 0:f00e68bef80c | 6 | * \date 15/05/17 |
haarkon | 0:f00e68bef80c | 7 | * |
haarkon | 0:f00e68bef80c | 8 | */ |
haarkon | 0:f00e68bef80c | 9 | |
DOREL | 8:ad8b64ca548d | 10 | #include "include_define_typedeflibrary.h" |
DOREL | 8:ad8b64ca548d | 11 | #include "function_library.h" |
haarkon | 2:1d440e938c44 | 12 | |
haarkon | 0:f00e68bef80c | 13 | int main() |
haarkon | 0:f00e68bef80c | 14 | { |
haarkon | 0:f00e68bef80c | 15 | |
haarkon | 2:1d440e938c44 | 16 | int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/; |
haarkon | 5:4955cb4b3646 | 17 | |
haarkon | 3:3dfe26a10796 | 18 | Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0; |
haarkon | 2:1d440e938c44 | 19 | int PIXY_objet; |
haarkon | 0:f00e68bef80c | 20 | |
haarkon | 6:88b4805d33e1 | 21 | int SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400; |
haarkon | 0:f00e68bef80c | 22 | T_SERVODIR SERVO_dir = S_monte; |
haarkon | 0:f00e68bef80c | 23 | |
haarkon | 0:f00e68bef80c | 24 | char MENU_choix = 0; |
haarkon | 0:f00e68bef80c | 25 | |
haarkon | 2:1d440e938c44 | 26 | char BOUSSOLE_status[1] = {0}; |
haarkon | 0:f00e68bef80c | 27 | char I2C_registerValue[4]; |
haarkon | 3:3dfe26a10796 | 28 | double BOUSSOLE_periode; |
haarkon | 0:f00e68bef80c | 29 | |
haarkon | 0:f00e68bef80c | 30 | double CAP_I2C, CAP_PWM; |
DOREL | 8:ad8b64ca548d | 31 | double CNY1_val, CNY2_val, CNY3_val, Vbat_val; |
DOREL | 8:ad8b64ca548d | 32 | double SD2_dist, LD2_dist; |
haarkon | 0:f00e68bef80c | 33 | |
haarkon | 6:88b4805d33e1 | 34 | int MOTG_evol = 1, MOTD_evol = 1; |
haarkon | 6:88b4805d33e1 | 35 | double MOTG_duty = 0.5, MOTD_duty = 0.5; |
haarkon | 5:4955cb4b3646 | 36 | |
DOREL | 8:ad8b64ca548d | 37 | //Variables Automate |
DOREL | 8:ad8b64ca548d | 38 | int etat_actuel = 0, etat_futur = 0, etat_precedent = 0; //Need etat_precedent ? |
DOREL | 8:ad8b64ca548d | 39 | |
DOREL | 8:ad8b64ca548d | 40 | //Variables test capteurs |
DOREL | 8:ad8b64ca548d | 41 | int ball_found = 0, ball_forward = 0, ball_captured = 0, face_ennemy_side = 0, face_our_side = 0; |
DOREL | 8:ad8b64ca548d | 42 | int wall = 0, white_line_crossed = 0, return_done = 0, rotation_done; //Rename white_line_crossed |
DOREL | 8:ad8b64ca548d | 43 | |
DOREL | 8:ad8b64ca548d | 44 | //Variables |
DOREL | 8:ad8b64ca548d | 45 | |
DOREL | 8:ad8b64ca548d | 46 | int orientation_camp_adversaire, orientation_mon_camp; |
DOREL | 8:ad8b64ca548d | 47 | |
haarkon | 2:1d440e938c44 | 48 | times.reset(); |
haarkon | 2:1d440e938c44 | 49 | times.start(); |
haarkon | 0:f00e68bef80c | 50 | |
haarkon | 6:88b4805d33e1 | 51 | // Initialisation des interruptions |
haarkon | 2:1d440e938c44 | 52 | tick.attach(&tickTime, 0.001); |
haarkon | 0:f00e68bef80c | 53 | |
haarkon | 6:88b4805d33e1 | 54 | BP.rise (&BPevent); |
haarkon | 0:f00e68bef80c | 55 | |
haarkon | 6:88b4805d33e1 | 56 | Echo1.rise (&echo1Rise); |
haarkon | 6:88b4805d33e1 | 57 | Echo2.rise (&echo2Rise); |
haarkon | 6:88b4805d33e1 | 58 | Echo3.rise (&echo3Rise); |
haarkon | 6:88b4805d33e1 | 59 | Echo1.fall (&echo1Fall); |
haarkon | 6:88b4805d33e1 | 60 | Echo2.fall (&echo2Fall); |
haarkon | 6:88b4805d33e1 | 61 | Echo3.fall (&echo3Fall); |
haarkon | 6:88b4805d33e1 | 62 | |
haarkon | 6:88b4805d33e1 | 63 | PWMG.rise (&PWM_motGRise); |
haarkon | 6:88b4805d33e1 | 64 | PWMD.rise (&PWM_motDRise); |
haarkon | 6:88b4805d33e1 | 65 | |
haarkon | 6:88b4805d33e1 | 66 | Pixy.attach (&getPixyByte); |
haarkon | 0:f00e68bef80c | 67 | |
haarkon | 1:d95546f84105 | 68 | BP.enable_irq(); |
haarkon | 0:f00e68bef80c | 69 | IG.enable_irq(); |
haarkon | 0:f00e68bef80c | 70 | Echo1.enable_irq(); |
haarkon | 0:f00e68bef80c | 71 | Echo2.enable_irq(); |
haarkon | 0:f00e68bef80c | 72 | Echo3.enable_irq(); |
haarkon | 6:88b4805d33e1 | 73 | PWMG.enable_irq(); |
haarkon | 6:88b4805d33e1 | 74 | PWMD.enable_irq(); |
haarkon | 0:f00e68bef80c | 75 | |
haarkon | 6:88b4805d33e1 | 76 | // Initialisation des périphériques |
haarkon | 6:88b4805d33e1 | 77 | // Bus I2C |
haarkon | 6:88b4805d33e1 | 78 | Bus_I2C.frequency (100000); |
haarkon | 0:f00e68bef80c | 79 | |
haarkon | 6:88b4805d33e1 | 80 | // PWM des moteurs |
haarkon | 0:f00e68bef80c | 81 | Pwm_MG.period_us(50); |
haarkon | 0:f00e68bef80c | 82 | Pwm_MD.period_us(50); |
haarkon | 0:f00e68bef80c | 83 | En = 0; |
haarkon | 5:4955cb4b3646 | 84 | |
haarkon | 6:88b4805d33e1 | 85 | // Bus SPI |
haarkon | 6:88b4805d33e1 | 86 | SPIG.format (16,1); |
haarkon | 6:88b4805d33e1 | 87 | SPIG.frequency (1000000); |
haarkon | 6:88b4805d33e1 | 88 | |
haarkon | 6:88b4805d33e1 | 89 | SPID.format (16,1); |
haarkon | 6:88b4805d33e1 | 90 | SPID.frequency (1000000); |
haarkon | 6:88b4805d33e1 | 91 | |
haarkon | 6:88b4805d33e1 | 92 | SS = 1; |
haarkon | 6:88b4805d33e1 | 93 | |
haarkon | 6:88b4805d33e1 | 94 | // Led |
haarkon | 0:f00e68bef80c | 95 | Led2 = 0; |
haarkon | 0:f00e68bef80c | 96 | |
haarkon | 6:88b4805d33e1 | 97 | Servo.period_ms (20); |
haarkon | 6:88b4805d33e1 | 98 | Servo.pulsewidth_us(200); |
DOREL | 8:ad8b64ca548d | 99 | |
haarkon | 0:f00e68bef80c | 100 | while(1) { |
haarkon | 0:f00e68bef80c | 101 | |
DOREL | 8:ad8b64ca548d | 102 | //capteurs_read(); //Lecture tous les capteurs |
DOREL | 8:ad8b64ca548d | 103 | CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10; |
DOREL | 8:ad8b64ca548d | 104 | Pc.printf ("\r PWM = %4.1lf", CAP_PWM); |
DOREL | 8:ad8b64ca548d | 105 | //capteurs_test(); //Teste les valeurs des capteurs et change des variables booleenne |
DOREL | 8:ad8b64ca548d | 106 | etat_actuel = etat_futur; |
DOREL | 8:ad8b64ca548d | 107 | |
DOREL | 8:ad8b64ca548d | 108 | switch(etat_actuel) { |
DOREL | 8:ad8b64ca548d | 109 | case START: |
DOREL | 8:ad8b64ca548d | 110 | etat_futur = STOP_RETURN; |
DOREL | 8:ad8b64ca548d | 111 | break; |
DOREL | 8:ad8b64ca548d | 112 | |
DOREL | 8:ad8b64ca548d | 113 | case SEEK_BALL: |
DOREL | 8:ad8b64ca548d | 114 | if(ball_found) etat_futur = TURN_TO_BALL; |
DOREL | 8:ad8b64ca548d | 115 | if(!ball_found) etat_futur = SEEK_ROTATION; |
DOREL | 8:ad8b64ca548d | 116 | break; |
DOREL | 8:ad8b64ca548d | 117 | |
DOREL | 8:ad8b64ca548d | 118 | case SEEK_ROTATION: |
DOREL | 8:ad8b64ca548d | 119 | if(rotation_done) etat_futur = SEEK_BALL; |
DOREL | 8:ad8b64ca548d | 120 | |
DOREL | 8:ad8b64ca548d | 121 | case TURN_TO_BALL: |
DOREL | 8:ad8b64ca548d | 122 | if(ball_forward) etat_futur = GO_TO_BALL; |
DOREL | 8:ad8b64ca548d | 123 | break; |
DOREL | 8:ad8b64ca548d | 124 | |
DOREL | 8:ad8b64ca548d | 125 | case GO_TO_BALL: |
DOREL | 8:ad8b64ca548d | 126 | if(!ball_forward) etat_futur = TURN_TO_BALL; |
DOREL | 8:ad8b64ca548d | 127 | if(ball_captured & face_ennemy_side) etat_futur = BALL_LAUNCHING; |
DOREL | 8:ad8b64ca548d | 128 | if(ball_captured & !face_ennemy_side) etat_futur = CAPTURE_AND_TURN; |
haarkon | 0:f00e68bef80c | 129 | break; |
DOREL | 8:ad8b64ca548d | 130 | |
DOREL | 8:ad8b64ca548d | 131 | case CAPTURE_AND_TURN: |
DOREL | 8:ad8b64ca548d | 132 | if(face_ennemy_side) etat_futur = RELEASE_CAPTURE; |
DOREL | 8:ad8b64ca548d | 133 | break; |
DOREL | 8:ad8b64ca548d | 134 | |
DOREL | 8:ad8b64ca548d | 135 | case WALL_CAPTURE_AND_TURN: |
DOREL | 8:ad8b64ca548d | 136 | if(!wall & face_ennemy_side) etat_futur = RELEASE_CAPTURE; |
DOREL | 8:ad8b64ca548d | 137 | break; |
DOREL | 8:ad8b64ca548d | 138 | |
DOREL | 8:ad8b64ca548d | 139 | case RELEASE_CAPTURE: |
DOREL | 8:ad8b64ca548d | 140 | etat_futur = BALL_LAUNCHING; |
haarkon | 0:f00e68bef80c | 141 | break; |
DOREL | 8:ad8b64ca548d | 142 | |
DOREL | 8:ad8b64ca548d | 143 | case BALL_LAUNCHING: |
DOREL | 8:ad8b64ca548d | 144 | if(wall) etat_futur = WALL_CAPTURE_AND_TURN ; |
DOREL | 8:ad8b64ca548d | 145 | if(white_line_crossed) etat_futur = STOP_BALL_LAUNCHING; |
DOREL | 8:ad8b64ca548d | 146 | break; |
DOREL | 8:ad8b64ca548d | 147 | |
DOREL | 8:ad8b64ca548d | 148 | case STOP_BALL_LAUNCHING: //Add tempo ? |
DOREL | 8:ad8b64ca548d | 149 | etat_futur = TURN_TO_BASE; |
haarkon | 0:f00e68bef80c | 150 | break; |
DOREL | 8:ad8b64ca548d | 151 | |
DOREL | 8:ad8b64ca548d | 152 | case TURN_TO_BASE: |
DOREL | 8:ad8b64ca548d | 153 | if(face_our_side) etat_futur = RETURN; |
DOREL | 8:ad8b64ca548d | 154 | break; |
DOREL | 8:ad8b64ca548d | 155 | |
DOREL | 8:ad8b64ca548d | 156 | case RETURN: //return_done is a tempo ? |
DOREL | 8:ad8b64ca548d | 157 | if(return_done) etat_futur = STOP_RETURN; |
DOREL | 8:ad8b64ca548d | 158 | else if(wall) etat_futur = WALL_RETURN; |
haarkon | 0:f00e68bef80c | 159 | break; |
DOREL | 8:ad8b64ca548d | 160 | |
DOREL | 8:ad8b64ca548d | 161 | case WALL_RETURN: |
DOREL | 8:ad8b64ca548d | 162 | if(!wall) etat_futur = STOP_RETURN; |
DOREL | 8:ad8b64ca548d | 163 | break; |
DOREL | 8:ad8b64ca548d | 164 | |
DOREL | 8:ad8b64ca548d | 165 | case STOP_RETURN: |
DOREL | 8:ad8b64ca548d | 166 | etat_futur = SEEK_BALL; |
DOREL | 8:ad8b64ca548d | 167 | break; |
DOREL | 8:ad8b64ca548d | 168 | |
DOREL | 8:ad8b64ca548d | 169 | default : |
DOREL | 8:ad8b64ca548d | 170 | etat_futur = STOP_RETURN; |
haarkon | 0:f00e68bef80c | 171 | break; |
DOREL | 8:ad8b64ca548d | 172 | } |
DOREL | 8:ad8b64ca548d | 173 | |
DOREL | 8:ad8b64ca548d | 174 | switch(etat_actuel) { |
DOREL | 8:ad8b64ca548d | 175 | case START: |
DOREL | 8:ad8b64ca548d | 176 | orientation_camp_adversaire = (int)CAP_PWM; |
DOREL | 8:ad8b64ca548d | 177 | orientation_mon_camp =(((int)CAP_PWM+180)%360); |
DOREL | 8:ad8b64ca548d | 178 | break; |
DOREL | 8:ad8b64ca548d | 179 | |
DOREL | 8:ad8b64ca548d | 180 | case SEEK_BALL: |
DOREL | 8:ad8b64ca548d | 181 | break; |
DOREL | 8:ad8b64ca548d | 182 | |
DOREL | 8:ad8b64ca548d | 183 | case TURN_TO_BALL: |
DOREL | 8:ad8b64ca548d | 184 | break; |
DOREL | 8:ad8b64ca548d | 185 | |
DOREL | 8:ad8b64ca548d | 186 | case GO_TO_BALL: |
DOREL | 8:ad8b64ca548d | 187 | //Avancer droit (ralentir a la fin) |
haarkon | 0:f00e68bef80c | 188 | break; |
DOREL | 8:ad8b64ca548d | 189 | |
DOREL | 8:ad8b64ca548d | 190 | case CAPTURE_AND_TURN: |
DOREL | 8:ad8b64ca548d | 191 | //Baisser cage |
DOREL | 8:ad8b64ca548d | 192 | //Tourner |
DOREL | 8:ad8b64ca548d | 193 | //-------------------->Separer etapes ? |
DOREL | 8:ad8b64ca548d | 194 | break; |
DOREL | 8:ad8b64ca548d | 195 | |
DOREL | 8:ad8b64ca548d | 196 | case RELEASE_CAPTURE: |
DOREL | 8:ad8b64ca548d | 197 | //Lever cage |
DOREL | 8:ad8b64ca548d | 198 | break; |
DOREL | 8:ad8b64ca548d | 199 | |
DOREL | 8:ad8b64ca548d | 200 | case BALL_LAUNCHING: |
DOREL | 8:ad8b64ca548d | 201 | //Avancer droit a fond |
haarkon | 0:f00e68bef80c | 202 | break; |
DOREL | 8:ad8b64ca548d | 203 | |
DOREL | 8:ad8b64ca548d | 204 | case STOP_BALL_LAUNCHING: |
DOREL | 8:ad8b64ca548d | 205 | //Arreter |
DOREL | 8:ad8b64ca548d | 206 | |
DOREL | 8:ad8b64ca548d | 207 | break; |
DOREL | 8:ad8b64ca548d | 208 | |
DOREL | 8:ad8b64ca548d | 209 | case TURN_TO_BASE: |
DOREL | 8:ad8b64ca548d | 210 | //Faire demi tour |
haarkon | 0:f00e68bef80c | 211 | break; |
DOREL | 8:ad8b64ca548d | 212 | |
DOREL | 8:ad8b64ca548d | 213 | case RETURN: |
DOREL | 8:ad8b64ca548d | 214 | //Avancer pendant x secondes |
haarkon | 0:f00e68bef80c | 215 | break; |
DOREL | 8:ad8b64ca548d | 216 | |
DOREL | 8:ad8b64ca548d | 217 | case STOP_RETURN: |
DOREL | 8:ad8b64ca548d | 218 | //S'arreter |
DOREL | 8:ad8b64ca548d | 219 | break; |
DOREL | 8:ad8b64ca548d | 220 | |
DOREL | 8:ad8b64ca548d | 221 | default : |
DOREL | 8:ad8b64ca548d | 222 | break; |
haarkon | 0:f00e68bef80c | 223 | } |
haarkon | 0:f00e68bef80c | 224 | } |
haarkon | 0:f00e68bef80c | 225 | } |