Diff: MY_Asserv.cpp

Revision:

1:162aa6608ffe

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MY_Asserv.cpp	Wed Apr 19 17:41:35 2017 +0000
@@ -0,0 +1,88 @@
+#include "mbed.h"
+#include "Nucleo_Encoder_16_bits.h"
+#include "MYCRAC_utility.h"
+#include "Asservissement.h"
+#include "global.h"
+#include "MY_Asserv.h"
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: Send_assev_info_comfig                                                */
+/* DESCRIPTION  : Envoie un asservissement paramètre retournant à une vitesse           */
+/****************************************************************************************/
+void Send_assev_info_comfig(void)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_INFO_CONSIGNE;   // si proposition accepter remplacer par "ACKNOWLEDGE_MOTEUR"
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=6;
+    msgTx.data[0]=(unsigned char)(((short)Kpp1)&0xFF);
+    msgTx.data[1]=(unsigned char)(((short)Kpp1 >> 8)&0xFF);
+    msgTx.data[2]=(unsigned char)(((short)Kip1)&0xFF);
+    msgTx.data[3]=(unsigned char)(((short)Kip1 >> 8)&0xFF);
+    msgTx.data[4]=(unsigned char)(((short)Kdp1)&0xFF);
+    msgTx.data[5]=(unsigned char)(((short)Kdp1 >> 8)&0xFF);
+
+    can1.write(msgTx);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Send_odometrie                                                        */
+/* DESCRIPTION  : Envoie  de l'odometrie                                                */
+/****************************************************************************************/
+void Send_odometrie(void)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ODOMETRIE_SMALL_POSITION;  // si proposition accepter remplacer par "ACKNOWLEDGE_MOTEUR"
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=6;
+    msgTx.data[0]=((short)Odo_x) & 0xFF;
+    msgTx.data[1]=((short)Odo_x >> 8) & 0xFF;
+    msgTx.data[2]=((short)Odo_y) & 0xFF;
+    msgTx.data[3]=((short)Odo_y >> 8) & 0xFF;
+    msgTx.data[4]=((short)Odo_theta % 3600) & 0xFF;
+    msgTx.data[5]=(((short)Odo_theta % 3600) >> 8) & 0xFF;
+
+    can1.write(msgTx);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Debug_Asserv                                                          */
+/* DESCRIPTION  : Envoie  vall encod + consigne_pos                                     */
+/* Ps           : les encodeurs sont sur 32 bits, là on ne prend que les 16 premiérs    */
+/****************************************************************************************/
+void Debug_Asserv(void)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=DEBUG_ASSERV;  // si proposition accepter remplacer par "ACKNOWLEDGE_MOTEUR"
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=6;
+    msgTx.data[0]=(short)encoder1.GetCounter(); 
+    msgTx.data[1]=(short)encoder1.GetCounter()>>8;
+    msgTx.data[2]=(short)encoder2.GetCounter();
+    msgTx.data[3]=(short)encoder2.GetCounter()>>8;
+    msgTx.data[4]=(short)(consigne_pos);
+    msgTx.data[5]=(char)consigne_pos >> 8;
+
+    can1.write(msgTx);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: motor_of                                                              */
+/* DESCRIPTION  : prevenir que les moteur sont à l'arret                                */
+/****************************************************************************************/
+void motor_of(void)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=INSTRUCTION_END_MOTEUR;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=2;
+    msgTx.data[0]= 0;
+    msgTx.data[1]= 0;
+    
+    can1.write(msgTx);
+}