Jonas DOREL / Mbed 2 deprecated Moteurs

Moteurs

Dependencies:   Encoder_Nucleo_16_bits PwmIn mbed

Fork of Automate by Jonas DOREL

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
Child:
9:168226ff8f76
Automate;

Who changed what in which revision?

UserRevisionLine numberNew 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 }