Jonas DOREL / Mbed 2 deprecated Moteurs

Moteurs

Dependencies:   Encoder_Nucleo_16_bits PwmIn mbed

Fork of Automate by Jonas DOREL

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?

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