C++ HardwareTimer::getCount方法代码示例

unsigned int baud_time_get() 
{
  #ifdef ___MAPLE  
    timer4.pause();
    unsigned int tmp = timer4.getCount();  //get current timer count
    timer4.setCount(0);                    // reset timer count
    timer4.refresh();
    timer4.resume();
  #endif

  #ifdef ___ARDUINO
    TCCR1B &= 0xF8;                        // Clear CS10/CS11/CS12 bits, stopping the timer
    unsigned int tmp = int(TCNT1/2);       // get current timer count, dividing by two
                                           // because Arduino timer is counting at 2Mhz instead of 1Mhz
                                           // due to limited prescaler selection
    tmp += 0x8000 * baudOverflow;          // if timer overflowed, we will add 0xFFFF/2
    TCNT1 = 0;                             // reset timer count    
    baudOverflow = false;                  // reset baudOverflow after resetting timer    
    TCCR1B |= (1 << CS11);                 // Configure for 8 prescaler, restarting timer
  #endif

  #ifdef ___TEENSY
    unsigned int tmp = int(FTM0_CNT*2.8);
    FTM0_CNT = 0x0;
  #endif
  
  return tmp;
}

void motor_read_current() {
  if (HAS_CURRENT_SENSING) {
    mot.current = analogRead(CURRENT_ADC_PIN) - 2048;

    if (abs(mot.current) > 500) {
      // Values that big are not taken into account. This is a bad hack and can
      // be optimized.
    } else {
      mot.averageCurrent =
          ((AVERAGE_FACTOR_FOR_CURRENT - 1) * mot.averageCurrent * PRESCALE +
           mot.current * PRESCALE) /
          (AVERAGE_FACTOR_FOR_CURRENT * PRESCALE);
    }

    if (currentDetailedDebugOn == true) {
      currentRawMeasures[currentMeasureIndex] = mot.current;
      currentTimming[currentMeasureIndex] = timer3.getCount();
      currentMeasureIndex++;
      if (currentMeasureIndex > (C_NB_RAW_MEASURES - 1)) {
        currentDetailedDebugOn = false;
        currentMeasureIndex = 0;
      }
    }
  }
}

static void serial_received(struct serial *serial)
{
    dma_disable(DMA1, serial->channel);
    usart_tcie(serial->port->c_dev()->regs, 0);
    serial->dmaEvent = false;
    serial->txComplete = true;
    receiveMode(serial);
    serial->syncReadStart = syncReadTimer.getCount();
}

void motor_add_benchmark_time() {
  if (timeIndexMotor < TIME_STAMP_SIZE_MOTOR) {
    arrayOfTimeStampsMotor[timeIndexMotor] = timer3.getCount();
    timeIndexMotor++;
  }
}

void motor_update(encoder *pEnc) {
  uint16 time = timer3.getCount();

  int16 dt = 10; /*
                  * This function should be called every 1 ms.
                  * The plan B would be to estimate dt = time - previousTime;
                  * */
  //    int16 dt = time - previousTime; // Need to check for the case where the
  //    timer overflows or resets.
  //    previousTime = time;

  // Updating the motor position (ie angle)
  buffer_add(&(mot.angleBuffer), mot.angle);
  // This command is the one from the previous tick since the control is updated
  // after the motor.
  //    buffer_add(&(mot.commandBuffer), mot.command);
  mot.previousAngle = mot.angle;
  // Taking into account the magic offset
  int tempAngle = pEnc->angle + mot.offset;
  if (tempAngle < 0) {
    tempAngle = MAX_ANGLE + tempAngle + 1;
  }

  // Should not be useful, just being careful
  tempAngle = tempAngle % (MAX_ANGLE + 1);

  mot.angle = tempAngle;
  if (mot.multiTurnOn == false) {
    mot.multiTurnAngle = mot.angle;
  } else {
    if ((mot.angle - mot.previousAngle) > (MAX_ANGLE + 1) / 2) {
      // Went from 3 to 4092 (4092 - 3 > 2048), the multiturn angle should be
      // what it was minus 7
      mot.multiTurnAngle =
          mot.multiTurnAngle + mot.angle - mot.previousAngle - (MAX_ANGLE + 1);
    } else if ((mot.angle - mot.previousAngle) < (-MAX_ANGLE - 1) / 2) {
      // Went from 4095 to 1, the multiturn angle should be what it was plus 2
      mot.multiTurnAngle =
          mot.multiTurnAngle + mot.angle - mot.previousAngle + (MAX_ANGLE + 1);
    } else {
      mot.multiTurnAngle = mot.multiTurnAngle + mot.angle - mot.previousAngle;
    }
  }

  int16 oldPosition = buffer_get(&(mot.angleBuffer));

  motor_update_sign_of_speed();

  // Updating the motor speed
  int32 previousSpeed = mot.speed;

  //mot.speed = mot.angle - oldPosition;
  // New way of calculating the speed
  mot.speed = filter_speed_update(&mot.filt_speed, mot.angle);
  if (abs(mot.speed) > MAX_ANGLE / 2) {
    // Position went from near max to near 0 or vice-versa
    if (mot.angle >= oldPosition) {
      mot.speed = mot.speed - MAX_ANGLE - 1;
    } else if (mot.angle < oldPosition) {
      mot.speed = mot.speed + MAX_ANGLE + 1;
    }
  }

  // The speed will be in steps/s :
  mot.speed = (mot.speed * 1000) / dxl_regs.ram.speedCalculationDelay;

  // Averaging with previous value (dangerous approach):
  mot.averageSpeed = ((previousSpeed * 99) + (mot.speed)) / 100.0;
  buffer_add(&(mot.speedBuffer), mot.speed);

  // Updating the motor acceleration
  int32 oldSpeed = buffer_get(&(mot.speedBuffer));
  mot.acceleration = mot.speed - oldSpeed;

  if (predictiveCommandOn) {
    if (changeId == 0) {
      changeId = timeIndexMotor;
    }
    if (counterUpdate % (dxl_regs.ram.predictiveCommandPeriod) == 0) {
      if (time > dxl_regs.ram.duration1 && controlMode != COMPLIANT_KIND_OF) {
        motor_restart_traj_timer();
        time = 0;
        if (dxl_regs.ram.copyNextBuffer != 0) {
          // Copying the buffer into the actual trajs
          dxl_regs.ram.copyNextBuffer = 0;
          dxl_copy_buffer_trajs();
        } else {
          digitalWrite(BOARD_LED_PIN, LOW);
          // Default action : forcing the motor to stay where it stands (through
          // PID)
          controlMode = POSITION_CONTROL;
          dxl_regs.ram.mode = 0;
          hardwareStruct.mot->targetAngle = hardwareStruct.mot->angle;
          dxl_regs.ram.goalPosition = hardwareStruct.mot->targetAngle;
        }
      }
      // These 3 lines make it impossible for the bootloader to load the binary
      // file, mainly because of the cos import. -> Known bug and known solution
      // but quite time consuming.
      //             float angleRad = (mot.angle * (float)PI) / 2048.0;
//.........这里部分代码省略.........

/**
 * Ticking
 */
static void dxl_serial_tick(volatile struct dxl_device *self) 
{
    struct serial *serial = (struct serial*)self->data;
    static int baudrate = DXL_DEFAULT_BAUDRATE;

    // Timeout on sending packet, this should never happen
    if (!serial->txComplete && ((millis() - serial->packetSent) > 3)) {
        serial_received(serial);
    }

    /*
    if (!serial->txComplete) {
        if (serial->dmaEvent) {
            // DMA completed
            serial->dmaEvent = false;
            serial->txComplete = true;
            //serial->port->waitDataToBeSent();
            receiveMode(serial);
            serial->syncReadStart = syncReadTimer.getCount();
        }
    } 
    */
 
    if (serial->txComplete) {
        if (syncReadMode) {
            bool processed = false;

            if (syncReadDevices <= 0 && syncReadResponse == &self->packet) {
                // The process is over, no devices are currently trying to get packet and we
                // are the device that will respond
                syncReadResponse->process = true;
                syncReadMode = false;
            } else if (serial->syncReadCount) {
                if (serial->syncReadCurrent == 0xff) {
                    // Sending the first packet
                    serial->syncReadCurrent = 0;
                    syncReadSendPacket(serial);
                } else {
                    // Reading available data from the port
                    while (serial->port->available() && !serial->syncReadPacket.process) {
                        dxl_packet_push_byte(&serial->syncReadPacket, serial->port->read());
                    }
                    if (serial->syncReadPacket.process && serial->syncReadPacket.parameter_nb == syncReadLength) {
                        // The packet is OK, copying data to the response at correct offset
                        ui8 i;
                        ui8 off = serial->syncReadOffsets[serial->syncReadCurrent]*(syncReadLength+1);
                        syncReadResponse->parameters[off] = serial->syncReadPacket.error;
                        for (i=0; i<syncReadLength; i++) {
                            syncReadResponse->parameters[off+1+i] = serial->syncReadPacket.parameters[i];
                        }
                        processed = true;
                    } else if (syncReadTimer.getCount()-serial->syncReadStart > 65) {
                        // The timeout is reached, answer with code 0xff
                        ui8 off = serial->syncReadOffsets[serial->syncReadCurrent]*(syncReadLength+1);
                        syncReadResponse->parameters[off] = 0xff;
                        processed = true;
                    }
                    if (processed) {
                        serial->syncReadCurrent++;
                        if (serial->syncReadCurrent >= serial->syncReadCount) {
                            // The process is over for this bus
                            syncReadDevices--;
                            serial->syncReadCount = 0;
                        } else {
                            // Sending the next packet
                            syncReadSendPacket(serial);
                        }
                    }
                }
            }
        } else {
            if (self->redirect_packets == NULL && baudrate != usb_cdcacm_get_baud()) {
                // baudrate = usb_cdcacm_get_baud();
                // initSerial(serial, baudrate);
            }

            // Reading data that come from the serial bus
            while (serial->port->available() && !self->packet.process) {
                dxl_packet_push_byte(&self->packet, serial->port->read());
                if (self->packet.process) {
                    // A packet is coming from our bus, noting it in the devices allocation
                    // table
                    devicePorts[self->packet.id] = serial->index;
                }
            }
        }
    }
}