code petit robot pour homologation

Fork of CRAC-Strat_2017_V2 by CRAC Team

Revision:
6:eddfa414fd11
Parent:
5:dcd817534b57
Child:
9:d0042422d95a
--- a/Asservissement/Asservissement.cpp	Sat Apr 23 09:16:14 2016 +0000
+++ b/Asservissement/Asservissement.cpp	Mon Apr 25 10:59:22 2016 +0000
@@ -87,6 +87,7 @@
 /*  isEnchainement : Indique si il faut executer l'instruction en enchainement               */
 /*                   0 => non                                                                */
 /*                   1 => oui                                                                */
+/*                   2 => dernière instruction de l'enchainement                             */
 /*********************************************************************************************/
 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
 {
@@ -107,4 +108,29 @@
     msgTx.data[5]=isEnchainement;
 
     can1.write(msgTx);
-}
\ No newline at end of file
+}
+
+/********************************************************************************************/
+/* FUNCTION NAME: BendRadius                                                                */
+/* DESCRIPTION  : Transmission CAN correspondant à un rayon de courbure                     */
+/********************************************************************************************/
+void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_COURBURE; // tx asservissement rayon de courbure
+    msgTx.len=6;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // Rayon sur 2 octets
+    msgTx.data[0]=(unsigned char)rayon;
+    msgTx.data[1]=(unsigned char)(rayon>>8);
+    // Angle signé sur 2 octets
+    msgTx.data[2]=(unsigned char)angle;
+    msgTx.data[3]=(unsigned char)(angle>>8);
+    // Sens signé sur 1 octet
+    msgTx.data[4]=sens;
+    // Enchainement sur 1 octet
+    msgTx.data[5]=enchainement;
+
+    can1.write(msgTx);
+}