code strat avant match 2, strat inversée OK normalement
Fork of CRAC-Strat_2017_fin_premier_match by
Telemetre/Telemetre.cpp@14:c8fc06c4887f, 2017-03-31 (annotated)
- Committer:
- ClementBreteau
- Date:
- Fri Mar 31 16:20:26 2017 +0000
- Revision:
- 14:c8fc06c4887f
- Child:
- 15:c2fc239e85df
code strategie du robot, version 31 mars 2017
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ClementBreteau | 14:c8fc06c4887f | 1 | # include "Telemetre.h" |
ClementBreteau | 14:c8fc06c4887f | 2 | |
ClementBreteau | 14:c8fc06c4887f | 3 | #define TELEMETRE_PROFONDEUR 50 |
ClementBreteau | 14:c8fc06c4887f | 4 | |
ClementBreteau | 14:c8fc06c4887f | 5 | T_LISTE_MODULES listeModules; |
ClementBreteau | 14:c8fc06c4887f | 6 | |
ClementBreteau | 14:c8fc06c4887f | 7 | /*********************************************************************************************************/ |
ClementBreteau | 14:c8fc06c4887f | 8 | /* FUNCTION NAME: SendTelemetreID */ |
ClementBreteau | 14:c8fc06c4887f | 9 | /* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */ |
ClementBreteau | 14:c8fc06c4887f | 10 | /*********************************************************************************************************/ |
ClementBreteau | 14:c8fc06c4887f | 11 | void SendTelemetreID (unsigned short id) |
ClementBreteau | 14:c8fc06c4887f | 12 | { |
ClementBreteau | 14:c8fc06c4887f | 13 | CANMessage msgTx=CANMessage(); |
ClementBreteau | 14:c8fc06c4887f | 14 | msgTx.id=id; |
ClementBreteau | 14:c8fc06c4887f | 15 | msgTx.len=0; |
ClementBreteau | 14:c8fc06c4887f | 16 | can1.write(msgTx); |
ClementBreteau | 14:c8fc06c4887f | 17 | wait_us(200); |
ClementBreteau | 14:c8fc06c4887f | 18 | } |
ClementBreteau | 14:c8fc06c4887f | 19 | |
ClementBreteau | 14:c8fc06c4887f | 20 | /*********************************************************************************************************/ |
ClementBreteau | 14:c8fc06c4887f | 21 | /* FUNCTION NAME: EspaceLibre */ |
ClementBreteau | 14:c8fc06c4887f | 22 | /* DESCRIPTION : Calcule si la place et libre pour poser un module */ |
ClementBreteau | 14:c8fc06c4887f | 23 | /*********************************************************************************************************/ |
ClementBreteau | 14:c8fc06c4887f | 24 | bool EspaceLibre (signed short xModule, signed short yModule, signed short xRobot, signed short yRobot){ |
ClementBreteau | 14:c8fc06c4887f | 25 | return true; |
ClementBreteau | 14:c8fc06c4887f | 26 | |
ClementBreteau | 14:c8fc06c4887f | 27 | } |
ClementBreteau | 14:c8fc06c4887f | 28 | |
ClementBreteau | 14:c8fc06c4887f | 29 | /*********************************************************************************************************/ |
ClementBreteau | 14:c8fc06c4887f | 30 | /* FUNCTION NAME: BonnePlace */ |
ClementBreteau | 14:c8fc06c4887f | 31 | /* DESCRIPTION : Calcule si le module est bien placé */ |
ClementBreteau | 14:c8fc06c4887f | 32 | /*********************************************************************************************************/ |
ClementBreteau | 14:c8fc06c4887f | 33 | bool BonnePlace(signed short xModule, signed short yModule, signed short xRobot, signed short yRobot){ |
ClementBreteau | 14:c8fc06c4887f | 34 | bool bienPlace = false; |
ClementBreteau | 14:c8fc06c4887f | 35 | double distance = abs(xModule - xRobot)*abs(yModule - yRobot) + abs(yModule - yRobot)*abs(yModule - yRobot); |
ClementBreteau | 14:c8fc06c4887f | 36 | |
ClementBreteau | 14:c8fc06c4887f | 37 | distance = sqrt(distance); |
ClementBreteau | 14:c8fc06c4887f | 38 | |
ClementBreteau | 14:c8fc06c4887f | 39 | |
ClementBreteau | 14:c8fc06c4887f | 40 | if ((TELEMETRE_PROFONDEUR - 10 < TELEMETRE_PROFONDEUR )||( TELEMETRE_PROFONDEUR + 10 > TELEMETRE_PROFONDEUR)){ |
ClementBreteau | 14:c8fc06c4887f | 41 | bienPlace = true; |
ClementBreteau | 14:c8fc06c4887f | 42 | } |
ClementBreteau | 14:c8fc06c4887f | 43 | return bienPlace; |
ClementBreteau | 14:c8fc06c4887f | 44 | } |
ClementBreteau | 14:c8fc06c4887f | 45 | |
ClementBreteau | 14:c8fc06c4887f | 46 | void initModules(void){ |
ClementBreteau | 14:c8fc06c4887f | 47 | |
ClementBreteau | 14:c8fc06c4887f | 48 | |
ClementBreteau | 14:c8fc06c4887f | 49 | /*listeModules ={true, 1350, 0}, |
ClementBreteau | 14:c8fc06c4887f | 50 | {true, 0, 1150}};*/ |
ClementBreteau | 14:c8fc06c4887f | 51 | |
ClementBreteau | 14:c8fc06c4887f | 52 | listeModules.module1.x=600; |
ClementBreteau | 14:c8fc06c4887f | 53 | listeModules.module1.y=1000; |
ClementBreteau | 14:c8fc06c4887f | 54 | listeModules.module1.existe = true; |
ClementBreteau | 14:c8fc06c4887f | 55 | listeModules.module1.timeout = 0; |
ClementBreteau | 14:c8fc06c4887f | 56 | |
ClementBreteau | 14:c8fc06c4887f | 57 | listeModules.module2.x=600; |
ClementBreteau | 14:c8fc06c4887f | 58 | listeModules.module2.y=200; |
ClementBreteau | 14:c8fc06c4887f | 59 | listeModules.module2.existe = true; |
ClementBreteau | 14:c8fc06c4887f | 60 | listeModules.module2.timeout = 0; |
ClementBreteau | 14:c8fc06c4887f | 61 | |
ClementBreteau | 14:c8fc06c4887f | 62 | listeModules.module3.x=1100; |
ClementBreteau | 14:c8fc06c4887f | 63 | listeModules.module3.y=500; |
ClementBreteau | 14:c8fc06c4887f | 64 | listeModules.module3.existe = true; |
ClementBreteau | 14:c8fc06c4887f | 65 | listeModules.module3.timeout = 0; |
ClementBreteau | 14:c8fc06c4887f | 66 | |
ClementBreteau | 14:c8fc06c4887f | 67 | listeModules.module4.x=1400; |
ClementBreteau | 14:c8fc06c4887f | 68 | listeModules.module4.y=900; |
ClementBreteau | 14:c8fc06c4887f | 69 | listeModules.module4.existe = true; |
ClementBreteau | 14:c8fc06c4887f | 70 | listeModules.module4.timeout = 0; |
ClementBreteau | 14:c8fc06c4887f | 71 | |
ClementBreteau | 14:c8fc06c4887f | 72 | listeModules.module5.x=1850; |
ClementBreteau | 14:c8fc06c4887f | 73 | listeModules.module5.y=800; |
ClementBreteau | 14:c8fc06c4887f | 74 | listeModules.module5.existe = true; |
ClementBreteau | 14:c8fc06c4887f | 75 | listeModules.module5.timeout = 0; |
ClementBreteau | 14:c8fc06c4887f | 76 | |
ClementBreteau | 14:c8fc06c4887f | 77 | listeModules.fusee1.x = 1350; |
ClementBreteau | 14:c8fc06c4887f | 78 | listeModules.fusee1.y = 0; |
ClementBreteau | 14:c8fc06c4887f | 79 | listeModules.fusee1.nb_module = 4; |
ClementBreteau | 14:c8fc06c4887f | 80 | listeModules.fusee1.timeout = 0 ; |
ClementBreteau | 14:c8fc06c4887f | 81 | |
ClementBreteau | 14:c8fc06c4887f | 82 | listeModules.fusee2.x = 0; |
ClementBreteau | 14:c8fc06c4887f | 83 | listeModules.fusee2.y = 1150; |
ClementBreteau | 14:c8fc06c4887f | 84 | listeModules.fusee2.nb_module = 4; |
ClementBreteau | 14:c8fc06c4887f | 85 | listeModules.fusee2.timeout = 0; |
ClementBreteau | 14:c8fc06c4887f | 86 | |
ClementBreteau | 14:c8fc06c4887f | 87 | } |
ClementBreteau | 14:c8fc06c4887f | 88 | |
ClementBreteau | 14:c8fc06c4887f | 89 | |
ClementBreteau | 14:c8fc06c4887f | 90 | void processData(int objectType, signed short x, signed short y, int nb_module){ |
ClementBreteau | 14:c8fc06c4887f | 91 | switch(objectType){ |
ClementBreteau | 14:c8fc06c4887f | 92 | case CYLINDRE: |
ClementBreteau | 14:c8fc06c4887f | 93 | // |
ClementBreteau | 14:c8fc06c4887f | 94 | break; |
ClementBreteau | 14:c8fc06c4887f | 95 | |
ClementBreteau | 14:c8fc06c4887f | 96 | case MODULE: |
ClementBreteau | 14:c8fc06c4887f | 97 | // |
ClementBreteau | 14:c8fc06c4887f | 98 | if( (x == listeModules.module1.x) && (y == listeModules.module1.y) ){ |
ClementBreteau | 14:c8fc06c4887f | 99 | printf("module 1"); |
ClementBreteau | 14:c8fc06c4887f | 100 | listeModules.module1.timeout ++; |
ClementBreteau | 14:c8fc06c4887f | 101 | } |
ClementBreteau | 14:c8fc06c4887f | 102 | break; |
ClementBreteau | 14:c8fc06c4887f | 103 | |
ClementBreteau | 14:c8fc06c4887f | 104 | case BALLE: |
ClementBreteau | 14:c8fc06c4887f | 105 | // |
ClementBreteau | 14:c8fc06c4887f | 106 | break; |
ClementBreteau | 14:c8fc06c4887f | 107 | } |
ClementBreteau | 14:c8fc06c4887f | 108 | |
ClementBreteau | 14:c8fc06c4887f | 109 | } |