Diff: Asservissement/Asservissement.cpp

Revision:

0:ad97421fb1fb

Child:

1:116040d14164

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement/Asservissement.cpp	Wed Apr 13 22:04:54 2016 +0000
@@ -0,0 +1,62 @@
+#include "Asservissement.h"
+
+/*********************************************************************************************************/
+/* FUNCTION NAME: SendRawId                                                                              */
+/* DESCRIPTION  : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
+/*********************************************************************************************************/
+
+void SendRawId (unsigned short id)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=id;
+    msgTx.len=0;
+    can1.write(msgTx);
+    wait_us(200);
+}
+
+/*********************************************************************************************/
+/* FUNCTION NAME: GoToPosition                                                               */
+/* DESCRIPTION  : Transmission CAN correspondant à un asservissement en position (x,y,theta) */
+/*********************************************************************************************/
+
+void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens)
+{
+    //id_to_expect=ACK_CONSIGNE;
+
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta)
+    msgTx.len=7;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // x sur 2 octets
+    msgTx.data[0]=(unsigned char)x;
+    msgTx.data[1]=(unsigned char)(x>>8);
+    // y sur 2 octets
+    msgTx.data[2]=(unsigned char)y;
+    msgTx.data[3]=(unsigned char)(y>>8);
+    // theta signé sur 2 octets
+    msgTx.data[4]=(unsigned char)theta;
+    msgTx.data[5]=(unsigned char)(theta>>8);
+    msgTx.data[6]=sens;
+
+    can1.write(msgTx);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Rotate                                                                */
+/* DESCRIPTION  : Transmission CAN correspondant à une rotation                         */
+/****************************************************************************************/
+
+void Rotate (signed short angle)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_ROTATION;  // Tx rotation autour du centre du robot
+    msgTx.len=1;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    //  Angle signé sur 2 octets
+    msgTx.data[0]=(unsigned char)angle;
+    msgTx.data[1]=(unsigned char)(angle>>8);
+
+    can1.write(msgTx);
+}
\ No newline at end of file