Diff: MX12.cpp

Revision:

9:b4a5187fdec6

Parent:

8:e74ef93ae660

Child:

10:ca9afe156ee1

--- a/MX12.cpp	Sat Nov 06 08:29:28 2021 +0000
+++ b/MX12.cpp	Sat Nov 06 16:15:34 2021 +0000
@@ -20,8 +20,7 @@
         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);
+    _mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
     
     // variable used for message reception
     _frame_pointer = 0;
@@ -34,33 +33,11 @@
     _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));
  
@@ -90,31 +67,11 @@
     _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.
@@ -215,14 +172,11 @@
 }
 
 // Debug function to print Serial read
-void MX12::PrintSerial() {
-    
-    printf("_frame_pointer = %d\n", _frame_pointer);
-    printf("frame = .");
+void MX12::PrintSerial() 
+{
     for(int i = 0; i < _frame_pointer; i++) {
         printf("%x.", _stored_frame[i]);
     }
- 
     printf("\n");
 }