Programme carte strategie (disco)

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Committer:
antbig
Date:
Fri May 26 19:01:56 2017 +0000
Revision:
27:76ead555a63d
Parent:
11:ed13a480ddca
Child:
29:41e02746041d
Debut du code pour le match 5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
antbig 0:ad97421fb1fb 1 #include "debug.h"
antbig 0:ad97421fb1fb 2
antbig 0:ad97421fb1fb 3 void debug_Instruction(struct S_Instruction instruction)
antbig 0:ad97421fb1fb 4 {
antbig 0:ad97421fb1fb 5 printf("\n********* Debug instruction *********\n");
antbig 0:ad97421fb1fb 6
antbig 0:ad97421fb1fb 7 printf("* Line => %d\n", instruction.lineNumber);
antbig 0:ad97421fb1fb 8 printf("* Type => %s\n", InstructionTypeToString(instruction.order));
antbig 0:ad97421fb1fb 9 printf("* Direction => %s\n", InstructionDirectionToString(instruction.direction));
antbig 0:ad97421fb1fb 10
antbig 0:ad97421fb1fb 11 printf("* Arg1 => %d\n", instruction.arg1);
antbig 0:ad97421fb1fb 12 printf("* Arg2 => %d\n", instruction.arg2);
antbig 0:ad97421fb1fb 13 printf("* Arg3 => %d\n", instruction.arg3);
antbig 0:ad97421fb1fb 14
antbig 0:ad97421fb1fb 15 printf("* Recalage => %s\n", InstructionPrecisionOuRecalageToString(instruction.precision));
antbig 0:ad97421fb1fb 16 printf("* NextAction=> %s\n", InstructionNextActionTypeToString(instruction.nextActionType));
antbig 0:ad97421fb1fb 17 printf("* JumpAction=> %s\n", InstructionNextActionJumpTypeToString(instruction.jumpAction));
antbig 0:ad97421fb1fb 18
antbig 0:ad97421fb1fb 19 printf("* JumpTimeOrX => %d\n", instruction.JumpTimeOrX);
antbig 0:ad97421fb1fb 20 printf("* JumpY => %d\n", instruction.JumpY);
antbig 0:ad97421fb1fb 21 printf("* nextLineOK => %d\n", instruction.nextLineOK);
antbig 0:ad97421fb1fb 22 printf("* nextLineError => %d\n", instruction.nextLineError);
antbig 0:ad97421fb1fb 23
antbig 0:ad97421fb1fb 24 printf("*************************************\n");
antbig 0:ad97421fb1fb 25 }
antbig 0:ad97421fb1fb 26
antbig 0:ad97421fb1fb 27 char* InstructionTypeToString(enum E_InstructionType type)
antbig 0:ad97421fb1fb 28 {
antbig 0:ad97421fb1fb 29 switch(type)
antbig 0:ad97421fb1fb 30 {
antbig 0:ad97421fb1fb 31 case MV_COURBURE: return "Courbure";
antbig 0:ad97421fb1fb 32 case MV_LINE: return "Ligne";
antbig 0:ad97421fb1fb 33 case MV_TURN: return "Rotation";
antbig 0:ad97421fb1fb 34 case MV_XYT: return "Position XYT";
antbig 0:ad97421fb1fb 35 case MV_RECALAGE: return "Recalage";
antbig 0:ad97421fb1fb 36 case ACTION: return "Action";
antbig 0:ad97421fb1fb 37 default: return "Inconnue";
antbig 0:ad97421fb1fb 38 }
antbig 0:ad97421fb1fb 39 }
antbig 0:ad97421fb1fb 40 char* InstructionDirectionToString(enum E_InstructionDirection type)
antbig 0:ad97421fb1fb 41 {
antbig 0:ad97421fb1fb 42 switch(type)
antbig 0:ad97421fb1fb 43 {
antbig 0:ad97421fb1fb 44 case BACKWARD: return "en arriere";
antbig 0:ad97421fb1fb 45 case FORWARD: return "en avant";
antbig 0:ad97421fb1fb 46 case RELATIVE: return "relatif";
antbig 0:ad97421fb1fb 47 case ABSOLUTE: return "absolu";
antbig 0:ad97421fb1fb 48 default: return "absent";
antbig 0:ad97421fb1fb 49 }
antbig 0:ad97421fb1fb 50 }
antbig 0:ad97421fb1fb 51 char* InstructionPrecisionOuRecalageToString(enum E_InstructionPrecisionOuRecalage type)
antbig 0:ad97421fb1fb 52 {
antbig 0:ad97421fb1fb 53 switch(type)
antbig 0:ad97421fb1fb 54 {
antbig 0:ad97421fb1fb 55 case PRECISION: return "correction position en fin de mouvement";
antbig 0:ad97421fb1fb 56 case RECALAGE_X: return "recalage en X";
antbig 0:ad97421fb1fb 57 case RECALAGE_Y: return "recalage en Y";
antbig 0:ad97421fb1fb 58 default: return "absent";
antbig 0:ad97421fb1fb 59 }
antbig 0:ad97421fb1fb 60 }
antbig 0:ad97421fb1fb 61 char* InstructionNextActionTypeToString(enum E_InstructionNextActionType type)
antbig 0:ad97421fb1fb 62 {
antbig 0:ad97421fb1fb 63 switch(type)
antbig 0:ad97421fb1fb 64 {
antbig 0:ad97421fb1fb 65 case JUMP: return "jump";
antbig 0:ad97421fb1fb 66 case WAIT: return "attente fin instruction";
antbig 0:ad97421fb1fb 67 case ENCHAINEMENT: return "enchainement";
antbig 0:ad97421fb1fb 68 default: return "absent";
antbig 0:ad97421fb1fb 69 }
antbig 0:ad97421fb1fb 70 }
antbig 0:ad97421fb1fb 71 char* InstructionNextActionJumpTypeToString(enum E_InstructionNextActionJumpType type)
antbig 0:ad97421fb1fb 72 {
antbig 0:ad97421fb1fb 73 switch(type)
antbig 0:ad97421fb1fb 74 {
antbig 0:ad97421fb1fb 75 case JUMP_TIME: return "attente temps";
antbig 0:ad97421fb1fb 76 case JUMP_POSITION: return "attente position";
antbig 0:ad97421fb1fb 77 default: return "absent";
antbig 0:ad97421fb1fb 78 }
antbig 0:ad97421fb1fb 79 }
antbig 0:ad97421fb1fb 80
antbig 0:ad97421fb1fb 81 void errorLoop(void) {
antbig 0:ad97421fb1fb 82 while(true) {
antbig 0:ad97421fb1fb 83 led1 = 1;
antbig 0:ad97421fb1fb 84 wait(0.2);
antbig 0:ad97421fb1fb 85 led1 = 0;
antbig 0:ad97421fb1fb 86 wait(0.2);
antbig 0:ad97421fb1fb 87 }
antbig 0:ad97421fb1fb 88 }
antbig 0:ad97421fb1fb 89
antbig 1:116040d14164 90 void errorInstructionLoop(void) {
antbig 1:116040d14164 91 while(true) {
antbig 1:116040d14164 92 led2 = 1;
antbig 1:116040d14164 93 wait(0.2);
antbig 1:116040d14164 94 led2 = 0;
antbig 1:116040d14164 95 wait(0.2);
antbig 1:116040d14164 96 }
antbig 1:116040d14164 97 }
antbig 1:116040d14164 98
antbig 0:ad97421fb1fb 99 /****************************************************************************************/
antbig 0:ad97421fb1fb 100 /* FUNCTION NAME: sendStratEtat */
antbig 0:ad97421fb1fb 101 /* DESCRIPTION : permet le debug de l'etat de l'automate de stratégie */
antbig 0:ad97421fb1fb 102 /****************************************************************************************/
antbig 11:ed13a480ddca 103 void sendStratEtat(unsigned char etat, unsigned char currentInstruction) {
antbig 0:ad97421fb1fb 104
antbig 0:ad97421fb1fb 105 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 106 msgTx.id=DEBUG_STRATEGIE_AUTOMATE; // tx nouvelle position en (x,y,theta)
antbig 0:ad97421fb1fb 107 msgTx.len=1;
antbig 0:ad97421fb1fb 108 msgTx.format=CANStandard;
antbig 0:ad97421fb1fb 109 msgTx.type=CANData;
antbig 0:ad97421fb1fb 110 // x sur 2 octets
antbig 0:ad97421fb1fb 111 msgTx.data[0]=(unsigned char)etat;
antbig 11:ed13a480ddca 112 msgTx.data[1]=(unsigned char)currentInstruction;
antbig 0:ad97421fb1fb 113
antbig 0:ad97421fb1fb 114 can1.write(msgTx);
antbig 27:76ead555a63d 115 }
antbig 27:76ead555a63d 116
antbig 27:76ead555a63d 117 void debugXYTTarget(signed short target_x_robot, signed short target_y_robot, signed short target_theta_robot) {
antbig 27:76ead555a63d 118 printf("going to new position x: %d, y: %d, t: %d\n",target_x_robot,target_y_robot,target_theta_robot);
antbig 0:ad97421fb1fb 119 }