Diff: MX12.cpp

Revision:

8:e74ef93ae660

Parent:

5:0cf54586a4be

Child:

9:b4a5187fdec6

--- a/MX12.cpp	Thu Nov 04 15:11:58 2021 +0000
+++ b/MX12.cpp	Sat Nov 06 08:29:28 2021 +0000
@@ -1,18 +1,65 @@
+/**  
+ * @file MX12.ccp  
+ * @brief this file will contain methods to manage au bus of servomotor 
+ *        Dynaminel MX12
+ * 
+ */ 
+
 #include "MX12.h"
 
-MX12::MX12(PinName tx, PinName rx, int baud) : _mx12(tx, rx) {
-    // Serial configuration
-    _mx12.baud(baud);
-    _mx12.format(8, SerialBase::None, 1);
-    _mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
+MX12::MX12(PinName tx, PinName rx, int baud)
+    : _mx12(tx, rx)    // initializes UnbufferedSerial object
+{
+    /*  Serial bus setup
+     */
+    // // Set desired properties (baud-8-N-1)
+    _mx12.baud(baud);     /* modulation speed */
+    _mx12.format(
+        8,                /* bits */
+        SerialBase::None, /* parity */
+        1                 /* stop bit */
+    ); 
+    // Register a callback to process a Rx (receive) interrupt.
+    //_mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
+    _mx12.attach(callback(this, &MX12::_Rx_BD_interrupt), SerialBase::RxIrq);
     
-    // Internal defaults
+    // variable used for message reception
+    _frame_pointer = 0;
+    _current_frame = { .motorId = 0, 
+                       .length = 0,
+                       .data="", 
+                       .valid=0};
+    
+    // Internal defaults states
     _sstate = SerialState::Idle;
     _pstate = ParsingState::Header;
+    
+    // for testing
+    _BDcount = 0;
+}
+
+
+// Interupt Routine to read in data from serial port
+void MX12::_Rx_BD_interrupt() 
+{
+    char c;
+    _BDcount++;
+    // Read the data to clear the receive interrupt.
+    if (_mx12.read(&c, 1)) {
+        // Echo the input back to the terminal.
+        _BDcount++;
+    }
+}
+
+int MX12::get_counter()
+{
+    return _BDcount;
 }
 
 void MX12::SetSpeed(unsigned char mot_id, float speed) {
     char data[2];
+    
+    _BDcount = _BDcount + 100;
  
     // Speed absolute value
     int goal = (0x3ff * abs(speed));
@@ -38,7 +85,36 @@
     
     /* Set variables for reception from servovotor */
     _answer  = 0;
-    memset(_current_frame.data, 0, MX12_DATA_MAX_SIZE);
+    //memset(_current_frame.data, 0, MX12_DATA_MAX_SIZE);
+    _scontext.checksum = 0;
+    _pstate = Header;
+    _scontext = {.headingCount = 0, .dataCount = 0, .checksum = 0};
+    
+    printf("Previous value\n");
+    printf("  _current_frame.motorId: %d\n", _current_frame.motorId);
+    printf("  _current_frame.length: %d\n", _current_frame.length);
+    printf("  _current_frame.valid: %d\n", _current_frame.length);
+    printf("  _frame_pointer: %d\n", _frame_pointer);
+    printf("  _scontext.headingCount: %d\n", _scontext.headingCount);
+    printf("  _scontext.dataCount: %d\n", _scontext.dataCount);
+    printf("  _scontext.checksum: %d\n", _scontext.checksum);
+    printf("  _BDcount: %d\n\n", _BDcount);
+
+    _current_frame = { .motorId = 0, 
+                       .length = 0,
+                       .data="", 
+                       .valid=0};
+    _frame_pointer = 0;
+
+    printf("Current value\n");
+    printf("  _current_frame.motorId: %d\n", _current_frame.motorId);
+    printf("  _current_frame.length: %d\n", _current_frame.length);
+    printf("  _current_frame.valid: %d\n", _current_frame.length);
+    printf("  _frame_pointer: %d\n", _frame_pointer);
+    printf("  _scontext.headingCount: %d\n", _scontext.headingCount);
+    printf("  _scontext.dataCount: %d\n", _scontext.dataCount);
+    printf("  _scontext.checksum: %d\n", _scontext.checksum);
+    printf("  _BDcount: %d\n\n", _BDcount);
     
     /* Initialise instruction packet to forge.
      * Instruction Packet is the command data sent to the servomotor.
@@ -141,8 +217,10 @@
 // Debug function to print Serial read
 void MX12::PrintSerial() {
     
+    printf("_frame_pointer = %d\n", _frame_pointer);
+    printf("frame = .");
     for(int i = 0; i < _frame_pointer; i++) {
-        printf("%x ", _stored_frame[i]);
+        printf("%x.", _stored_frame[i]);
     }
  
     printf("\n");
@@ -181,13 +259,18 @@
 }
  
 void MX12::_ReadCallback() {
+
     char c;
     
     // Try to read serial
     if(_mx12.read(&c, 1)) {
+        
         _stored_frame[_frame_pointer++] = c;
-        //_res[_res_count] = c;
-        //_res_count++;
+        
+        /* Include all characters in the checksum calculation, 
+         * even those that shouldn't, such as header characters. 
+         * They will be subtracted later (B. Denis remark)
+         */
         _scontext.checksum += c;
 
         // State-machine parsing
@@ -195,7 +278,7 @@
             
             case Header:
                 if(++(_scontext.headingCount) >= 2) {
-                    _scontext.headingCount = 0;
+                    //_scontext.headingCount = 0;
                     _pstate = Id;
                 }