Moteurs
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of Automate by
main.cpp@9:168226ff8f76, 2017-06-09 (annotated)
- Committer:
- DOREL
- Date:
- Fri Jun 09 19:38:56 2017 +0000
- Revision:
- 9:168226ff8f76
- Parent:
- 8:ad8b64ca548d
Moteurs
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 | 2:1d440e938c44 | 15 | int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/; |
haarkon | 5:4955cb4b3646 | 16 | |
haarkon | 3:3dfe26a10796 | 17 | Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0; |
haarkon | 2:1d440e938c44 | 18 | int PIXY_objet; |
haarkon | 0:f00e68bef80c | 19 | |
haarkon | 6:88b4805d33e1 | 20 | int SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400; |
haarkon | 0:f00e68bef80c | 21 | T_SERVODIR SERVO_dir = S_monte; |
haarkon | 0:f00e68bef80c | 22 | |
haarkon | 0:f00e68bef80c | 23 | char MENU_choix = 0; |
haarkon | 0:f00e68bef80c | 24 | |
haarkon | 2:1d440e938c44 | 25 | char BOUSSOLE_status[1] = {0}; |
haarkon | 0:f00e68bef80c | 26 | char I2C_registerValue[4]; |
haarkon | 3:3dfe26a10796 | 27 | double BOUSSOLE_periode; |
haarkon | 0:f00e68bef80c | 28 | |
haarkon | 0:f00e68bef80c | 29 | double CAP_I2C, CAP_PWM; |
DOREL | 8:ad8b64ca548d | 30 | double CNY1_val, CNY2_val, CNY3_val, Vbat_val; |
DOREL | 8:ad8b64ca548d | 31 | double SD2_dist, LD2_dist; |
haarkon | 0:f00e68bef80c | 32 | |
DOREL | 8:ad8b64ca548d | 33 | //Variables Automate |
DOREL | 8:ad8b64ca548d | 34 | int etat_actuel = 0, etat_futur = 0, etat_precedent = 0; //Need etat_precedent ? |
DOREL | 8:ad8b64ca548d | 35 | |
DOREL | 8:ad8b64ca548d | 36 | //Variables test capteurs |
DOREL | 8:ad8b64ca548d | 37 | int ball_found = 0, ball_forward = 0, ball_captured = 0, face_ennemy_side = 0, face_our_side = 0; |
DOREL | 8:ad8b64ca548d | 38 | int wall = 0, white_line_crossed = 0, return_done = 0, rotation_done; //Rename white_line_crossed |
DOREL | 9:168226ff8f76 | 39 | |
DOREL | 9:168226ff8f76 | 40 | //Variables moteurs |
DOREL | 9:168226ff8f76 | 41 | |
DOREL | 9:168226ff8f76 | 42 | int md_pwm = 0; |
DOREL | 9:168226ff8f76 | 43 | int mg_pwm = 0; |
DOREL | 9:168226ff8f76 | 44 | int md_sens = 0; |
DOREL | 9:168226ff8f76 | 45 | int mg_sens = 0; |
DOREL | 8:ad8b64ca548d | 46 | |
DOREL | 8:ad8b64ca548d | 47 | //Variables |
DOREL | 8:ad8b64ca548d | 48 | |
DOREL | 8:ad8b64ca548d | 49 | int orientation_camp_adversaire, orientation_mon_camp; |
DOREL | 8:ad8b64ca548d | 50 | |
haarkon | 2:1d440e938c44 | 51 | times.reset(); |
haarkon | 2:1d440e938c44 | 52 | times.start(); |
haarkon | 0:f00e68bef80c | 53 | |
haarkon | 6:88b4805d33e1 | 54 | // Initialisation des interruptions |
haarkon | 2:1d440e938c44 | 55 | tick.attach(&tickTime, 0.001); |
haarkon | 0:f00e68bef80c | 56 | |
haarkon | 6:88b4805d33e1 | 57 | BP.rise (&BPevent); |
haarkon | 0:f00e68bef80c | 58 | |
haarkon | 6:88b4805d33e1 | 59 | Echo1.rise (&echo1Rise); |
haarkon | 6:88b4805d33e1 | 60 | Echo2.rise (&echo2Rise); |
haarkon | 6:88b4805d33e1 | 61 | Echo3.rise (&echo3Rise); |
haarkon | 6:88b4805d33e1 | 62 | Echo1.fall (&echo1Fall); |
haarkon | 6:88b4805d33e1 | 63 | Echo2.fall (&echo2Fall); |
haarkon | 6:88b4805d33e1 | 64 | Echo3.fall (&echo3Fall); |
haarkon | 6:88b4805d33e1 | 65 | |
haarkon | 6:88b4805d33e1 | 66 | PWMG.rise (&PWM_motGRise); |
haarkon | 6:88b4805d33e1 | 67 | PWMD.rise (&PWM_motDRise); |
haarkon | 6:88b4805d33e1 | 68 | |
haarkon | 6:88b4805d33e1 | 69 | Pixy.attach (&getPixyByte); |
haarkon | 0:f00e68bef80c | 70 | |
haarkon | 1:d95546f84105 | 71 | BP.enable_irq(); |
haarkon | 0:f00e68bef80c | 72 | IG.enable_irq(); |
haarkon | 0:f00e68bef80c | 73 | Echo1.enable_irq(); |
haarkon | 0:f00e68bef80c | 74 | Echo2.enable_irq(); |
haarkon | 0:f00e68bef80c | 75 | Echo3.enable_irq(); |
haarkon | 6:88b4805d33e1 | 76 | PWMG.enable_irq(); |
haarkon | 6:88b4805d33e1 | 77 | PWMD.enable_irq(); |
haarkon | 0:f00e68bef80c | 78 | |
haarkon | 6:88b4805d33e1 | 79 | // Initialisation des périphériques |
haarkon | 6:88b4805d33e1 | 80 | // Bus I2C |
haarkon | 6:88b4805d33e1 | 81 | Bus_I2C.frequency (100000); |
haarkon | 0:f00e68bef80c | 82 | |
haarkon | 6:88b4805d33e1 | 83 | // PWM des moteurs |
haarkon | 0:f00e68bef80c | 84 | Pwm_MG.period_us(50); |
haarkon | 0:f00e68bef80c | 85 | Pwm_MD.period_us(50); |
haarkon | 0:f00e68bef80c | 86 | En = 0; |
haarkon | 5:4955cb4b3646 | 87 | |
haarkon | 6:88b4805d33e1 | 88 | // Bus SPI |
haarkon | 6:88b4805d33e1 | 89 | SPIG.format (16,1); |
haarkon | 6:88b4805d33e1 | 90 | SPIG.frequency (1000000); |
haarkon | 6:88b4805d33e1 | 91 | |
haarkon | 6:88b4805d33e1 | 92 | SPID.format (16,1); |
haarkon | 6:88b4805d33e1 | 93 | SPID.frequency (1000000); |
haarkon | 6:88b4805d33e1 | 94 | |
haarkon | 6:88b4805d33e1 | 95 | SS = 1; |
haarkon | 6:88b4805d33e1 | 96 | |
haarkon | 6:88b4805d33e1 | 97 | // Led |
haarkon | 0:f00e68bef80c | 98 | Led2 = 0; |
haarkon | 0:f00e68bef80c | 99 | |
haarkon | 6:88b4805d33e1 | 100 | Servo.period_ms (20); |
haarkon | 6:88b4805d33e1 | 101 | Servo.pulsewidth_us(200); |
DOREL | 8:ad8b64ca548d | 102 | |
DOREL | 9:168226ff8f76 | 103 | //Moteurs |
DOREL | 9:168226ff8f76 | 104 | En = 1; |
DOREL | 9:168226ff8f76 | 105 | |
DOREL | 9:168226ff8f76 | 106 | while(1) { |
DOREL | 9:168226ff8f76 | 107 | int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/; |
DOREL | 9:168226ff8f76 | 108 | |
DOREL | 9:168226ff8f76 | 109 | Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0; |
DOREL | 9:168226ff8f76 | 110 | int PIXY_objet; |
DOREL | 9:168226ff8f76 | 111 | |
DOREL | 9:168226ff8f76 | 112 | int SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400; |
DOREL | 9:168226ff8f76 | 113 | T_SERVODIR SERVO_dir = S_monte; |
DOREL | 9:168226ff8f76 | 114 | |
DOREL | 9:168226ff8f76 | 115 | char MENU_choix = 0; |
DOREL | 9:168226ff8f76 | 116 | |
DOREL | 9:168226ff8f76 | 117 | char BOUSSOLE_status[1] = {0}; |
DOREL | 9:168226ff8f76 | 118 | char I2C_registerValue[4]; |
DOREL | 9:168226ff8f76 | 119 | double BOUSSOLE_periode; |
DOREL | 9:168226ff8f76 | 120 | |
DOREL | 9:168226ff8f76 | 121 | double CAP_I2C, CAP_PWM; |
DOREL | 9:168226ff8f76 | 122 | double SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val; |
DOREL | 9:168226ff8f76 | 123 | double SD1_dist, SD2_dist, LD1_dist, LD2_dist; |
DOREL | 9:168226ff8f76 | 124 | |
DOREL | 9:168226ff8f76 | 125 | |
DOREL | 8:ad8b64ca548d | 126 | //capteurs_read(); //Lecture tous les capteurs |
DOREL | 8:ad8b64ca548d | 127 | CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10; |
DOREL | 9:168226ff8f76 | 128 | |
DOREL | 8:ad8b64ca548d | 129 | //capteurs_test(); //Teste les valeurs des capteurs et change des variables booleenne |
DOREL | 9:168226ff8f76 | 130 | face_ennemy_side = (((int)CAP_PWM > (orientation_camp_adversaire - 10))&((int)CAP_PWM < (orientation_camp_adversaire + 10))); |
DOREL | 9:168226ff8f76 | 131 | |
DOREL | 9:168226ff8f76 | 132 | //Actions moteurs |
DOREL | 9:168226ff8f76 | 133 | Pwm_MD = md_pwm; |
DOREL | 9:168226ff8f76 | 134 | Pwm_MG = mg_pwm; |
DOREL | 9:168226ff8f76 | 135 | SensD = md_sens; |
DOREL | 9:168226ff8f76 | 136 | SensG = mg_sens; |
DOREL | 9:168226ff8f76 | 137 | |
DOREL | 9:168226ff8f76 | 138 | //Test |
DOREL | 9:168226ff8f76 | 139 | |
DOREL | 9:168226ff8f76 | 140 | motor_command(100, 100, *mg_pwm, *md_pwm, *mg_sens, *md_sens); |
DOREL | 9:168226ff8f76 | 141 | |
DOREL | 9:168226ff8f76 | 142 | //Action cage |
DOREL | 9:168226ff8f76 | 143 | |
DOREL | 9:168226ff8f76 | 144 | //Automate |
DOREL | 8:ad8b64ca548d | 145 | etat_actuel = etat_futur; |
DOREL | 8:ad8b64ca548d | 146 | |
DOREL | 8:ad8b64ca548d | 147 | switch(etat_actuel) { |
DOREL | 8:ad8b64ca548d | 148 | case START: |
DOREL | 8:ad8b64ca548d | 149 | etat_futur = STOP_RETURN; |
DOREL | 8:ad8b64ca548d | 150 | break; |
DOREL | 8:ad8b64ca548d | 151 | |
DOREL | 8:ad8b64ca548d | 152 | case SEEK_BALL: |
DOREL | 8:ad8b64ca548d | 153 | if(ball_found) etat_futur = TURN_TO_BALL; |
DOREL | 8:ad8b64ca548d | 154 | if(!ball_found) etat_futur = SEEK_ROTATION; |
DOREL | 8:ad8b64ca548d | 155 | break; |
DOREL | 8:ad8b64ca548d | 156 | |
DOREL | 8:ad8b64ca548d | 157 | case SEEK_ROTATION: |
DOREL | 8:ad8b64ca548d | 158 | if(rotation_done) etat_futur = SEEK_BALL; |
DOREL | 8:ad8b64ca548d | 159 | |
DOREL | 8:ad8b64ca548d | 160 | case TURN_TO_BALL: |
DOREL | 8:ad8b64ca548d | 161 | if(ball_forward) etat_futur = GO_TO_BALL; |
DOREL | 8:ad8b64ca548d | 162 | break; |
DOREL | 8:ad8b64ca548d | 163 | |
DOREL | 8:ad8b64ca548d | 164 | case GO_TO_BALL: |
DOREL | 8:ad8b64ca548d | 165 | if(!ball_forward) etat_futur = TURN_TO_BALL; |
DOREL | 8:ad8b64ca548d | 166 | if(ball_captured & face_ennemy_side) etat_futur = BALL_LAUNCHING; |
DOREL | 8:ad8b64ca548d | 167 | if(ball_captured & !face_ennemy_side) etat_futur = CAPTURE_AND_TURN; |
haarkon | 0:f00e68bef80c | 168 | break; |
DOREL | 8:ad8b64ca548d | 169 | |
DOREL | 8:ad8b64ca548d | 170 | case CAPTURE_AND_TURN: |
DOREL | 8:ad8b64ca548d | 171 | if(face_ennemy_side) etat_futur = RELEASE_CAPTURE; |
DOREL | 8:ad8b64ca548d | 172 | break; |
DOREL | 8:ad8b64ca548d | 173 | |
DOREL | 8:ad8b64ca548d | 174 | case WALL_CAPTURE_AND_TURN: |
DOREL | 8:ad8b64ca548d | 175 | if(!wall & face_ennemy_side) etat_futur = RELEASE_CAPTURE; |
DOREL | 8:ad8b64ca548d | 176 | break; |
DOREL | 8:ad8b64ca548d | 177 | |
DOREL | 8:ad8b64ca548d | 178 | case RELEASE_CAPTURE: |
DOREL | 8:ad8b64ca548d | 179 | etat_futur = BALL_LAUNCHING; |
haarkon | 0:f00e68bef80c | 180 | break; |
DOREL | 8:ad8b64ca548d | 181 | |
DOREL | 8:ad8b64ca548d | 182 | case BALL_LAUNCHING: |
DOREL | 8:ad8b64ca548d | 183 | if(wall) etat_futur = WALL_CAPTURE_AND_TURN ; |
DOREL | 8:ad8b64ca548d | 184 | if(white_line_crossed) etat_futur = STOP_BALL_LAUNCHING; |
DOREL | 8:ad8b64ca548d | 185 | break; |
DOREL | 8:ad8b64ca548d | 186 | |
DOREL | 8:ad8b64ca548d | 187 | case STOP_BALL_LAUNCHING: //Add tempo ? |
DOREL | 8:ad8b64ca548d | 188 | etat_futur = TURN_TO_BASE; |
haarkon | 0:f00e68bef80c | 189 | break; |
DOREL | 8:ad8b64ca548d | 190 | |
DOREL | 8:ad8b64ca548d | 191 | case TURN_TO_BASE: |
DOREL | 8:ad8b64ca548d | 192 | if(face_our_side) etat_futur = RETURN; |
DOREL | 8:ad8b64ca548d | 193 | break; |
DOREL | 8:ad8b64ca548d | 194 | |
DOREL | 8:ad8b64ca548d | 195 | case RETURN: //return_done is a tempo ? |
DOREL | 8:ad8b64ca548d | 196 | if(return_done) etat_futur = STOP_RETURN; |
DOREL | 8:ad8b64ca548d | 197 | else if(wall) etat_futur = WALL_RETURN; |
haarkon | 0:f00e68bef80c | 198 | break; |
DOREL | 8:ad8b64ca548d | 199 | |
DOREL | 8:ad8b64ca548d | 200 | case WALL_RETURN: |
DOREL | 8:ad8b64ca548d | 201 | if(!wall) etat_futur = STOP_RETURN; |
DOREL | 8:ad8b64ca548d | 202 | break; |
DOREL | 8:ad8b64ca548d | 203 | |
DOREL | 8:ad8b64ca548d | 204 | case STOP_RETURN: |
DOREL | 8:ad8b64ca548d | 205 | etat_futur = SEEK_BALL; |
DOREL | 8:ad8b64ca548d | 206 | break; |
DOREL | 8:ad8b64ca548d | 207 | |
DOREL | 8:ad8b64ca548d | 208 | default : |
DOREL | 8:ad8b64ca548d | 209 | etat_futur = STOP_RETURN; |
haarkon | 0:f00e68bef80c | 210 | break; |
DOREL | 8:ad8b64ca548d | 211 | } |
DOREL | 8:ad8b64ca548d | 212 | |
DOREL | 8:ad8b64ca548d | 213 | switch(etat_actuel) { |
DOREL | 8:ad8b64ca548d | 214 | case START: |
DOREL | 8:ad8b64ca548d | 215 | orientation_camp_adversaire = (int)CAP_PWM; |
DOREL | 8:ad8b64ca548d | 216 | orientation_mon_camp =(((int)CAP_PWM+180)%360); |
DOREL | 8:ad8b64ca548d | 217 | break; |
DOREL | 8:ad8b64ca548d | 218 | |
DOREL | 8:ad8b64ca548d | 219 | case SEEK_BALL: |
DOREL | 8:ad8b64ca548d | 220 | break; |
DOREL | 8:ad8b64ca548d | 221 | |
DOREL | 8:ad8b64ca548d | 222 | case TURN_TO_BALL: |
DOREL | 8:ad8b64ca548d | 223 | break; |
DOREL | 8:ad8b64ca548d | 224 | |
DOREL | 8:ad8b64ca548d | 225 | case GO_TO_BALL: |
DOREL | 8:ad8b64ca548d | 226 | //Avancer droit (ralentir a la fin) |
haarkon | 0:f00e68bef80c | 227 | break; |
DOREL | 8:ad8b64ca548d | 228 | |
DOREL | 8:ad8b64ca548d | 229 | case CAPTURE_AND_TURN: |
DOREL | 8:ad8b64ca548d | 230 | //Baisser cage |
DOREL | 8:ad8b64ca548d | 231 | //Tourner |
DOREL | 8:ad8b64ca548d | 232 | //-------------------->Separer etapes ? |
DOREL | 8:ad8b64ca548d | 233 | break; |
DOREL | 8:ad8b64ca548d | 234 | |
DOREL | 8:ad8b64ca548d | 235 | case RELEASE_CAPTURE: |
DOREL | 8:ad8b64ca548d | 236 | //Lever cage |
DOREL | 8:ad8b64ca548d | 237 | break; |
DOREL | 8:ad8b64ca548d | 238 | |
DOREL | 8:ad8b64ca548d | 239 | case BALL_LAUNCHING: |
DOREL | 8:ad8b64ca548d | 240 | //Avancer droit a fond |
haarkon | 0:f00e68bef80c | 241 | break; |
DOREL | 8:ad8b64ca548d | 242 | |
DOREL | 8:ad8b64ca548d | 243 | case STOP_BALL_LAUNCHING: |
DOREL | 8:ad8b64ca548d | 244 | //Arreter |
DOREL | 8:ad8b64ca548d | 245 | |
DOREL | 8:ad8b64ca548d | 246 | break; |
DOREL | 8:ad8b64ca548d | 247 | |
DOREL | 8:ad8b64ca548d | 248 | case TURN_TO_BASE: |
DOREL | 8:ad8b64ca548d | 249 | //Faire demi tour |
haarkon | 0:f00e68bef80c | 250 | break; |
DOREL | 8:ad8b64ca548d | 251 | |
DOREL | 8:ad8b64ca548d | 252 | case RETURN: |
DOREL | 8:ad8b64ca548d | 253 | //Avancer pendant x secondes |
haarkon | 0:f00e68bef80c | 254 | break; |
DOREL | 8:ad8b64ca548d | 255 | |
DOREL | 8:ad8b64ca548d | 256 | case STOP_RETURN: |
DOREL | 8:ad8b64ca548d | 257 | //S'arreter |
DOREL | 8:ad8b64ca548d | 258 | break; |
DOREL | 8:ad8b64ca548d | 259 | |
DOREL | 8:ad8b64ca548d | 260 | default : |
DOREL | 8:ad8b64ca548d | 261 | break; |
haarkon | 0:f00e68bef80c | 262 | } |
haarkon | 0:f00e68bef80c | 263 | } |
haarkon | 0:f00e68bef80c | 264 | } |